| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderContactAlgebra.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 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary>Shared exact radial support-candidate encoding for finite cylinders.</summary> |
| | | 13 | | internal static class CylinderContactAlgebra |
| | | 14 | | { |
| | | 15 | | internal const int Words = ConvexContactCandidate.Words; |
| | | 16 | | |
| | | 17 | | /// <summary>Encodes a signed quadratic numerator over a positive square-root denominator.</summary> |
| | | 18 | | internal static int BuildQuadraticCandidate(ContactQuadratic numerator, ReadOnlySpan<ulong> root, |
| | | 19 | | ReadOnlySpan<ulong> denominator, Span<ulong> values, Span<int> signs) |
| | | 20 | | { |
| | | 21 | | // (A+B*sqrt(K))²=(A²+B²K)+2AB*sqrt(K). Squaring needs one |
| | | 22 | | // cross-product; its rational coefficient is always nonnegative. |
| | 1824 | 23 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 3648 | 24 | | Span<ulong> rational = Slot(values, 7), radical = Slot(values, 8); |
| | 1824 | 25 | | WideArithmetic.MultiplyMagnitudes(numerator.Rational, numerator.Rational, rational); |
| | 1824 | 26 | | WideArithmetic.MultiplyMagnitudes(numerator.Radical, numerator.Radical, product); |
| | 1824 | 27 | | WideArithmetic.MultiplyMagnitudes(product, root, radical); |
| | 1824 | 28 | | WideArithmetic.AddMagnitudeInto(radical, rational); |
| | 1824 | 29 | | WideArithmetic.MultiplyMagnitudes(numerator.Rational, numerator.Radical, radical); |
| | 1824 | 30 | | WideArithmetic.AddMagnitudeInto(radical, radical); |
| | 3648 | 31 | | root.CopyTo(Slot(values, 9)); denominator.CopyTo(Slot(values, 10)); |
| | 1824 | 32 | | signs[7] = IsZero(rational) ? 0 : 1; |
| | 1824 | 33 | | signs[8] = IsZero(radical) ? 0 : numerator.Signs[0] * numerator.Signs[1]; |
| | 1824 | 34 | | return numerator.Sign(root); |
| | | 35 | | } |
| | | 36 | | |
| | | 37 | | /// <summary>Encodes (p+sqrt(q))/sqrt(d), including its signed gap, at the input field stride.</summary> |
| | | 38 | | // q/d may borrow output slots9/10; consume them before overwriting either slot. |
| | | 39 | | internal static int BuildRadialCandidate(ReadOnlySpan<ulong> p, int pSign, |
| | | 40 | | ReadOnlySpan<ulong> q, ReadOnlySpan<ulong> d, Span<ulong> values, Span<int> signs) |
| | | 41 | | { |
| | 67589 | 42 | | int words = p.Length; |
| | 67589 | 43 | | Span<ulong> squared = stackalloc ulong[words]; |
| | 67589 | 44 | | WideArithmetic.MultiplyMagnitudes(p, p, squared); |
| | 67589 | 45 | | WideArithmetic.AddEqualMagnitudes(squared, q, Slot(values, 7, words)); |
| | 67589 | 46 | | WideArithmetic.AddEqualMagnitudes(p, p, Slot(values, 8, words)); |
| | 67589 | 47 | | q.CopyTo(Slot(values, 9, words)); |
| | 67589 | 48 | | d.CopyTo(Slot(values, 10, words)); |
| | 67589 | 49 | | signs[7] = IsZero(Slot(values, 7, words)) ? 0 : 1; |
| | 67589 | 50 | | signs[8] = pSign; |
| | 67589 | 51 | | return pSign < 0 ? WideArithmetic.CompareMagnitudeEqualLength(q, squared) |
| | 67589 | 52 | | : pSign > 0 || !IsZero(q) ? 1 : 0; |
| | | 53 | | } |
| | | 54 | | |
| | | 55 | | internal static void WriteWorldDirection(WideRationalBasis3d basis, |
| | | 56 | | ReadOnlySpan<ulong> direction, ReadOnlySpan<int> directionSigns, |
| | | 57 | | Span<ulong> values, Span<int> signs, int orientation = 1) |
| | | 58 | | { |
| | 51167 | 59 | | int Words = direction.Length / 3; |
| | 409336 | 60 | | for (int component = 0; component < 3; component++) |
| | | 61 | | { |
| | 153501 | 62 | | WideAxis3 row = component == 0 ? Axis(basis.Xx, basis.Yx, basis.Zx) |
| | 153501 | 63 | | : component == 1 ? Axis(basis.Xy, basis.Yy, basis.Zy) : Axis(basis.Xz, basis.Yz, basis.Zz); |
| | 153501 | 64 | | Dot(row, direction, directionSigns, Slot(values, component, Words), out int sign); |
| | 153501 | 65 | | signs[component] = orientation * sign; |
| | | 66 | | } |
| | 51167 | 67 | | } |
| | | 68 | | |
| | | 69 | | internal static void Dot(WideAxis3 axis, ReadOnlySpan<ulong> direction, ReadOnlySpan<int> signs, |
| | | 70 | | Span<ulong> result, out int sign) |
| | | 71 | | { |
| | 290205 | 72 | | int Words = result.Length; |
| | 290205 | 73 | | Span<ulong> component = stackalloc ulong[Words]; |
| | 290205 | 74 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 580410 | 75 | | result.Clear(); sign = 0; |
| | 2321640 | 76 | | for (int index = 0; index < 3; index++) |
| | | 77 | | { |
| | 870615 | 78 | | Signed320 value = Component(axis, index); |
| | 870615 | 79 | | Import(value, component); |
| | 870615 | 80 | | WideArithmetic.MultiplyMagnitudes(component, Slot(direction, index, Words), product); |
| | 870615 | 81 | | Add(product, value.Sign * signs[index], result, ref sign); |
| | | 82 | | } |
| | 290205 | 83 | | } |
| | | 84 | | |
| | | 85 | | internal static void Dot(in WideDirection3d axis, ReadOnlySpan<ulong> direction, ReadOnlySpan<int> signs, |
| | | 86 | | Span<ulong> result, out int sign) |
| | | 87 | | { |
| | 651337 | 88 | | int Words = result.Length; |
| | 1302674 | 89 | | Span<ulong> component = stackalloc ulong[Words], product = stackalloc ulong[Words]; |
| | 1302674 | 90 | | result.Clear(); sign = 0; |
| | 5210696 | 91 | | for (int index = 0; index < 3; index++) |
| | | 92 | | { |
| | 1954011 | 93 | | Signed576 value = Component(axis, index); |
| | 1954011 | 94 | | Import(value, component); |
| | 1954011 | 95 | | WideArithmetic.MultiplyMagnitudes(component, Slot(direction, index, Words), product); |
| | 1954011 | 96 | | Add(product, value.Sign * signs[index], result, ref sign); |
| | | 97 | | } |
| | 651337 | 98 | | } |
| | | 99 | | |
| | | 100 | | internal static void ProjectPerpendicular(WideAxis3 axis, WideAxis3 value, |
| | | 101 | | Span<ulong> direction, Span<int> signs) |
| | | 102 | | { |
| | 14287 | 103 | | Span<ulong> work = stackalloc ulong[4 * Words]; |
| | 57148 | 104 | | Span<ulong> u = Slot(work, 0), dot = Slot(work, 1), component = Slot(work, 2), product = Slot(work, 3); |
| | 14287 | 105 | | Import(axis.SquaredLength, u); |
| | 14287 | 106 | | Signed576 projection = WideAxis3.Dot(axis, value); |
| | 14287 | 107 | | Import(projection, dot); |
| | 114296 | 108 | | for (int index = 0; index < 3; index++) |
| | | 109 | | { |
| | 42861 | 110 | | Signed320 v = Component(value, index); |
| | 42861 | 111 | | Signed320 a = Component(axis, index); |
| | 42861 | 112 | | Import(v, component); |
| | 42861 | 113 | | WideArithmetic.MultiplyMagnitudes(u, component, Slot(direction, index)); |
| | 42861 | 114 | | int sign = v.Sign; |
| | 42861 | 115 | | Import(a, component); |
| | 42861 | 116 | | WideArithmetic.MultiplyMagnitudes(dot, component, product); |
| | 42861 | 117 | | Add(product, -projection.Sign * a.Sign, Slot(direction, index), ref sign); |
| | 42861 | 118 | | signs[index] = sign; |
| | | 119 | | } |
| | 14287 | 120 | | } |
| | | 121 | | |
| | | 122 | | internal static void SumSquares(ReadOnlySpan<ulong> direction, Span<ulong> result) |
| | | 123 | | { |
| | 104405 | 124 | | int Words = result.Length; |
| | 104405 | 125 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 104405 | 126 | | result.Clear(); |
| | 835240 | 127 | | for (int index = 0; index < 3; index++) |
| | | 128 | | { |
| | 313215 | 129 | | ReadOnlySpan<ulong> component = Slot(direction, index, Words); |
| | 313215 | 130 | | WideArithmetic.MultiplyMagnitudes(component, component, product); |
| | 313215 | 131 | | WideArithmetic.AddMagnitudeInto(product, result); |
| | | 132 | | } |
| | 104405 | 133 | | } |
| | | 134 | | |
| | | 135 | | internal static void ProjectPerpendicular(in WideDirection3d axis, in WideDirection3d value, |
| | | 136 | | Span<ulong> direction, Span<int> signs) |
| | | 137 | | { |
| | 13358 | 138 | | Span<ulong> work = stackalloc ulong[4 * Words]; |
| | 53432 | 139 | | Span<ulong> norm = Slot(work, 0), dot = Slot(work, 1), component = Slot(work, 2), product = Slot(work, 3); |
| | 13358 | 140 | | Import(axis.SquaredLength, norm); |
| | 13358 | 141 | | Signed832 projection = WideDirection3d.Dot(axis, value); |
| | 13358 | 142 | | Import(projection, dot); |
| | 106864 | 143 | | for (int index = 0; index < 3; index++) |
| | | 144 | | { |
| | 80148 | 145 | | Signed576 v = Component(value, index), a = Component(axis, index); |
| | 40074 | 146 | | Import(v, component); |
| | 40074 | 147 | | WideArithmetic.MultiplyMagnitudes(norm, component, Slot(direction, index)); |
| | 40074 | 148 | | int sign = v.Sign; |
| | 40074 | 149 | | Import(a, component); |
| | 40074 | 150 | | WideArithmetic.MultiplyMagnitudes(dot, component, product); |
| | 40074 | 151 | | Add(product, -projection.Sign * a.Sign, Slot(direction, index), ref sign); |
| | 40074 | 152 | | signs[index] = sign; |
| | | 153 | | } |
| | 13358 | 154 | | } |
| | | 155 | | |
| | | 156 | | internal static void WriteDirection(WideAxis3 axis, Span<ulong> direction, Span<int> signs) |
| | | 157 | | { |
| | 9915 | 158 | | int Words = direction.Length / 3; |
| | 79320 | 159 | | for (int index = 0; index < 3; index++) |
| | | 160 | | { |
| | 29745 | 161 | | Signed320 value = Component(axis, index); |
| | 59490 | 162 | | Import(value, Slot(direction, index, Words)); signs[index] = value.Sign; |
| | | 163 | | } |
| | 9915 | 164 | | } |
| | | 165 | | internal static void WriteDirection(in WideDirection3d axis, Span<ulong> direction, Span<int> signs) |
| | | 166 | | { |
| | 52910 | 167 | | int Words = direction.Length / 3; |
| | 423280 | 168 | | for (int index = 0; index < 3; index++) |
| | | 169 | | { |
| | 158730 | 170 | | Signed576 value = Component(axis, index); |
| | 317460 | 171 | | Import(value, Slot(direction, index, Words)); signs[index] = value.Sign; |
| | | 172 | | } |
| | 52910 | 173 | | } |
| | | 174 | | |
| | | 175 | | internal static void Add(ReadOnlySpan<ulong> value, int sign, Span<ulong> destination, ref int destinationSign) => |
| | 5949188 | 176 | | WideArithmetic.AddShiftedSignedMagnitude(value, sign, 0, destination, ref destinationSign); |
| | 1776685 | 177 | | internal static bool IsZero(ReadOnlySpan<ulong> value) => WideArithmetic.GetActiveMagnitudeLength(value) == 0; |
| | 1629945 | 178 | | internal static Span<ulong> Slot(Span<ulong> values, int index) => values.Slice(index * Words, Words); |
| | 457155 | 179 | | internal static ReadOnlySpan<ulong> Slot(ReadOnlySpan<ulong> values, int index) => values.Slice(index * Words, Words |
| | 984643 | 180 | | internal static Span<ulong> Slot(Span<ulong> values, int index, int words) => values.Slice(index * words, words); |
| | 3139055 | 181 | | internal static ReadOnlySpan<ulong> Slot(ReadOnlySpan<ulong> values, int index, int words) => values.Slice(index * w |
| | 1306361 | 182 | | internal static void Import(Signed320 value, Span<ulong> result) => Import(Signed576.ExtendValue(value), result); |
| | 4902772 | 183 | | internal static void Import(Signed576 value, Span<ulong> result) => WideArithmetic.GetMagnitude(value, result); |
| | 128844 | 184 | | internal static void Import(Signed832 value, Span<ulong> result) => WideArithmetic.GetMagnitude(value, result); |
| | | 185 | | internal static void DifferenceOfProducts(Signed832 a, Signed832 b, Signed832 c, Signed832 d, |
| | | 186 | | Span<ulong> result, out int sign) |
| | | 187 | | { |
| | 2691 | 188 | | Span<ulong> left = stackalloc ulong[13], right = stackalloc ulong[13], product = stackalloc ulong[result.Length] |
| | 1794 | 189 | | WideArithmetic.GetMagnitude(a, left); WideArithmetic.GetMagnitude(b, right); |
| | 897 | 190 | | WideArithmetic.MultiplyMagnitudes(left, right, result); |
| | 897 | 191 | | sign = a.Sign * b.Sign; |
| | 1794 | 192 | | WideArithmetic.GetMagnitude(c, left); WideArithmetic.GetMagnitude(d, right); |
| | 897 | 193 | | WideArithmetic.MultiplyMagnitudes(left, right, product); |
| | 897 | 194 | | Add(product, -c.Sign * d.Sign, result, ref sign); |
| | 897 | 195 | | } |
| | | 196 | | internal static Signed320 Component(in WideAxis3 value, int index) => |
| | 989250 | 197 | | index == 0 ? value.X : index == 1 ? value.Y : value.Z; |
| | | 198 | | internal static Signed576 Component(in WideDirection3d value, int index) => |
| | 2477085 | 199 | | index == 0 ? value.X : index == 1 ? value.Y : value.Z; |
| | 153501 | 200 | | private static WideAxis3 Axis(Signed192 x, Signed192 y, Signed192 z) => new( |
| | 153501 | 201 | | Signed320.ExtendValue(x), Signed320.ExtendValue(y), Signed320.ExtendValue(z)); |
| | | 202 | | } |