| | | 1 | | //======================================================================= |
| | | 2 | | // GjkSimplexScale.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.Queries; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Uniformly scales GJK simplex coordinates before evaluating products whose |
| | | 16 | | /// ratios and signs are invariant under a common scale. |
| | | 17 | | /// </summary> |
| | | 18 | | internal static class GjkSimplexScale |
| | | 19 | | { |
| | 1 | 20 | | private static readonly Fixed64 ProductSafeComponentLimit = (Fixed64)8; |
| | | 21 | | |
| | | 22 | | /// <summary> |
| | | 23 | | /// Creates a two-term Minkowski difference in the shared GJK working |
| | | 24 | | /// coordinate. An exact arithmetic raw shift avoids round-to-even endpoint |
| | | 25 | | /// overshoot, so halving before subtraction covers every Fixed64 pair. |
| | | 26 | | /// </summary> |
| | | 27 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 28 | | public static Vector3d CreateWorkingDifference(Vector3d first, Vector3d second) => |
| | 1 | 29 | | CreateWorkingDifference(first, second, 1); |
| | | 30 | | |
| | | 31 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 32 | | public static Vector3d CreateWorkingDifference(Vector3d first, Vector3d second, int shift) |
| | | 33 | | { |
| | 4 | 34 | | ValidateShift(shift); |
| | 2 | 35 | | if (Vector3d.TrySubtract( |
| | 2 | 36 | | ScaleByPowerOfTwo(first, shift), |
| | 2 | 37 | | ScaleByPowerOfTwo(second, shift), |
| | 2 | 38 | | out Vector3d difference)) |
| | | 39 | | { |
| | 1 | 40 | | return difference; |
| | | 41 | | } |
| | | 42 | | |
| | 1 | 43 | | throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference."); |
| | | 44 | | } |
| | | 45 | | |
| | | 46 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 47 | | public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second) => |
| | 1 | 48 | | CreateWorkingDifference(first, second, 2); |
| | | 49 | | |
| | | 50 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 51 | | public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second, int shift) |
| | | 52 | | { |
| | 331 | 53 | | ValidateShift(shift); |
| | 329 | 54 | | if (Vector2d.TrySubtract( |
| | 329 | 55 | | ScaleByPowerOfTwo(first, shift), |
| | 329 | 56 | | ScaleByPowerOfTwo(second, shift), |
| | 329 | 57 | | out Vector2d difference)) |
| | | 58 | | { |
| | 328 | 59 | | return difference; |
| | | 60 | | } |
| | | 61 | | |
| | 1 | 62 | | throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference."); |
| | | 63 | | } |
| | | 64 | | |
| | | 65 | | /// <summary> |
| | | 66 | | /// Creates a three-term Minkowski difference in the shared GJK working |
| | | 67 | | /// coordinate without first forming a potentially saturated sum. |
| | | 68 | | /// </summary> |
| | | 69 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 70 | | public static Vector2d CreateWorkingDifference(Vector2d first, Vector2d second, Vector2d third) => |
| | 1 | 71 | | CreateWorkingDifference(first, second, third, 2); |
| | | 72 | | |
| | | 73 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 74 | | public static Vector2d CreateWorkingDifference( |
| | | 75 | | Vector2d first, |
| | | 76 | | Vector2d second, |
| | | 77 | | Vector2d third, |
| | | 78 | | int shift) |
| | | 79 | | { |
| | 1833 | 80 | | ValidateShift(shift); |
| | 1829 | 81 | | if (Vector2d.TrySubtract( |
| | 1829 | 82 | | ScaleByPowerOfTwo(first, shift), |
| | 1829 | 83 | | ScaleByPowerOfTwo(second, shift), |
| | 1829 | 84 | | out Vector2d difference) |
| | 1829 | 85 | | && Vector2d.TrySubtract( |
| | 1829 | 86 | | difference, |
| | 1829 | 87 | | ScaleByPowerOfTwo(third, shift), |
| | 1829 | 88 | | out Vector2d result)) |
| | | 89 | | { |
| | 1826 | 90 | | return result; |
| | | 91 | | } |
| | | 92 | | |
| | 3 | 93 | | throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference."); |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | /// <summary> |
| | | 97 | | /// Creates a four-term Minkowski difference for owner-relative support |
| | | 98 | | /// without materializing the support in absolute coordinates. |
| | | 99 | | /// </summary> |
| | | 100 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 101 | | public static Vector2d CreateWorkingDifference( |
| | | 102 | | Vector2d first, |
| | | 103 | | Vector2d second, |
| | | 104 | | Vector2d third, |
| | | 105 | | Vector2d fourth, |
| | | 106 | | int shift) |
| | | 107 | | { |
| | 419 | 108 | | Vector2d difference = CreateWorkingDifference(first, second, third, shift); |
| | 416 | 109 | | if (Vector2d.TrySubtract(difference, ScaleByPowerOfTwo(fourth, shift), out Vector2d result)) |
| | 415 | 110 | | return result; |
| | | 111 | | |
| | 1 | 112 | | throw new InvalidOperationException("The selected GJK working shift does not preserve an exact difference."); |
| | | 113 | | } |
| | | 114 | | |
| | | 115 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 116 | | public static Fixed64 RestoreTwoTermDistance(Fixed64 workingDistance) => |
| | 1 | 117 | | RestoreDistance(workingDistance, 1); |
| | | 118 | | |
| | | 119 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 120 | | public static Fixed64 RestoreThreeTermDistance(Fixed64 workingDistance) => |
| | 1 | 121 | | RestoreDistance(workingDistance, 2); |
| | | 122 | | |
| | | 123 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 124 | | public static Fixed64 RestoreDistance(Fixed64 workingDistance, int shift) |
| | | 125 | | { |
| | 1296 | 126 | | ValidateShift(shift); |
| | 1294 | 127 | | return shift == 0 ? workingDistance : workingDistance * (1 << shift); |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 131 | | public static Fixed64 GetCoordinateScale(int shift) |
| | | 132 | | { |
| | 5242 | 133 | | ValidateShift(shift); |
| | 5240 | 134 | | return shift switch |
| | 5240 | 135 | | { |
| | 549 | 136 | | 0 => Fixed64.One, |
| | 1 | 137 | | 1 => Fixed64.Half, |
| | 4690 | 138 | | _ => Fixed64.Quarter |
| | 5240 | 139 | | }; |
| | | 140 | | } |
| | | 141 | | |
| | | 142 | | public static int SelectTwoTermShift( |
| | | 143 | | Vector3d firstMin, |
| | | 144 | | Vector3d firstMax, |
| | | 145 | | Vector3d secondMin, |
| | | 146 | | Vector3d secondMax) => |
| | 2 | 147 | | CanSubtractBounds(firstMin, firstMax, secondMin, secondMax) ? 0 : 1; |
| | | 148 | | |
| | | 149 | | public static int SelectThreeTermShift( |
| | | 150 | | Vector2d point, |
| | | 151 | | Vector2d targetMin, |
| | | 152 | | Vector2d targetMax, |
| | | 153 | | Fixed64 expansionRadius) |
| | | 154 | | { |
| | 388 | 155 | | if (CanSubtractExpandedBounds(point, targetMin, targetMax, expansionRadius, 0)) |
| | 383 | 156 | | return 0; |
| | | 157 | | |
| | 5 | 158 | | return CanSubtractExpandedBounds(point, targetMin, targetMax, expansionRadius, 1) ? 1 : 2; |
| | | 159 | | } |
| | | 160 | | |
| | | 161 | | public static int SelectFourTermShift( |
| | | 162 | | Vector2d point, |
| | | 163 | | Vector2d origin, |
| | | 164 | | Vector2d targetMinOffset, |
| | | 165 | | Vector2d targetMaxOffset, |
| | | 166 | | Fixed64 expansionRadius) |
| | | 167 | | { |
| | 174 | 168 | | if (CanSubtractExpandedBounds(point, targetMinOffset, targetMaxOffset, expansionRadius, 0, origin)) |
| | 168 | 169 | | return 0; |
| | | 170 | | |
| | 6 | 171 | | return CanSubtractExpandedBounds(point, targetMinOffset, targetMaxOffset, expansionRadius, 1, origin) ? 1 : 2; |
| | | 172 | | } |
| | | 173 | | |
| | | 174 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 175 | | private static Vector3d ScaleByPowerOfTwo(Vector3d value, int shift) => |
| | 4 | 176 | | new( |
| | 4 | 177 | | value.X >> shift, |
| | 4 | 178 | | value.Y >> shift, |
| | 4 | 179 | | value.Z >> shift); |
| | | 180 | | |
| | | 181 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 182 | | private static Vector2d ScaleByPowerOfTwo(Vector2d value, int shift) => |
| | 8446 | 183 | | new( |
| | 8446 | 184 | | value.X >> shift, |
| | 8446 | 185 | | value.Y >> shift); |
| | | 186 | | |
| | | 187 | | private static bool CanSubtractBounds( |
| | | 188 | | Vector3d firstMin, |
| | | 189 | | Vector3d firstMax, |
| | | 190 | | Vector3d secondMin, |
| | | 191 | | Vector3d secondMax) => |
| | 2 | 192 | | Vector3d.TrySubtract(firstMax, secondMin, out _) |
| | 2 | 193 | | && Vector3d.TrySubtract(firstMin, secondMax, out _); |
| | | 194 | | |
| | | 195 | | private static bool CanSubtractExpandedBounds( |
| | | 196 | | Vector2d point, |
| | | 197 | | Vector2d targetMin, |
| | | 198 | | Vector2d targetMax, |
| | | 199 | | Fixed64 expansionRadius, |
| | | 200 | | int shift, |
| | | 201 | | Vector2d? origin = null) |
| | | 202 | | { |
| | 573 | 203 | | Vector2d scaledPoint = ScaleByPowerOfTwo(point, shift); |
| | 573 | 204 | | if (origin is Vector2d targetOrigin) |
| | | 205 | | { |
| | 180 | 206 | | Vector2d scaledOrigin = ScaleByPowerOfTwo(targetOrigin, shift); |
| | | 207 | | // The initial direction reverses this difference; both signs must fit. |
| | 180 | 208 | | if (!Vector2d.TrySubtract(scaledOrigin, scaledPoint, out _) |
| | 180 | 209 | | || !Vector2d.TrySubtract(scaledPoint, scaledOrigin, out scaledPoint)) |
| | 6 | 210 | | return false; |
| | | 211 | | } |
| | | 212 | | |
| | 567 | 213 | | Vector2d scaledMin = ScaleByPowerOfTwo(targetMin, shift); |
| | 567 | 214 | | Vector2d scaledMax = ScaleByPowerOfTwo(targetMax, shift); |
| | 567 | 215 | | Fixed64 scaledRadius = ScaleRadiusCeiling(expansionRadius, shift); |
| | 567 | 216 | | Vector2d radius = new(scaledRadius, scaledRadius); |
| | | 217 | | |
| | 567 | 218 | | return Vector2d.TrySubtract(scaledPoint, scaledMin, out Vector2d positiveDifference) |
| | 567 | 219 | | && Vector2d.TryAdd(positiveDifference, radius, out _) |
| | 567 | 220 | | && Vector2d.TrySubtract(scaledPoint, scaledMax, out Vector2d negativeDifference) |
| | 567 | 221 | | && Vector2d.TrySubtract(negativeDifference, radius, out _); |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 225 | | private static Fixed64 ScaleRadiusCeiling(Fixed64 radius, int shift) |
| | | 226 | | { |
| | 567 | 227 | | Fixed64 scaled = radius >> shift; |
| | 567 | 228 | | if (shift == 0 || (scaled << shift) == radius) |
| | 561 | 229 | | return scaled; |
| | | 230 | | |
| | 6 | 231 | | return scaled + Fixed64.MinIncrement; |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 235 | | private static void ValidateShift(int shift) |
| | | 236 | | { |
| | 8706 | 237 | | if ((uint)shift > 2U) |
| | 12 | 238 | | throw new ArgumentOutOfRangeException(nameof(shift), "GJK coordinate shifts must be between zero and two."); |
| | 8694 | 239 | | } |
| | | 240 | | |
| | | 241 | | public static Fixed64 ScaleForProducts(Span<Vector3d> points) |
| | | 242 | | { |
| | 24070 | 243 | | Fixed64 largestComponent = Fixed64.Zero; |
| | 183420 | 244 | | for (int i = 0; i < points.Length; i++) |
| | | 245 | | { |
| | 67640 | 246 | | Vector3d point = points[i]; |
| | 67640 | 247 | | largestComponent = FixedMath.Max( |
| | 67640 | 248 | | largestComponent, |
| | 67640 | 249 | | FixedMath.Max(point.X.Abs(), FixedMath.Max(point.Y.Abs(), point.Z.Abs()))); |
| | | 250 | | } |
| | | 251 | | |
| | 24070 | 252 | | Fixed64 scale = Fixed64.One; |
| | 24904 | 253 | | while (largestComponent > ProductSafeComponentLimit) |
| | | 254 | | { |
| | 834 | 255 | | largestComponent *= Fixed64.Half; |
| | 834 | 256 | | scale *= Fixed64.Half; |
| | | 257 | | } |
| | | 258 | | |
| | 24070 | 259 | | if (scale == Fixed64.One) |
| | 23701 | 260 | | return scale; |
| | | 261 | | |
| | | 262 | | // With components <= 8, 3D simplex differences are <= 16 and the |
| | | 263 | | // largest tetrahedron face-side product remains inside Fixed64. |
| | 2900 | 264 | | for (int i = 0; i < points.Length; i++) |
| | 1081 | 265 | | points[i] *= scale; |
| | | 266 | | |
| | 369 | 267 | | return scale; |
| | | 268 | | } |
| | | 269 | | |
| | | 270 | | public static Fixed64 ScaleForProducts(Span<Vector2d> points) |
| | | 271 | | { |
| | 2 | 272 | | Fixed64 largestComponent = Fixed64.Zero; |
| | 12 | 273 | | for (int i = 0; i < points.Length; i++) |
| | | 274 | | { |
| | 4 | 275 | | Vector2d point = points[i]; |
| | 4 | 276 | | largestComponent = FixedMath.Max( |
| | 4 | 277 | | largestComponent, |
| | 4 | 278 | | FixedMath.Max(point.X.Abs(), point.Y.Abs())); |
| | | 279 | | } |
| | | 280 | | |
| | 2 | 281 | | Fixed64 scale = Fixed64.One; |
| | 30 | 282 | | while (largestComponent > ProductSafeComponentLimit) |
| | | 283 | | { |
| | 28 | 284 | | largestComponent *= Fixed64.Half; |
| | 28 | 285 | | scale *= Fixed64.Half; |
| | | 286 | | } |
| | | 287 | | |
| | 2 | 288 | | if (scale == Fixed64.One) |
| | 1 | 289 | | return scale; |
| | | 290 | | |
| | 6 | 291 | | for (int i = 0; i < points.Length; i++) |
| | 2 | 292 | | points[i] *= scale; |
| | | 293 | | |
| | 1 | 294 | | return scale; |
| | | 295 | | } |
| | | 296 | | } |