| | | 1 | | //======================================================================= |
| | | 2 | | // WideSlabProjection.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> |
| | | 13 | | /// Owns full-domain candidate construction for finite-slab projections. |
| | | 14 | | /// </summary> |
| | | 15 | | internal static class WideSlabProjection |
| | | 16 | | { |
| | 1 | 17 | | private static readonly Signed192 Scale = Signed192.Signed(Fixed64.One.m_rawValue); |
| | 1 | 18 | | private static readonly Signed192 CenteredAxisScale = WideArithmetic.Double(Scale); |
| | 1 | 19 | | private static readonly Signed192 ScaleSquared = new(0UL, 1UL, 0UL); |
| | 1 | 20 | | private static readonly Signed192 CenteredAxisScaleTimesScale = WideArithmetic.Double(ScaleSquared); |
| | 1 | 21 | | private static readonly Signed192 CenteredAxisScaleSquared = WideArithmetic.Double(CenteredAxisScaleTimesScale); |
| | | 22 | | |
| | | 23 | | #region Nested Types |
| | | 24 | | |
| | | 25 | | private readonly struct WidePlanarCandidate |
| | | 26 | | { |
| | | 27 | | internal readonly Signed576 X; |
| | | 28 | | internal readonly Signed576 Z; |
| | | 29 | | internal readonly Signed576 Denominator; |
| | | 30 | | |
| | | 31 | | internal WidePlanarCandidate(Signed576 x, Signed576 z, Signed576 denominator) |
| | | 32 | | { |
| | 235 | 33 | | X = x; |
| | 235 | 34 | | Z = z; |
| | 235 | 35 | | Denominator = denominator; |
| | 235 | 36 | | } |
| | | 37 | | } |
| | | 38 | | |
| | | 39 | | #endregion |
| | | 40 | | |
| | | 41 | | private static bool TryCreateResult(bool found, WidePlanarCandidate candidate, out Vector2d support) |
| | | 42 | | { |
| | 193 | 43 | | if (!found |
| | 193 | 44 | | || !Fixed64.TryGetSignedRawRatio(candidate.X, candidate.Denominator, out Fixed64 x) |
| | 193 | 45 | | || !Fixed64.TryGetSignedRawRatio(candidate.Z, candidate.Denominator, out Fixed64 z)) |
| | | 46 | | { |
| | 26 | 47 | | support = default; |
| | 26 | 48 | | return false; |
| | | 49 | | } |
| | | 50 | | |
| | 167 | 51 | | support = new Vector2d(x, z); |
| | 167 | 52 | | return true; |
| | | 53 | | } |
| | | 54 | | |
| | | 55 | | private static void KeepBest( |
| | | 56 | | WidePlanarCandidate candidate, |
| | | 57 | | Vector2d direction, |
| | | 58 | | ref bool found, |
| | | 59 | | ref WidePlanarCandidate best) |
| | | 60 | | { |
| | 225 | 61 | | if (found) |
| | | 62 | | { |
| | 45 | 63 | | int projection = CompareProjection(candidate, best, direction); |
| | 45 | 64 | | if (projection < 0 || (projection == 0 && ComesAfter(candidate, best))) |
| | 17 | 65 | | return; |
| | | 66 | | } |
| | | 67 | | |
| | 208 | 68 | | found = true; |
| | 208 | 69 | | best = candidate; |
| | 208 | 70 | | } |
| | | 71 | | |
| | | 72 | | private static int CompareProjection(WidePlanarCandidate left, WidePlanarCandidate right, Vector2d direction) |
| | | 73 | | { |
| | 53 | 74 | | Signed576 leftProjection = WideArithmetic.AddSigned576( |
| | 53 | 75 | | WideArithmetic.MultiplySigned576(left.X, Signed192.Raw(direction.X)), |
| | 53 | 76 | | WideArithmetic.MultiplySigned576(left.Z, Signed192.Raw(direction.Y))); |
| | 53 | 77 | | Signed576 rightProjection = WideArithmetic.AddSigned576( |
| | 53 | 78 | | WideArithmetic.MultiplySigned576(right.X, Signed192.Raw(direction.X)), |
| | 53 | 79 | | WideArithmetic.MultiplySigned576(right.Z, Signed192.Raw(direction.Y))); |
| | 53 | 80 | | return WideArithmetic.SubtractSigned832( |
| | 53 | 81 | | WideArithmetic.MultiplySigned576ToSigned832(leftProjection, right.Denominator), |
| | 53 | 82 | | WideArithmetic.MultiplySigned576ToSigned832(rightProjection, left.Denominator)).Sign; |
| | | 83 | | } |
| | | 84 | | |
| | | 85 | | private static bool ComesAfter(WidePlanarCandidate left, WidePlanarCandidate right) |
| | | 86 | | { |
| | 22 | 87 | | int x = CompareRatio(left.X, left.Denominator, right.X, right.Denominator); |
| | 22 | 88 | | return x > 0 || (x == 0 && CompareRatio(left.Z, left.Denominator, right.Z, right.Denominator) > 0); |
| | | 89 | | } |
| | | 90 | | |
| | | 91 | | private static int CompareRatio(Signed576 left, Signed576 leftDenominator, Signed576 right, Signed576 rightDenominat |
| | 40 | 92 | | WideArithmetic.SubtractSigned832( |
| | 40 | 93 | | WideArithmetic.MultiplySigned576ToSigned832(left, rightDenominator), |
| | 40 | 94 | | WideArithmetic.MultiplySigned576ToSigned832(right, leftDenominator)).Sign; |
| | | 95 | | |
| | | 96 | | private static bool IsInRange(Signed320 numerator, Signed192 denominator, FixedRange range) => |
| | 209 | 97 | | Compare(numerator, WideArithmetic.MultiplySigned192(Signed192.Raw(range.Min), denominator)) >= 0 |
| | 209 | 98 | | && Compare(numerator, WideArithmetic.MultiplySigned192(Signed192.Raw(range.Max), denominator)) <= 0; |
| | | 99 | | |
| | | 100 | | private static bool IsInRange(Signed576 numerator, Signed576 denominator, FixedRange range) => |
| | 50 | 101 | | WideArithmetic.SubtractSigned576(numerator, WideArithmetic.MultiplySigned576(denominator, Signed192.Raw(range.Mi |
| | 50 | 102 | | && WideArithmetic.SubtractSigned576(numerator, WideArithmetic.MultiplySigned576(denominator, Signed192.Raw(range |
| | | 103 | | |
| | | 104 | | private static Signed192 GetPlanarDirectionLength(Vector2d direction) |
| | | 105 | | { |
| | 149 | 106 | | Signed192 squared = WideGeometry.GetDifferenceDotProduct2D( |
| | 149 | 107 | | direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero, |
| | 149 | 108 | | direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero); |
| | 149 | 109 | | return WideArithmetic.GetFloorSquareRoot( |
| | 149 | 110 | | WideArithmetic.MultiplySigned192(squared, ScaleSquared), out _); |
| | | 111 | | } |
| | | 112 | | |
| | | 113 | | private static Signed192 GetPlanarDirectionLengthSquared(Vector2d direction) => |
| | 13 | 114 | | WideGeometry.GetDifferenceDotProduct2D( |
| | 13 | 115 | | direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero, |
| | 13 | 116 | | direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero); |
| | | 117 | | |
| | | 118 | | private static Signed576 SumProducts(Vector3d axis, Signed576 x, Signed576 y, Signed576 z) => |
| | 15 | 119 | | WideArithmetic.AddSigned576( |
| | 15 | 120 | | WideArithmetic.AddSigned576( |
| | 15 | 121 | | WideArithmetic.MultiplySigned576(x, Signed192.Raw(axis.X)), |
| | 15 | 122 | | WideArithmetic.MultiplySigned576(y, Signed192.Raw(axis.Y))), |
| | 15 | 123 | | WideArithmetic.MultiplySigned576(z, Signed192.Raw(axis.Z))); |
| | | 124 | | |
| | | 125 | | private static bool IsUnitInterval(Signed576 numerator, Signed576 denominator) |
| | | 126 | | { |
| | 14 | 127 | | int denominatorSign = denominator.Sign; |
| | 14 | 128 | | int numeratorSign = numerator.Sign; |
| | 14 | 129 | | int remainderSign = WideArithmetic.SubtractSigned576(numerator, denominator).Sign; |
| | 14 | 130 | | return ((denominatorSign > 0) & (numeratorSign >= 0) & (remainderSign <= 0)) |
| | 14 | 131 | | | ((denominatorSign < 0) & (numeratorSign <= 0) & (remainderSign >= 0)); |
| | | 132 | | } |
| | | 133 | | |
| | | 134 | | private static void ReduceDirection( |
| | | 135 | | Signed576 x, |
| | | 136 | | Signed576 y, |
| | | 137 | | Signed576 z, |
| | | 138 | | Signed576 length, |
| | | 139 | | out Signed192 reducedX, |
| | | 140 | | out Signed192 reducedY, |
| | | 141 | | out Signed192 reducedZ, |
| | | 142 | | out Signed192 reducedLength) |
| | | 143 | | { |
| | | 144 | | // The extra zero word makes cross-word shifts branch-free at the |
| | | 145 | | // upper edge of a 576-bit magnitude. |
| | 11 | 146 | | Span<ulong> xMagnitude = stackalloc ulong[10]; |
| | 11 | 147 | | Span<ulong> yMagnitude = stackalloc ulong[10]; |
| | 11 | 148 | | Span<ulong> zMagnitude = stackalloc ulong[10]; |
| | 11 | 149 | | Span<ulong> lengthMagnitude = stackalloc ulong[10]; |
| | 11 | 150 | | xMagnitude.Clear(); |
| | 11 | 151 | | yMagnitude.Clear(); |
| | 11 | 152 | | zMagnitude.Clear(); |
| | 11 | 153 | | lengthMagnitude.Clear(); |
| | 11 | 154 | | WideArithmetic.GetMagnitude(x, xMagnitude[..9]); |
| | 11 | 155 | | WideArithmetic.GetMagnitude(y, yMagnitude[..9]); |
| | 11 | 156 | | WideArithmetic.GetMagnitude(z, zMagnitude[..9]); |
| | 11 | 157 | | WideArithmetic.GetMagnitude(length, lengthMagnitude[..9]); |
| | 11 | 158 | | int shift = Math.Max(0, |
| | 11 | 159 | | Math.Max( |
| | 11 | 160 | | Math.Max(WideArithmetic.GetMagnitudeBitLength(xMagnitude), WideArithmetic.GetMagnitudeBitLength(yMagnitu |
| | 11 | 161 | | Math.Max(WideArithmetic.GetMagnitudeBitLength(zMagnitude), WideArithmetic.GetMagnitudeBitLength(lengthMa |
| | 11 | 162 | | reducedX = CreateReduced(xMagnitude, shift, x.Sign < 0); |
| | 11 | 163 | | reducedY = CreateReduced(yMagnitude, shift, y.Sign < 0); |
| | 11 | 164 | | reducedZ = CreateReduced(zMagnitude, shift, z.Sign < 0); |
| | 11 | 165 | | reducedLength = CreateReduced(lengthMagnitude, shift, false); |
| | 11 | 166 | | } |
| | | 167 | | |
| | | 168 | | private static Signed192 CreateReduced(ReadOnlySpan<ulong> magnitude, int shift, bool negative) |
| | | 169 | | { |
| | 44 | 170 | | int wordShift = shift >> 6; |
| | 44 | 171 | | int bitShift = shift & 63; |
| | 44 | 172 | | ulong low = GetShiftedWord(magnitude, wordShift, bitShift); |
| | 44 | 173 | | ulong middle = GetShiftedWord(magnitude, wordShift + 1, bitShift); |
| | 44 | 174 | | ulong high = GetShiftedWord(magnitude, wordShift + 2, bitShift); |
| | 44 | 175 | | Signed192 value = new(high, middle, low); |
| | 44 | 176 | | return negative ? WideArithmetic.SubtractSigned192(default, value) : value; |
| | | 177 | | } |
| | | 178 | | |
| | | 179 | | private static ulong GetShiftedWord(ReadOnlySpan<ulong> magnitude, int wordIndex, int bitShift) |
| | | 180 | | { |
| | 132 | 181 | | ulong value = magnitude[wordIndex] >> bitShift; |
| | 132 | 182 | | int complementaryShift = (64 - bitShift) & 63; |
| | 132 | 183 | | ulong nonZeroShiftMask = 0UL - ((ulong)(bitShift + 63) >> 6); |
| | 132 | 184 | | return value | ((magnitude[wordIndex + 1] << complementaryShift) & nonZeroShiftMask); |
| | | 185 | | } |
| | | 186 | | |
| | | 187 | | private static Signed192 GetAxisLengthSquared(Vector3d axis) => |
| | 70 | 188 | | WideGeometry.GetDifferenceDotProduct3D( |
| | 70 | 189 | | axis.X, Fixed64.Zero, axis.Y, Fixed64.Zero, axis.Z, Fixed64.Zero, |
| | 70 | 190 | | axis.X, Fixed64.Zero, axis.Y, Fixed64.Zero, axis.Z, Fixed64.Zero); |
| | | 191 | | |
| | | 192 | | private static Signed192 GetPlanarDot(Vector3d axis, Vector2d direction) => |
| | 272 | 193 | | WideGeometry.GetDifferenceDotProduct2D( |
| | 272 | 194 | | axis.X, Fixed64.Zero, axis.Z, Fixed64.Zero, |
| | 272 | 195 | | direction.X, Fixed64.Zero, direction.Y, Fixed64.Zero); |
| | | 196 | | |
| | | 197 | | private static Signed576 SumSquares(Signed320 x, Signed320 y, Signed320 z) => |
| | 106 | 198 | | WideArithmetic.AddSigned576( |
| | 106 | 199 | | WideArithmetic.AddSigned576( |
| | 106 | 200 | | WideArithmetic.MultiplySigned320(x, x), |
| | 106 | 201 | | WideArithmetic.MultiplySigned320(y, y)), |
| | 106 | 202 | | WideArithmetic.MultiplySigned320(z, z)); |
| | | 203 | | |
| | | 204 | | private static Signed320 GetEndpointNumerator(Fixed64 center, Fixed64 axis, Fixed64 axisLength, int sign) => |
| | 621 | 205 | | WideArithmetic.AddSigned320( |
| | 621 | 206 | | WideArithmetic.MultiplySigned192(Signed192.Raw(center), CenteredAxisScale), |
| | 621 | 207 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis), Signed192.Signed(sign * axisLength.m_rawValue))); |
| | | 208 | | |
| | | 209 | | private static Signed320 GetConeEndpointNumerator(Fixed64 center, Fixed64 axis, Fixed64 height, int sign) => |
| | 190 | 210 | | WideArithmetic.AddSigned320( |
| | 190 | 211 | | WideArithmetic.MultiplySigned192(Signed192.Raw(center), WideArithmetic.Double(Scale)), |
| | 190 | 212 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis), Signed192.Signed(sign * height.m_rawValue))); |
| | | 213 | | |
| | | 214 | | private static int Compare(Signed320 left, Signed320 right) => |
| | 419 | 215 | | WideArithmetic.SubtractSigned320(left, right).Sign; |
| | | 216 | | |
| | 2 | 217 | | private static Signed320 Minimum(Signed320 left, Signed320 right) => Compare(left, right) <= 0 ? left : right; |
| | | 218 | | |
| | 5 | 219 | | private static Signed320 Maximum(Signed320 left, Signed320 right) => Compare(left, right) >= 0 ? left : right; |
| | | 220 | | |
| | | 221 | | private static int CompareMagnitude(Signed576 left, Signed576 right) => |
| | 12 | 222 | | WideArithmetic.CompareNonNegative(WideArithmetic.Absolute(left), WideArithmetic.Absolute(right)); |
| | | 223 | | |
| | | 224 | | private static void Normalize(ref Signed576 x, ref Signed576 z, ref Signed576 denominator) |
| | | 225 | | { |
| | 14 | 226 | | if (denominator.Sign >= 0) |
| | 6 | 227 | | return; |
| | 8 | 228 | | x = WideArithmetic.SubtractSigned576(default, x); |
| | 8 | 229 | | z = WideArithmetic.SubtractSigned576(default, z); |
| | 8 | 230 | | denominator = WideArithmetic.SubtractSigned576(default, denominator); |
| | 8 | 231 | | } |
| | | 232 | | |
| | | 233 | | internal static bool TryGetCapsuleSupport( |
| | | 234 | | Vector3d center, |
| | | 235 | | Vector3d axis, |
| | | 236 | | Fixed64 axisLength, |
| | | 237 | | Fixed64 radius, |
| | | 238 | | FixedRange slab, |
| | | 239 | | Vector2d direction, |
| | | 240 | | out Vector2d support) |
| | | 241 | | { |
| | 144 | 242 | | Signed192 directionLength = GetPlanarDirectionLength(direction); |
| | 144 | 243 | | if (TryAdmitCapsuleSupport(center, axis, axisLength, radius, slab, direction, directionLength, out support)) |
| | 134 | 244 | | return true; |
| | | 245 | | |
| | 10 | 246 | | bool found = false; |
| | 10 | 247 | | WidePlanarCandidate best = default; |
| | 10 | 248 | | AddSphereEndpoint(center, axis, axisLength, radius, -1, slab, direction, directionLength, ref found, ref best); |
| | 10 | 249 | | AddSphereEndpoint(center, axis, axisLength, radius, 1, slab, direction, directionLength, ref found, ref best); |
| | 10 | 250 | | AddSpherePlaneCandidate(center, axis, axisLength, radius, -1, slab.Min, direction, directionLength, ref found, r |
| | 10 | 251 | | AddSpherePlaneCandidate(center, axis, axisLength, radius, 1, slab.Min, direction, directionLength, ref found, re |
| | 10 | 252 | | AddCapsuleSideCandidate(center, axis, axisLength, radius, slab.Min, direction, ref found, ref best); |
| | 10 | 253 | | if (slab.Max != slab.Min) |
| | | 254 | | { |
| | 5 | 255 | | AddSpherePlaneCandidate(center, axis, axisLength, radius, -1, slab.Max, direction, directionLength, ref foun |
| | 5 | 256 | | AddSpherePlaneCandidate(center, axis, axisLength, radius, 1, slab.Max, direction, directionLength, ref found |
| | 5 | 257 | | AddCapsuleSideCandidate(center, axis, axisLength, radius, slab.Max, direction, ref found, ref best); |
| | | 258 | | } |
| | | 259 | | |
| | 10 | 260 | | return TryCreateResult(found, best, out support); |
| | | 261 | | } |
| | | 262 | | |
| | | 263 | | internal static bool TryGetCylinderSupport( |
| | | 264 | | Vector3d center, |
| | | 265 | | Vector3d axis, |
| | | 266 | | Fixed64 axisLength, |
| | | 267 | | Fixed64 radius, |
| | | 268 | | FixedRange slab, |
| | | 269 | | Vector2d direction, |
| | | 270 | | out Vector2d support) |
| | | 271 | | { |
| | 15 | 272 | | Signed192 axisLengthSquared = GetAxisLengthSquared(axis); |
| | 15 | 273 | | if (TryAdmitCylinderSupport(center, axis, axisLength, radius, slab, direction, axisLengthSquared, out support)) |
| | 4 | 274 | | return true; |
| | | 275 | | |
| | 11 | 276 | | bool found = false; |
| | 11 | 277 | | WidePlanarCandidate best = default; |
| | 11 | 278 | | AddDiskEndpoint(center, axis, axisLength, radius, -1, slab, direction, axisLengthSquared, ref found, ref best); |
| | 11 | 279 | | AddDiskEndpoint(center, axis, axisLength, radius, 1, slab, direction, axisLengthSquared, ref found, ref best); |
| | 11 | 280 | | AddDiskPlaneCandidates(center, axis, axisLength, radius, -1, slab.Min, direction, axisLengthSquared, ref found, |
| | 11 | 281 | | AddDiskPlaneCandidates(center, axis, axisLength, radius, 1, slab.Min, direction, axisLengthSquared, ref found, r |
| | 11 | 282 | | AddCapsuleSideCandidate(center, axis, axisLength, radius, slab.Min, direction, ref found, ref best); |
| | 11 | 283 | | if (slab.Max != slab.Min) |
| | | 284 | | { |
| | 7 | 285 | | AddDiskPlaneCandidates(center, axis, axisLength, radius, -1, slab.Max, direction, axisLengthSquared, ref fou |
| | 7 | 286 | | AddDiskPlaneCandidates(center, axis, axisLength, radius, 1, slab.Max, direction, axisLengthSquared, ref foun |
| | 7 | 287 | | AddCapsuleSideCandidate(center, axis, axisLength, radius, slab.Max, direction, ref found, ref best); |
| | | 288 | | } |
| | | 289 | | |
| | 11 | 290 | | return TryCreateResult(found, best, out support); |
| | | 291 | | } |
| | | 292 | | |
| | | 293 | | internal static bool TryGetConeSupport( |
| | | 294 | | Vector3d center, |
| | | 295 | | Vector3d axis, |
| | | 296 | | Fixed64 height, |
| | | 297 | | Fixed64 radius, |
| | | 298 | | FixedRange slab, |
| | | 299 | | Vector2d direction, |
| | | 300 | | out Vector2d support) |
| | | 301 | | { |
| | | 302 | | // A vertical cone slice is a disk whose radius changes linearly with Y. |
| | | 303 | | // The general rotated-cone branch is added below after the shared cap |
| | | 304 | | // candidates so the same exact endpoint ownership is retained. |
| | 22 | 305 | | if (axis.X == Fixed64.Zero && axis.Z == Fixed64.Zero) |
| | 7 | 306 | | return TryGetVerticalConeSupport(center, axis, height, radius, slab, direction, out support); |
| | | 307 | | |
| | 15 | 308 | | Signed192 axisLengthSquared = GetAxisLengthSquared(axis); |
| | 15 | 309 | | if (TryAdmitConeSupport(center, axis, height, radius, slab, direction, axisLengthSquared, out support)) |
| | 2 | 310 | | return true; |
| | | 311 | | |
| | 13 | 312 | | return TryGetRotatedConeSupport(center, axis, height, radius, slab, direction, axisLengthSquared, out support); |
| | | 313 | | } |
| | | 314 | | |
| | | 315 | | private static bool TryAdmitCapsuleSupport( |
| | | 316 | | Vector3d center, |
| | | 317 | | Vector3d axis, |
| | | 318 | | Fixed64 axisLength, |
| | | 319 | | Fixed64 radius, |
| | | 320 | | FixedRange slab, |
| | | 321 | | Vector2d direction, |
| | | 322 | | Signed192 directionLength, |
| | | 323 | | out Vector2d support) |
| | | 324 | | { |
| | 144 | 325 | | Signed192 dot = GetPlanarDot(axis, direction); |
| | 144 | 326 | | bool found = false; |
| | 144 | 327 | | WidePlanarCandidate best = default; |
| | 144 | 328 | | if (dot.Sign <= 0) |
| | 7 | 329 | | AddSphereEndpoint(center, axis, axisLength, radius, -1, slab, direction, directionLength, ref found, ref bes |
| | 144 | 330 | | if (dot.Sign >= 0) |
| | 142 | 331 | | AddSphereEndpoint(center, axis, axisLength, radius, 1, slab, direction, directionLength, ref found, ref best |
| | 144 | 332 | | return TryCreateResult(found, best, out support); |
| | | 333 | | } |
| | | 334 | | |
| | | 335 | | private static bool TryAdmitCylinderSupport( |
| | | 336 | | Vector3d center, |
| | | 337 | | Vector3d axis, |
| | | 338 | | Fixed64 axisLength, |
| | | 339 | | Fixed64 radius, |
| | | 340 | | FixedRange slab, |
| | | 341 | | Vector2d direction, |
| | | 342 | | Signed192 axisLengthSquared, |
| | | 343 | | out Vector2d support) |
| | | 344 | | { |
| | 15 | 345 | | if (!HasUniqueDiskSupport(axis, direction, axisLengthSquared)) |
| | | 346 | | { |
| | 6 | 347 | | support = default; |
| | 6 | 348 | | return false; |
| | | 349 | | } |
| | | 350 | | |
| | 9 | 351 | | Signed192 dot = GetPlanarDot(axis, direction); |
| | 9 | 352 | | bool found = false; |
| | 9 | 353 | | WidePlanarCandidate best = default; |
| | 9 | 354 | | if (dot.Sign <= 0) |
| | 8 | 355 | | AddDiskEndpoint(center, axis, axisLength, radius, -1, slab, direction, axisLengthSquared, ref found, ref bes |
| | 9 | 356 | | if (dot.Sign >= 0) |
| | 8 | 357 | | AddDiskEndpoint(center, axis, axisLength, radius, 1, slab, direction, axisLengthSquared, ref found, ref best |
| | 9 | 358 | | return TryCreateResult(found, best, out support); |
| | | 359 | | } |
| | | 360 | | |
| | | 361 | | private static bool TryAdmitConeSupport( |
| | | 362 | | Vector3d center, |
| | | 363 | | Vector3d axis, |
| | | 364 | | Fixed64 height, |
| | | 365 | | Fixed64 radius, |
| | | 366 | | FixedRange slab, |
| | | 367 | | Vector2d direction, |
| | | 368 | | Signed192 axisLengthSquared, |
| | | 369 | | out Vector2d support) |
| | | 370 | | { |
| | 15 | 371 | | if (!HasUniqueDiskSupport(axis, direction, axisLengthSquared)) |
| | | 372 | | { |
| | 2 | 373 | | support = default; |
| | 2 | 374 | | return false; |
| | | 375 | | } |
| | | 376 | | |
| | 13 | 377 | | bool apexFound = false; |
| | 13 | 378 | | WidePlanarCandidate apex = default; |
| | 13 | 379 | | AddConeApex(center, axis, height, slab, direction, ref apexFound, ref apex); |
| | 13 | 380 | | bool baseFound = false; |
| | 13 | 381 | | WidePlanarCandidate baseCandidate = default; |
| | 13 | 382 | | AddConeBaseDisk(center, axis, height, radius, slab, direction, axisLengthSquared, ref baseFound, ref baseCandida |
| | 13 | 383 | | if (!apexFound || !baseFound) |
| | | 384 | | { |
| | 11 | 385 | | support = default; |
| | 11 | 386 | | return false; |
| | | 387 | | } |
| | | 388 | | |
| | 2 | 389 | | bool found = true; |
| | 2 | 390 | | KeepBest(baseCandidate, direction, ref found, ref apex); |
| | 2 | 391 | | return TryCreateResult(true, apex, out support); |
| | | 392 | | } |
| | | 393 | | |
| | | 394 | | private static bool HasUniqueDiskSupport(Vector3d axis, Vector2d direction, Signed192 axisLengthSquared) |
| | | 395 | | { |
| | 30 | 396 | | Signed192 dot = GetPlanarDot(axis, direction); |
| | 30 | 397 | | Signed320 gx = WideArithmetic.SubtractSigned320( |
| | 30 | 398 | | WideArithmetic.MultiplySigned192(axisLengthSquared, Signed192.Raw(direction.X)), |
| | 30 | 399 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.X), dot)); |
| | 30 | 400 | | Signed320 gy = WideArithmetic.SubtractSigned320( |
| | 30 | 401 | | default, |
| | 30 | 402 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), dot)); |
| | 30 | 403 | | Signed320 gz = WideArithmetic.SubtractSigned320( |
| | 30 | 404 | | WideArithmetic.MultiplySigned192(axisLengthSquared, Signed192.Raw(direction.Y)), |
| | 30 | 405 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Z), dot)); |
| | 30 | 406 | | return !SumSquares(gx, gy, gz).IsZero; |
| | | 407 | | } |
| | | 408 | | |
| | | 409 | | private static void AddSphereEndpoint( |
| | | 410 | | Vector3d center, |
| | | 411 | | Vector3d axis, |
| | | 412 | | Fixed64 axisLength, |
| | | 413 | | Fixed64 radius, |
| | | 414 | | int sign, |
| | | 415 | | FixedRange slab, |
| | | 416 | | Vector2d direction, |
| | | 417 | | Signed192 directionLength, |
| | | 418 | | ref bool found, |
| | | 419 | | ref WidePlanarCandidate best) |
| | | 420 | | { |
| | 169 | 421 | | Signed320 y = GetEndpointNumerator(center.Y, axis.Y, axisLength, sign); |
| | 169 | 422 | | if (!IsInRange(y, CenteredAxisScale, slab)) |
| | 23 | 423 | | return; |
| | | 424 | | |
| | 146 | 425 | | KeepBest(CreateEndpointRadialCandidate( |
| | 146 | 426 | | center, axis, axisLength, radius, sign, direction, directionLength), direction, ref found, ref best); |
| | 146 | 427 | | } |
| | | 428 | | |
| | | 429 | | private static void AddSpherePlaneCandidate( |
| | | 430 | | Vector3d center, |
| | | 431 | | Vector3d axis, |
| | | 432 | | Fixed64 axisLength, |
| | | 433 | | Fixed64 radius, |
| | | 434 | | int sign, |
| | | 435 | | Fixed64 plane, |
| | | 436 | | Vector2d direction, |
| | | 437 | | Signed192 directionLength, |
| | | 438 | | ref bool found, |
| | | 439 | | ref WidePlanarCandidate best) |
| | | 440 | | { |
| | 30 | 441 | | Signed320 k = WideArithmetic.SubtractSigned320( |
| | 30 | 442 | | WideArithmetic.MultiplySigned192(WideArithmetic.SubtractSigned192(Signed192.Raw(plane), Signed192.Raw(center |
| | 30 | 443 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Signed(sign * axisLength.m_rawValue))); |
| | 30 | 444 | | Signed576 squaredRadius = WideArithmetic.MultiplySigned576( |
| | 30 | 445 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192(Signed192.Raw(radius), Signed192.Raw(radius))), |
| | 30 | 446 | | CenteredAxisScaleSquared); |
| | 30 | 447 | | Signed576 radicand = WideArithmetic.SubtractSigned576( |
| | 30 | 448 | | squaredRadius, |
| | 30 | 449 | | WideArithmetic.MultiplySigned320(k, k)); |
| | 30 | 450 | | if (radicand.Sign < 0) |
| | 24 | 451 | | return; |
| | | 452 | | |
| | 6 | 453 | | Signed320 root = WideArithmetic.GetFloorSquareRootScaledByFixed64(radicand); |
| | 6 | 454 | | Signed576 denominator = Signed576.ExtendValue( |
| | 6 | 455 | | WideArithmetic.MultiplySigned192(CenteredAxisScale, directionLength)); |
| | 6 | 456 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 6 | 457 | | WideArithmetic.MultiplySigned320(GetEndpointNumerator(center.X, axis.X, axisLength, sign), directionLength), |
| | 6 | 458 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(direction.X))); |
| | 6 | 459 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 6 | 460 | | WideArithmetic.MultiplySigned320(GetEndpointNumerator(center.Z, axis.Z, axisLength, sign), directionLength), |
| | 6 | 461 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(direction.Y))); |
| | 6 | 462 | | KeepBest(new WidePlanarCandidate(x, z, denominator), direction, ref found, ref best); |
| | 6 | 463 | | } |
| | | 464 | | |
| | | 465 | | private static WidePlanarCandidate CreateEndpointRadialCandidate( |
| | | 466 | | Vector3d center, |
| | | 467 | | Vector3d axis, |
| | | 468 | | Fixed64 axisLength, |
| | | 469 | | Fixed64 radius, |
| | | 470 | | int sign, |
| | | 471 | | Vector2d direction, |
| | | 472 | | Signed192 directionLength) |
| | | 473 | | { |
| | 147 | 474 | | Signed576 denominator = Signed576.ExtendValue( |
| | 147 | 475 | | WideArithmetic.MultiplySigned192(CenteredAxisScale, directionLength)); |
| | 147 | 476 | | Signed576 radialScale = WideArithmetic.MultiplySigned576( |
| | 147 | 477 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192( |
| | 147 | 478 | | Signed192.Raw(radius), |
| | 147 | 479 | | CenteredAxisScaleTimesScale)), |
| | 147 | 480 | | Signed192.Raw(direction.X)); |
| | 147 | 481 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 147 | 482 | | WideArithmetic.MultiplySigned320(GetEndpointNumerator(center.X, axis.X, axisLength, sign), directionLength), |
| | 147 | 483 | | radialScale); |
| | 147 | 484 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 147 | 485 | | WideArithmetic.MultiplySigned320(GetEndpointNumerator(center.Z, axis.Z, axisLength, sign), directionLength), |
| | 147 | 486 | | WideArithmetic.MultiplySigned576( |
| | 147 | 487 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192( |
| | 147 | 488 | | Signed192.Raw(radius), |
| | 147 | 489 | | CenteredAxisScaleTimesScale)), |
| | 147 | 490 | | Signed192.Raw(direction.Y))); |
| | 147 | 491 | | return new WidePlanarCandidate(x, z, denominator); |
| | | 492 | | } |
| | | 493 | | |
| | | 494 | | private static void AddCapsuleSideCandidate( |
| | | 495 | | Vector3d center, |
| | | 496 | | Vector3d axis, |
| | | 497 | | Fixed64 axisLength, |
| | | 498 | | Fixed64 radius, |
| | | 499 | | Fixed64 plane, |
| | | 500 | | Vector2d direction, |
| | | 501 | | ref bool found, |
| | | 502 | | ref WidePlanarCandidate best) |
| | | 503 | | { |
| | 33 | 504 | | if (axis.Y == Fixed64.Zero) |
| | 21 | 505 | | return; |
| | | 506 | | |
| | 12 | 507 | | Signed192 m = GetPlanarDot(axis, direction); |
| | 12 | 508 | | int sign = axis.Y.m_rawValue < 0L ? -1 : 1; |
| | 12 | 509 | | Signed192 absY = Signed192.Raw(axis.Y.Abs()); |
| | 12 | 510 | | Signed320 wx = WideArithmetic.MultiplySigned192(absY, Signed192.Raw(direction.X)); |
| | 12 | 511 | | Signed320 wy = Signed320.ExtendValue(sign < 0 ? m : WideArithmetic.SubtractSigned192(default, m)); |
| | 12 | 512 | | Signed320 wz = WideArithmetic.MultiplySigned192(absY, Signed192.Raw(direction.Y)); |
| | 12 | 513 | | Signed576 squared = SumSquares(wx, wy, wz); |
| | 12 | 514 | | Signed320 length = WideArithmetic.GetFloorSquareRootScaledByFixed64(squared); |
| | 12 | 515 | | Signed576 radialX = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(Signed576.ExtendValue(wx), |
| | 12 | 516 | | Signed576 radialY = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(Signed576.ExtendValue(wy), |
| | 12 | 517 | | Signed576 radialZ = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(Signed576.ExtendValue(wz), |
| | 12 | 518 | | Signed576 delta = WideArithmetic.SubtractSigned576( |
| | 12 | 519 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(length), WideArithmetic.SubtractSigned192(Signed192.R |
| | 12 | 520 | | radialY); |
| | 12 | 521 | | Signed576 axisDenominator = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(length), Signed192.Raw(axis.Y |
| | 12 | 522 | | Signed576 parameterNumerator = WideArithmetic.MultiplySigned576(delta, CenteredAxisScale); |
| | 12 | 523 | | Signed576 parameterLimit = WideArithmetic.MultiplySigned576(WideArithmetic.Absolute(axisDenominator), Signed192. |
| | 12 | 524 | | if (CompareMagnitude(parameterNumerator, parameterLimit) > 0) |
| | 6 | 525 | | return; |
| | | 526 | | |
| | 6 | 527 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 6 | 528 | | WideArithmetic.AddSigned576( |
| | 6 | 529 | | WideArithmetic.MultiplySigned576(axisDenominator, Signed192.Raw(center.X)), |
| | 6 | 530 | | WideArithmetic.MultiplySigned576(delta, Signed192.Raw(axis.X))), |
| | 6 | 531 | | WideArithmetic.MultiplySigned576(radialX, Signed192.Raw(axis.Y))); |
| | 6 | 532 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 6 | 533 | | WideArithmetic.AddSigned576( |
| | 6 | 534 | | WideArithmetic.MultiplySigned576(axisDenominator, Signed192.Raw(center.Z)), |
| | 6 | 535 | | WideArithmetic.MultiplySigned576(delta, Signed192.Raw(axis.Z))), |
| | 6 | 536 | | WideArithmetic.MultiplySigned576(radialZ, Signed192.Raw(axis.Y))); |
| | 6 | 537 | | Normalize(ref x, ref z, ref axisDenominator); |
| | 6 | 538 | | KeepBest(new WidePlanarCandidate(x, z, axisDenominator), direction, ref found, ref best); |
| | 6 | 539 | | } |
| | | 540 | | |
| | | 541 | | private static void AddDiskEndpoint( |
| | | 542 | | Vector3d center, |
| | | 543 | | Vector3d axis, |
| | | 544 | | Fixed64 axisLength, |
| | | 545 | | Fixed64 radius, |
| | | 546 | | int sign, |
| | | 547 | | FixedRange slab, |
| | | 548 | | Vector2d direction, |
| | | 549 | | Signed192 axisLengthSquared, |
| | | 550 | | ref bool found, |
| | | 551 | | ref WidePlanarCandidate best) |
| | | 552 | | { |
| | 38 | 553 | | Signed320 baseX = GetEndpointNumerator(center.X, axis.X, axisLength, sign); |
| | 38 | 554 | | Signed320 baseY = GetEndpointNumerator(center.Y, axis.Y, axisLength, sign); |
| | 38 | 555 | | Signed320 baseZ = GetEndpointNumerator(center.Z, axis.Z, axisLength, sign); |
| | 38 | 556 | | AddDiskAtRationalCenter( |
| | 38 | 557 | | baseX, |
| | 38 | 558 | | baseY, |
| | 38 | 559 | | baseZ, |
| | 38 | 560 | | CenteredAxisScale, |
| | 38 | 561 | | axis, |
| | 38 | 562 | | radius, |
| | 38 | 563 | | slab, |
| | 38 | 564 | | direction, |
| | 38 | 565 | | axisLengthSquared, |
| | 38 | 566 | | ref found, |
| | 38 | 567 | | ref best); |
| | 38 | 568 | | } |
| | | 569 | | |
| | | 570 | | private static void AddDiskAtRationalCenter( |
| | | 571 | | Signed320 baseX, |
| | | 572 | | Signed320 baseY, |
| | | 573 | | Signed320 baseZ, |
| | | 574 | | Signed192 baseDenominator, |
| | | 575 | | Vector3d axis, |
| | | 576 | | Fixed64 radius, |
| | | 577 | | FixedRange slab, |
| | | 578 | | Vector2d direction, |
| | | 579 | | Signed192 axisLengthSquared, |
| | | 580 | | ref bool found, |
| | | 581 | | ref WidePlanarCandidate best) |
| | | 582 | | { |
| | 64 | 583 | | Signed192 m = GetPlanarDot(axis, direction); |
| | 64 | 584 | | Signed320 gx = WideArithmetic.SubtractSigned320( |
| | 64 | 585 | | WideArithmetic.MultiplySigned192(axisLengthSquared, Signed192.Raw(direction.X)), |
| | 64 | 586 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.X), m)); |
| | 64 | 587 | | Signed320 gy = WideArithmetic.SubtractSigned320( |
| | 64 | 588 | | default, |
| | 64 | 589 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), m)); |
| | 64 | 590 | | Signed320 gz = WideArithmetic.SubtractSigned320( |
| | 64 | 591 | | WideArithmetic.MultiplySigned192(axisLengthSquared, Signed192.Raw(direction.Y)), |
| | 64 | 592 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Z), m)); |
| | 64 | 593 | | Signed576 squared = SumSquares(gx, gy, gz); |
| | 64 | 594 | | if (squared.IsZero) |
| | | 595 | | { |
| | 14 | 596 | | if (IsInRange(baseY, baseDenominator, slab)) |
| | | 597 | | { |
| | 11 | 598 | | Signed576 denominator = Signed576.ExtendValue(Signed320.ExtendValue(baseDenominator)); |
| | 11 | 599 | | KeepBest(new WidePlanarCandidate( |
| | 11 | 600 | | Signed576.ExtendValue(baseX), |
| | 11 | 601 | | Signed576.ExtendValue(baseZ), |
| | 11 | 602 | | denominator), direction, ref found, ref best); |
| | | 603 | | } |
| | 14 | 604 | | return; |
| | | 605 | | } |
| | | 606 | | |
| | 50 | 607 | | Signed320 length = WideArithmetic.GetFloorSquareRootScaledByFixed64(squared); |
| | 50 | 608 | | Signed576 denominatorWide = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(length), baseDenominator); |
| | 50 | 609 | | Signed320 radialFactor = WideArithmetic.MultiplySigned192(Signed192.Raw(radius), baseDenominator); |
| | 50 | 610 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 50 | 611 | | WideArithmetic.MultiplySigned320(baseX, length), |
| | 50 | 612 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(radialFactor, gx), Scale)); |
| | 50 | 613 | | Signed576 y = WideArithmetic.AddSigned576( |
| | 50 | 614 | | WideArithmetic.MultiplySigned320(baseY, length), |
| | 50 | 615 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(radialFactor, gy), Scale)); |
| | 50 | 616 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 50 | 617 | | WideArithmetic.MultiplySigned320(baseZ, length), |
| | 50 | 618 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(radialFactor, gz), Scale)); |
| | 50 | 619 | | if (!IsInRange(y, denominatorWide, slab)) |
| | 32 | 620 | | return; |
| | | 621 | | |
| | 18 | 622 | | KeepBest(new WidePlanarCandidate(x, z, denominatorWide), direction, ref found, ref best); |
| | 18 | 623 | | } |
| | | 624 | | |
| | | 625 | | private static void AddDiskPlaneCandidates( |
| | | 626 | | Vector3d center, |
| | | 627 | | Vector3d axis, |
| | | 628 | | Fixed64 axisLength, |
| | | 629 | | Fixed64 radius, |
| | | 630 | | int sign, |
| | | 631 | | Fixed64 plane, |
| | | 632 | | Vector2d direction, |
| | | 633 | | Signed192 axisLengthSquared, |
| | | 634 | | ref bool found, |
| | | 635 | | ref WidePlanarCandidate best) |
| | | 636 | | { |
| | 36 | 637 | | Signed192 planarAxisSquared = WideArithmetic.SubtractSigned192( |
| | 36 | 638 | | axisLengthSquared, |
| | 36 | 639 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Raw(axis.Y)))); |
| | 36 | 640 | | Signed320 k = WideArithmetic.SubtractSigned320( |
| | 36 | 641 | | WideArithmetic.MultiplySigned192(WideArithmetic.SubtractSigned192(Signed192.Raw(plane), Signed192.Raw(center |
| | 36 | 642 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Signed(sign * axisLength.m_rawValue))); |
| | 36 | 643 | | if (planarAxisSquared.IsZero) |
| | | 644 | | { |
| | 10 | 645 | | if (!k.IsZero) |
| | 9 | 646 | | return; |
| | 1 | 647 | | KeepBest(CreateEndpointRadialCandidate( |
| | 1 | 648 | | center, axis, axisLength, radius, sign, direction, GetPlanarDirectionLength(direction)), |
| | 1 | 649 | | direction, ref found, ref best); |
| | 1 | 650 | | return; |
| | | 651 | | } |
| | | 652 | | |
| | 26 | 653 | | Signed576 first = WideArithmetic.MultiplySigned576( |
| | 26 | 654 | | WideArithmetic.MultiplySigned576( |
| | 26 | 655 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192(Signed192.Raw(radius), Signed192.Raw(radius))), |
| | 26 | 656 | | CenteredAxisScaleSquared), |
| | 26 | 657 | | planarAxisSquared); |
| | 26 | 658 | | Signed576 second = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(k, k), axisLengthSquared); |
| | 26 | 659 | | Signed576 radicand = WideArithmetic.SubtractSigned576(first, second); |
| | 26 | 660 | | if (radicand.Sign < 0) |
| | 18 | 661 | | return; |
| | | 662 | | |
| | 8 | 663 | | Signed320 root = WideArithmetic.GetFloorSquareRootScaledByFixed64(radicand); |
| | 8 | 664 | | Signed320 denominator320 = WideArithmetic.MultiplySigned192( |
| | 8 | 665 | | CenteredAxisScaleSquared, |
| | 8 | 666 | | planarAxisSquared); |
| | 8 | 667 | | Signed576 denominator = Signed576.ExtendValue(denominator320); |
| | 8 | 668 | | AddDiskPlaneCandidate(center, axis, axisLength, sign, k, root, planarAxisSquared, -axis.Z, axis.X, -1, denominat |
| | 8 | 669 | | AddDiskPlaneCandidate(center, axis, axisLength, sign, k, root, planarAxisSquared, -axis.Z, axis.X, 1, denominato |
| | 8 | 670 | | } |
| | | 671 | | |
| | | 672 | | private static void AddDiskPlaneCandidate( |
| | | 673 | | Vector3d center, |
| | | 674 | | Vector3d axis, |
| | | 675 | | Fixed64 axisLength, |
| | | 676 | | int endpointSign, |
| | | 677 | | Signed320 k, |
| | | 678 | | Signed320 root, |
| | | 679 | | Signed192 planarAxisSquared, |
| | | 680 | | Fixed64 tangentX, |
| | | 681 | | Fixed64 tangentZ, |
| | | 682 | | int tangentSign, |
| | | 683 | | Signed576 denominator, |
| | | 684 | | Vector2d direction, |
| | | 685 | | ref bool found, |
| | | 686 | | ref WidePlanarCandidate best) |
| | | 687 | | { |
| | 16 | 688 | | Signed576 x = WideArithmetic.MultiplySigned576( |
| | 16 | 689 | | WideArithmetic.MultiplySigned320( |
| | 16 | 690 | | GetEndpointNumerator(center.X, axis.X, axisLength, endpointSign), |
| | 16 | 691 | | CenteredAxisScale), |
| | 16 | 692 | | planarAxisSquared); |
| | 16 | 693 | | Signed576 z = WideArithmetic.MultiplySigned576( |
| | 16 | 694 | | WideArithmetic.MultiplySigned320( |
| | 16 | 695 | | GetEndpointNumerator(center.Z, axis.Z, axisLength, endpointSign), |
| | 16 | 696 | | CenteredAxisScale), |
| | 16 | 697 | | planarAxisSquared); |
| | 16 | 698 | | Signed576 xParticular = WideArithmetic.MultiplySigned576( |
| | 16 | 699 | | WideArithmetic.MultiplySigned576( |
| | 16 | 700 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(k), Signed192.Raw(axis.Y)), |
| | 16 | 701 | | Signed192.Raw(axis.X)), |
| | 16 | 702 | | CenteredAxisScale); |
| | 16 | 703 | | Signed576 zParticular = WideArithmetic.MultiplySigned576( |
| | 16 | 704 | | WideArithmetic.MultiplySigned576( |
| | 16 | 705 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(k), Signed192.Raw(axis.Y)), |
| | 16 | 706 | | Signed192.Raw(axis.Z)), |
| | 16 | 707 | | CenteredAxisScale); |
| | 16 | 708 | | x = WideArithmetic.SubtractSigned576(x, xParticular); |
| | 16 | 709 | | z = WideArithmetic.SubtractSigned576(z, zParticular); |
| | 16 | 710 | | Signed576 xTangent = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(tangentX)); |
| | 16 | 711 | | Signed576 zTangent = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(tangentZ)); |
| | | 712 | | // The disk denominator contains the squared centered-axis scale; the |
| | | 713 | | // square root contributes only one factor. |
| | 16 | 714 | | xTangent = WideArithmetic.Double(xTangent); |
| | 16 | 715 | | zTangent = WideArithmetic.Double(zTangent); |
| | 16 | 716 | | x = tangentSign < 0 ? WideArithmetic.SubtractSigned576(x, xTangent) : WideArithmetic.AddSigned576(x, xTangent); |
| | 16 | 717 | | z = tangentSign < 0 ? WideArithmetic.SubtractSigned576(z, zTangent) : WideArithmetic.AddSigned576(z, zTangent); |
| | 16 | 718 | | KeepBest(new WidePlanarCandidate(x, z, denominator), direction, ref found, ref best); |
| | 16 | 719 | | } |
| | | 720 | | |
| | | 721 | | private static bool TryGetVerticalConeSupport( |
| | | 722 | | Vector3d center, |
| | | 723 | | Vector3d axis, |
| | | 724 | | Fixed64 height, |
| | | 725 | | Fixed64 radius, |
| | | 726 | | FixedRange slab, |
| | | 727 | | Vector2d direction, |
| | | 728 | | out Vector2d support) |
| | | 729 | | { |
| | 7 | 730 | | Signed320 apexY = GetConeEndpointNumerator(center.Y, axis.Y, height, 1); |
| | 7 | 731 | | Signed320 baseY = GetConeEndpointNumerator(center.Y, axis.Y, height, -1); |
| | 7 | 732 | | Signed320 lower = WideArithmetic.MultiplySigned192(Signed192.Raw(slab.Min), WideArithmetic.Double(Scale)); |
| | 7 | 733 | | Signed320 upper = WideArithmetic.MultiplySigned192(Signed192.Raw(slab.Max), WideArithmetic.Double(Scale)); |
| | 7 | 734 | | bool pointsUp = axis.Y.m_rawValue >= 0L; |
| | 7 | 735 | | Signed320 selectedY = pointsUp ? Maximum(baseY, lower) : Minimum(baseY, upper); |
| | 7 | 736 | | bool outsideCone = pointsUp |
| | 7 | 737 | | ? Compare(selectedY, apexY) > 0 |
| | 7 | 738 | | : Compare(selectedY, apexY) < 0; |
| | 7 | 739 | | if (Compare(selectedY, lower) < 0 || Compare(selectedY, upper) > 0 || outsideCone) |
| | | 740 | | { |
| | 3 | 741 | | support = default; |
| | 3 | 742 | | return false; |
| | | 743 | | } |
| | | 744 | | |
| | 4 | 745 | | Signed320 axial = axis.Y.m_rawValue >= 0L |
| | 4 | 746 | | ? WideArithmetic.SubtractSigned320(apexY, selectedY) |
| | 4 | 747 | | : WideArithmetic.SubtractSigned320(selectedY, apexY); |
| | 4 | 748 | | Signed576 radiusNumerator = WideArithmetic.MultiplySigned320(axial, Signed192.Raw(radius)); |
| | 4 | 749 | | Signed320 heightDenominator = WideArithmetic.AddSigned320( |
| | 4 | 750 | | WideArithmetic.MultiplySigned192(Signed192.Raw(height), Signed192.Raw(axis.Y.Abs())), |
| | 4 | 751 | | WideArithmetic.MultiplySigned192(Signed192.Raw(height), Signed192.Raw(axis.Y.Abs()))); |
| | 4 | 752 | | Signed576 planarRadiusNumerator = radiusNumerator; |
| | 4 | 753 | | Signed576 planarRadiusDenominator = Signed576.ExtendValue(heightDenominator); |
| | 4 | 754 | | Signed192 directionLength = GetPlanarDirectionLength(direction); |
| | 4 | 755 | | Signed576 denominator = WideArithmetic.MultiplySigned576(planarRadiusDenominator, directionLength); |
| | 4 | 756 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 4 | 757 | | WideArithmetic.MultiplySigned576(denominator, Signed192.Raw(center.X)), |
| | 4 | 758 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(planarRadiusNumerator, Signed192.Raw(direc |
| | 4 | 759 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 4 | 760 | | WideArithmetic.MultiplySigned576(denominator, Signed192.Raw(center.Z)), |
| | 4 | 761 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(planarRadiusNumerator, Signed192.Raw(direc |
| | 4 | 762 | | return TryCreateResult(true, new WidePlanarCandidate(x, z, denominator), out support); |
| | | 763 | | } |
| | | 764 | | |
| | | 765 | | private static bool TryGetRotatedConeSupport( |
| | | 766 | | Vector3d center, |
| | | 767 | | Vector3d axis, |
| | | 768 | | Fixed64 height, |
| | | 769 | | Fixed64 radius, |
| | | 770 | | FixedRange slab, |
| | | 771 | | Vector2d direction, |
| | | 772 | | Signed192 axisLengthSquared, |
| | | 773 | | out Vector2d support) |
| | | 774 | | { |
| | | 775 | | // Endpoint/cap candidates are exact. Lateral slab-plane candidates use |
| | | 776 | | // the cone's exact centered containment polynomial to select the |
| | | 777 | | // representable planar boundary witness without constructing a |
| | | 778 | | // saturated cone endpoint. |
| | 13 | 779 | | bool found = false; |
| | 13 | 780 | | WidePlanarCandidate best = default; |
| | 13 | 781 | | AddConeApex(center, axis, height, slab, direction, ref found, ref best); |
| | 13 | 782 | | AddConeBaseDisk(center, axis, height, radius, slab, direction, axisLengthSquared, ref found, ref best); |
| | 13 | 783 | | AddConeBasePlaneCandidate(center, axis, height, radius, slab.Min, direction, ref found, ref best); |
| | 13 | 784 | | if (slab.Max != slab.Min) |
| | 3 | 785 | | AddConeBasePlaneCandidate(center, axis, height, radius, slab.Max, direction, ref found, ref best); |
| | 13 | 786 | | AddConeLateralCandidates(center, axis, height, radius, slab, direction, ref found, ref best); |
| | 13 | 787 | | return TryCreateResult(found, best, out support); |
| | | 788 | | } |
| | | 789 | | |
| | | 790 | | private static void AddConeApex( |
| | | 791 | | Vector3d center, |
| | | 792 | | Vector3d axis, |
| | | 793 | | Fixed64 height, |
| | | 794 | | FixedRange slab, |
| | | 795 | | Vector2d direction, |
| | | 796 | | ref bool found, |
| | | 797 | | ref WidePlanarCandidate best) |
| | | 798 | | { |
| | 26 | 799 | | Signed320 y = GetConeEndpointNumerator(center.Y, axis.Y, height, 1); |
| | 26 | 800 | | Signed192 denominator = WideArithmetic.Double(Scale); |
| | 26 | 801 | | if (!IsInRange(y, denominator, slab)) |
| | 23 | 802 | | return; |
| | | 803 | | |
| | 3 | 804 | | KeepBest( |
| | 3 | 805 | | new WidePlanarCandidate( |
| | 3 | 806 | | Signed576.ExtendValue(GetConeEndpointNumerator(center.X, axis.X, height, 1)), |
| | 3 | 807 | | Signed576.ExtendValue(GetConeEndpointNumerator(center.Z, axis.Z, height, 1)), |
| | 3 | 808 | | Signed576.ExtendValue(Signed320.ExtendValue(denominator))), |
| | 3 | 809 | | direction, |
| | 3 | 810 | | ref found, |
| | 3 | 811 | | ref best); |
| | 3 | 812 | | } |
| | | 813 | | |
| | | 814 | | private static void AddConeBaseDisk( |
| | | 815 | | Vector3d center, |
| | | 816 | | Vector3d axis, |
| | | 817 | | Fixed64 height, |
| | | 818 | | Fixed64 radius, |
| | | 819 | | FixedRange slab, |
| | | 820 | | Vector2d direction, |
| | | 821 | | Signed192 axisLengthSquared, |
| | | 822 | | ref bool found, |
| | | 823 | | ref WidePlanarCandidate best) |
| | | 824 | | { |
| | | 825 | | // Double every centered quantity so an odd raw-unit height is retained. |
| | 26 | 826 | | Signed320 baseX = GetConeEndpointNumerator(center.X, axis.X, height, -1); |
| | 26 | 827 | | Signed320 baseY = GetConeEndpointNumerator(center.Y, axis.Y, height, -1); |
| | 26 | 828 | | Signed320 baseZ = GetConeEndpointNumerator(center.Z, axis.Z, height, -1); |
| | 26 | 829 | | AddDiskAtRationalCenter(baseX, baseY, baseZ, WideArithmetic.Double(Scale), axis, radius, slab, direction, axisLe |
| | 26 | 830 | | } |
| | | 831 | | |
| | | 832 | | private static void AddConeBasePlaneCandidate( |
| | | 833 | | Vector3d center, |
| | | 834 | | Vector3d axis, |
| | | 835 | | Fixed64 height, |
| | | 836 | | Fixed64 radius, |
| | | 837 | | Fixed64 plane, |
| | | 838 | | Vector2d direction, |
| | | 839 | | ref bool found, |
| | | 840 | | ref WidePlanarCandidate best) |
| | | 841 | | { |
| | | 842 | | const ulong baseParameter = (ulong)long.MaxValue; |
| | 16 | 843 | | if (TryCreateConeDiskPlaneCandidate( |
| | 16 | 844 | | center, axis, height, radius, plane, direction, baseParameter, out WidePlanarCandidate baseCandidate)) |
| | | 845 | | { |
| | 8 | 846 | | KeepBest(baseCandidate, direction, ref found, ref best); |
| | | 847 | | } |
| | | 848 | | |
| | 16 | 849 | | } |
| | | 850 | | |
| | | 851 | | private static void AddConeLateralCandidates( |
| | | 852 | | Vector3d center, |
| | | 853 | | Vector3d axis, |
| | | 854 | | Fixed64 height, |
| | | 855 | | Fixed64 radius, |
| | | 856 | | FixedRange slab, |
| | | 857 | | Vector2d direction, |
| | | 858 | | ref bool found, |
| | | 859 | | ref WidePlanarCandidate best) |
| | | 860 | | { |
| | 13 | 861 | | GetConeLateralStationaryPolynomial(axis, height, radius, direction, |
| | 13 | 862 | | out Signed576 coefficient, out Signed576 projection, out Signed576 constant); |
| | 13 | 863 | | if (coefficient.IsZero) |
| | | 864 | | { |
| | 5 | 865 | | if (!projection.IsZero) |
| | | 866 | | { |
| | 2 | 867 | | Signed576 doubleProjection = WideArithmetic.AddSigned576(projection, projection); |
| | 2 | 868 | | Signed576 absoluteProjection = WideArithmetic.Absolute(doubleProjection); |
| | 2 | 869 | | Signed576 y = WideArithmetic.SubtractSigned576(default, constant); |
| | 2 | 870 | | if (doubleProjection.Sign < 0) |
| | 1 | 871 | | y = WideArithmetic.SubtractSigned576(default, y); |
| | 2 | 872 | | AddConeLateralNormal(center, axis, height, radius, slab, direction, |
| | 2 | 873 | | WideArithmetic.MultiplySigned576(absoluteProjection, Signed192.Raw(direction.X)), y, |
| | 2 | 874 | | WideArithmetic.MultiplySigned576(absoluteProjection, Signed192.Raw(direction.Y)), ref found, ref bes |
| | | 875 | | } |
| | 5 | 876 | | return; |
| | | 877 | | } |
| | | 878 | | |
| | 8 | 879 | | Signed832 discriminant = WideArithmetic.SubtractSigned832( |
| | 8 | 880 | | WideArithmetic.MultiplySigned576ToSigned832(projection, projection), |
| | 8 | 881 | | WideArithmetic.MultiplySigned576ToSigned832(coefficient, constant)); |
| | 8 | 882 | | if (discriminant.Sign < 0) |
| | 1 | 883 | | return; |
| | | 884 | | |
| | 7 | 885 | | Signed576 squareRoot = WideArithmetic.GetFloorSquareRootOfProduct(discriminant, ScaleSquared); |
| | 7 | 886 | | Signed576 scaledProjection = WideArithmetic.MultiplySigned576(projection, Scale); |
| | 7 | 887 | | Signed576 scaledCoefficient = WideArithmetic.MultiplySigned576(coefficient, Scale); |
| | 7 | 888 | | Signed576 absoluteCoefficient = WideArithmetic.Absolute(scaledCoefficient); |
| | 7 | 889 | | Signed576 negativeProjection = WideArithmetic.SubtractSigned576(default, scaledProjection); |
| | 7 | 890 | | Signed576 firstY = WideArithmetic.SubtractSigned576(negativeProjection, squareRoot); |
| | 7 | 891 | | if (coefficient.Sign < 0) |
| | 2 | 892 | | firstY = WideArithmetic.SubtractSigned576(default, firstY); |
| | 7 | 893 | | AddConeLateralNormal(center, axis, height, radius, slab, direction, |
| | 7 | 894 | | WideArithmetic.MultiplySigned576(absoluteCoefficient, Signed192.Raw(direction.X)), firstY, |
| | 7 | 895 | | WideArithmetic.MultiplySigned576(absoluteCoefficient, Signed192.Raw(direction.Y)), ref found, ref best); |
| | 7 | 896 | | if (!squareRoot.IsZero) |
| | | 897 | | { |
| | 6 | 898 | | Signed576 secondY = WideArithmetic.AddSigned576(negativeProjection, squareRoot); |
| | 6 | 899 | | if (coefficient.Sign < 0) |
| | 2 | 900 | | secondY = WideArithmetic.SubtractSigned576(default, secondY); |
| | 6 | 901 | | AddConeLateralNormal(center, axis, height, radius, slab, direction, |
| | 6 | 902 | | WideArithmetic.MultiplySigned576(absoluteCoefficient, Signed192.Raw(direction.X)), secondY, |
| | 6 | 903 | | WideArithmetic.MultiplySigned576(absoluteCoefficient, Signed192.Raw(direction.Y)), ref found, ref best); |
| | | 904 | | } |
| | 7 | 905 | | } |
| | | 906 | | |
| | | 907 | | private static void GetConeLateralStationaryPolynomial( |
| | | 908 | | Vector3d axis, |
| | | 909 | | Fixed64 height, |
| | | 910 | | Fixed64 radius, |
| | | 911 | | Vector2d direction, |
| | | 912 | | out Signed576 coefficient, |
| | | 913 | | out Signed576 projection, |
| | | 914 | | out Signed576 constant) |
| | | 915 | | { |
| | 13 | 916 | | Signed192 q = GetAxisLengthSquared(axis); |
| | 13 | 917 | | Signed192 s = GetPlanarDot(axis, direction); |
| | 13 | 918 | | Signed192 d = GetPlanarDirectionLengthSquared(direction); |
| | 13 | 919 | | Signed320 heightSquared = WideArithmetic.MultiplySigned192(Signed192.Raw(height), Signed192.Raw(height)); |
| | 13 | 920 | | Signed320 radiusSquared = WideArithmetic.MultiplySigned192(Signed192.Raw(radius), Signed192.Raw(radius)); |
| | 13 | 921 | | Signed320 k = Signed320.NarrowValue(WideArithmetic.AddSigned576( |
| | 13 | 922 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(heightSquared), q), |
| | 13 | 923 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(radiusSquared), ScaleSquared))); |
| | 13 | 924 | | Signed320 axisYSquared = WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Raw(axis.Y)); |
| | 13 | 925 | | Signed576 radiusQScaleSquared = WideArithmetic.MultiplySigned576( |
| | 13 | 926 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(radiusSquared), q), ScaleSquared); |
| | | 927 | | |
| | 13 | 928 | | coefficient = WideArithmetic.SubtractSigned576( |
| | 13 | 929 | | WideArithmetic.MultiplySigned320(k, axisYSquared), |
| | 13 | 930 | | radiusQScaleSquared); |
| | 13 | 931 | | projection = WideArithmetic.MultiplySigned320( |
| | 13 | 932 | | k, |
| | 13 | 933 | | WideArithmetic.MultiplySigned192(s, Signed192.Raw(axis.Y))); |
| | 13 | 934 | | constant = WideArithmetic.SubtractSigned576( |
| | 13 | 935 | | WideArithmetic.MultiplySigned320(k, WideArithmetic.MultiplySigned192(s, s)), |
| | 13 | 936 | | WideArithmetic.MultiplySigned576( |
| | 13 | 937 | | WideArithmetic.MultiplySigned576( |
| | 13 | 938 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(radiusSquared), q), d), |
| | 13 | 939 | | ScaleSquared)); |
| | 13 | 940 | | } |
| | | 941 | | |
| | | 942 | | private static void AddConeLateralNormal( |
| | | 943 | | Vector3d center, |
| | | 944 | | Vector3d axis, |
| | | 945 | | Fixed64 height, |
| | | 946 | | Fixed64 radius, |
| | | 947 | | FixedRange slab, |
| | | 948 | | Vector2d direction, |
| | | 949 | | Signed576 nx, |
| | | 950 | | Signed576 ny, |
| | | 951 | | Signed576 nz, |
| | | 952 | | ref bool found, |
| | | 953 | | ref WidePlanarCandidate best) |
| | | 954 | | { |
| | 15 | 955 | | Signed576 axial = SumProducts(axis, nx, ny, nz); |
| | 15 | 956 | | if (axial.Sign < 0 || (radius != Fixed64.Zero && axial.IsZero)) |
| | 4 | 957 | | return; |
| | | 958 | | |
| | 11 | 959 | | Signed192 q = GetAxisLengthSquared(axis); |
| | 11 | 960 | | Signed576 gx = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned576(nx, q), WideArithmetic.Multiply |
| | 11 | 961 | | Signed576 gy = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned576(ny, q), WideArithmetic.Multiply |
| | 11 | 962 | | Signed576 gz = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned576(nz, q), WideArithmetic.Multiply |
| | 11 | 963 | | Signed832 radialSquared = WideArithmetic.AddSigned832( |
| | 11 | 964 | | WideArithmetic.AddSigned832( |
| | 11 | 965 | | WideArithmetic.MultiplySigned576ToSigned832(gx, gx), |
| | 11 | 966 | | WideArithmetic.MultiplySigned576ToSigned832(gy, gy)), |
| | 11 | 967 | | WideArithmetic.MultiplySigned576ToSigned832(gz, gz)); |
| | 11 | 968 | | Signed576 radialLength = WideArithmetic.GetFloorSquareRootOfProduct(radialSquared, ScaleSquared); |
| | 11 | 969 | | ReduceDirection(gx, gy, gz, radialLength, |
| | 11 | 970 | | out Signed192 reducedX, out Signed192 reducedY, out Signed192 reducedZ, out Signed192 reducedLength); |
| | 11 | 971 | | Signed192 doubleScale = WideArithmetic.Double(Scale); |
| | 11 | 972 | | Signed320 apexX = GetConeEndpointNumerator(center.X, axis.X, height, 1); |
| | 11 | 973 | | Signed320 apexY = GetConeEndpointNumerator(center.Y, axis.Y, height, 1); |
| | 11 | 974 | | Signed320 apexZ = GetConeEndpointNumerator(center.Z, axis.Z, height, 1); |
| | 11 | 975 | | Signed320 baseX = GetConeEndpointNumerator(center.X, axis.X, height, -1); |
| | 11 | 976 | | Signed320 baseY = GetConeEndpointNumerator(center.Y, axis.Y, height, -1); |
| | 11 | 977 | | Signed320 baseZ = GetConeEndpointNumerator(center.Z, axis.Z, height, -1); |
| | 11 | 978 | | Signed192 radialScale = WideArithmetic.Double(Signed192.NarrowValue(WideArithmetic.MultiplySigned576( |
| | 11 | 979 | | Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Raw(radius))), ScaleSquared))); |
| | 11 | 980 | | Signed576 rimX = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(baseX, reducedLength), Signed576.E |
| | 11 | 981 | | Signed576 rimY = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(baseY, reducedLength), Signed576.E |
| | 11 | 982 | | Signed576 rimZ = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(baseZ, reducedLength), Signed576.E |
| | 11 | 983 | | Signed576 apexScaleX = WideArithmetic.MultiplySigned320(apexX, reducedLength); |
| | 11 | 984 | | Signed576 apexScaleY = WideArithmetic.MultiplySigned320(apexY, reducedLength); |
| | 11 | 985 | | Signed576 apexScaleZ = WideArithmetic.MultiplySigned320(apexZ, reducedLength); |
| | 11 | 986 | | Signed576 ux = WideArithmetic.SubtractSigned576(rimX, apexScaleX); |
| | 11 | 987 | | Signed576 uy = WideArithmetic.SubtractSigned576(rimY, apexScaleY); |
| | 11 | 988 | | Signed576 uz = WideArithmetic.SubtractSigned576(rimZ, apexScaleZ); |
| | 11 | 989 | | AddConeGeneratorPlane(apexX, apexY, apexZ, ux, uy, uz, reducedLength, |
| | 11 | 990 | | slab.Min, direction, doubleScale, ref found, ref best); |
| | 11 | 991 | | if (slab.Max != slab.Min) |
| | | 992 | | { |
| | 3 | 993 | | AddConeGeneratorPlane(apexX, apexY, apexZ, ux, uy, uz, reducedLength, |
| | 3 | 994 | | slab.Max, direction, doubleScale, ref found, ref best); |
| | | 995 | | } |
| | 11 | 996 | | } |
| | | 997 | | |
| | | 998 | | private static void AddConeGeneratorPlane( |
| | | 999 | | Signed320 apexX, |
| | | 1000 | | Signed320 apexY, |
| | | 1001 | | Signed320 apexZ, |
| | | 1002 | | Signed576 ux, |
| | | 1003 | | Signed576 uy, |
| | | 1004 | | Signed576 uz, |
| | | 1005 | | Signed192 radialLength, |
| | | 1006 | | Fixed64 plane, |
| | | 1007 | | Vector2d direction, |
| | | 1008 | | Signed192 doubleScale, |
| | | 1009 | | ref bool found, |
| | | 1010 | | ref WidePlanarCandidate best) |
| | | 1011 | | { |
| | 14 | 1012 | | Signed320 planeOffset = WideArithmetic.SubtractSigned320(WideArithmetic.MultiplySigned192(Signed192.Raw(plane), |
| | 14 | 1013 | | Signed576 scaledPlaneOffset = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(planeOffset), radialLength) |
| | 14 | 1014 | | if (!IsUnitInterval(scaledPlaneOffset, uy)) |
| | 6 | 1015 | | return; |
| | | 1016 | | |
| | 8 | 1017 | | Signed192 planeOffsetNarrow = Signed192.NarrowValue(planeOffset); |
| | 8 | 1018 | | Signed576 candidateX = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned576(uy, Signed192.NarrowValue(ap |
| | 8 | 1019 | | Signed576 candidateZ = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned576(uy, Signed192.NarrowValue(ap |
| | 8 | 1020 | | Signed576 candidateDenominator = WideArithmetic.MultiplySigned576(uy, doubleScale); |
| | 8 | 1021 | | Normalize(ref candidateX, ref candidateZ, ref candidateDenominator); |
| | 8 | 1022 | | KeepBest(new WidePlanarCandidate(candidateX, candidateZ, candidateDenominator), direction, ref found, ref best); |
| | 8 | 1023 | | } |
| | | 1024 | | |
| | | 1025 | | private static bool TryCreateConeDiskPlaneCandidate( |
| | | 1026 | | Vector3d center, |
| | | 1027 | | Vector3d axis, |
| | | 1028 | | Fixed64 height, |
| | | 1029 | | Fixed64 radius, |
| | | 1030 | | Fixed64 plane, |
| | | 1031 | | Vector2d direction, |
| | | 1032 | | ulong parameter, |
| | | 1033 | | out WidePlanarCandidate candidate) |
| | | 1034 | | { |
| | 16 | 1035 | | Signed192 maximum = Signed192.Signed(long.MaxValue); |
| | 16 | 1036 | | Signed192 parameterValue = Signed192.Signed((long)parameter); |
| | 16 | 1037 | | Signed192 doubleScale = WideArithmetic.Double(Scale); |
| | 16 | 1038 | | Signed320 centerDenominator = WideArithmetic.MultiplySigned192(doubleScale, maximum); |
| | 16 | 1039 | | Signed192 axialWeight = WideArithmetic.SubtractSigned192( |
| | 16 | 1040 | | maximum, |
| | 16 | 1041 | | WideArithmetic.AddSigned192(parameterValue, parameterValue)); |
| | 16 | 1042 | | Signed320 centerX = GetConeDiskCenterNumerator(center.X, axis.X, height, maximum, axialWeight); |
| | 16 | 1043 | | Signed320 centerY = GetConeDiskCenterNumerator(center.Y, axis.Y, height, maximum, axialWeight); |
| | 16 | 1044 | | Signed320 centerZ = GetConeDiskCenterNumerator(center.Z, axis.Z, height, maximum, axialWeight); |
| | 16 | 1045 | | Signed320 radiusNumerator = WideArithmetic.MultiplySigned192(Signed192.Raw(radius), parameterValue); |
| | 16 | 1046 | | Signed192 q = GetAxisLengthSquared(axis); |
| | 16 | 1047 | | Signed192 planarAxisSquared = WideArithmetic.SubtractSigned192( |
| | 16 | 1048 | | q, |
| | 16 | 1049 | | Signed192.NarrowValue(WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Raw(axis.Y)))); |
| | 16 | 1050 | | Signed320 k = Signed320.NarrowValue(WideArithmetic.SubtractSigned576( |
| | 16 | 1051 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(centerDenominator), Signed192.Raw(plane)), |
| | 16 | 1052 | | Signed576.ExtendValue(centerY))); |
| | | 1053 | | |
| | 16 | 1054 | | Signed192 radiusNarrow = Signed192.NarrowValue(radiusNumerator); |
| | 16 | 1055 | | Signed192 centerDenominatorNarrow = Signed192.NarrowValue(centerDenominator); |
| | 16 | 1056 | | Signed192 kNarrow = Signed192.NarrowValue(k); |
| | 16 | 1057 | | Signed320 radiusSquared = WideArithmetic.MultiplySigned192(radiusNarrow, radiusNarrow); |
| | 16 | 1058 | | Signed320 centerDenominatorSquared = WideArithmetic.MultiplySigned192( |
| | 16 | 1059 | | centerDenominatorNarrow, |
| | 16 | 1060 | | centerDenominatorNarrow); |
| | 16 | 1061 | | Signed576 first = WideArithmetic.MultiplySigned576( |
| | 16 | 1062 | | WideArithmetic.MultiplySigned320(radiusSquared, centerDenominatorSquared), |
| | 16 | 1063 | | planarAxisSquared); |
| | 16 | 1064 | | Signed320 kSquared = WideArithmetic.MultiplySigned192(kNarrow, kNarrow); |
| | 16 | 1065 | | Signed320 qParameterSquared = Signed320.NarrowValue(WideArithmetic.MultiplySigned576( |
| | 16 | 1066 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192(maximum, maximum)), |
| | 16 | 1067 | | q)); |
| | 16 | 1068 | | Signed576 second = WideArithmetic.MultiplySigned320(kSquared, qParameterSquared); |
| | 16 | 1069 | | Signed576 radicand = WideArithmetic.SubtractSigned576(first, second); |
| | 16 | 1070 | | if (radicand.Sign < 0) |
| | | 1071 | | { |
| | 8 | 1072 | | candidate = default; |
| | 8 | 1073 | | return false; |
| | | 1074 | | } |
| | | 1075 | | |
| | 8 | 1076 | | Signed320 root = WideArithmetic.GetFloorSquareRootScaledByFixed64(radicand); |
| | 8 | 1077 | | Signed320 denominator320 = Signed320.NarrowValue(WideArithmetic.MultiplySigned576( |
| | 8 | 1078 | | WideArithmetic.MultiplySigned576( |
| | 8 | 1079 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(centerDenominator), maximum), |
| | 8 | 1080 | | planarAxisSquared), |
| | 8 | 1081 | | Scale)); |
| | 8 | 1082 | | Signed576 denominatorWide = Signed576.ExtendValue(denominator320); |
| | 8 | 1083 | | Signed576 common = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576( |
| | 8 | 1084 | | Signed576.ExtendValue(Signed320.ExtendValue(maximum)), |
| | 8 | 1085 | | planarAxisSquared), Scale); |
| | 8 | 1086 | | Signed576 x = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(centerX), Signed192.NarrowValue(common)); |
| | 8 | 1087 | | Signed576 z = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(centerZ), Signed192.NarrowValue(common)); |
| | 8 | 1088 | | Signed576 particularScale = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576( |
| | 8 | 1089 | | Signed576.ExtendValue(Signed320.ExtendValue(maximum)), |
| | 8 | 1090 | | Scale), Signed192.Raw(axis.Y)); |
| | 8 | 1091 | | x = WideArithmetic.SubtractSigned576(x, WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(partic |
| | 8 | 1092 | | z = WideArithmetic.SubtractSigned576(z, WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576(partic |
| | 8 | 1093 | | Signed576 tangentX = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(-axis.Z)); |
| | 8 | 1094 | | Signed576 tangentZ = WideArithmetic.MultiplySigned576(Signed576.ExtendValue(root), Signed192.Raw(axis.X)); |
| | 8 | 1095 | | WidePlanarCandidate firstCandidate = new( |
| | 8 | 1096 | | WideArithmetic.AddSigned576(x, tangentX), |
| | 8 | 1097 | | WideArithmetic.AddSigned576(z, tangentZ), |
| | 8 | 1098 | | denominatorWide); |
| | 8 | 1099 | | WidePlanarCandidate secondCandidate = new( |
| | 8 | 1100 | | WideArithmetic.SubtractSigned576(x, tangentX), |
| | 8 | 1101 | | WideArithmetic.SubtractSigned576(z, tangentZ), |
| | 8 | 1102 | | denominatorWide); |
| | 8 | 1103 | | candidate = CompareProjection(firstCandidate, secondCandidate, direction) >= 0 |
| | 8 | 1104 | | ? firstCandidate |
| | 8 | 1105 | | : secondCandidate; |
| | 8 | 1106 | | return true; |
| | | 1107 | | } |
| | | 1108 | | |
| | | 1109 | | private static Signed320 GetConeDiskCenterNumerator( |
| | | 1110 | | Fixed64 center, |
| | | 1111 | | Fixed64 axis, |
| | | 1112 | | Fixed64 height, |
| | | 1113 | | Signed192 maximum, |
| | | 1114 | | Signed192 axialWeight) => |
| | 48 | 1115 | | WideArithmetic.AddSigned320( |
| | 48 | 1116 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned576( |
| | 48 | 1117 | | Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Raw(center))), |
| | 48 | 1118 | | WideArithmetic.Double(Scale)), maximum)), |
| | 48 | 1119 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(WideArithmetic.MultiplySigned192(Signed192.Raw(axis), |
| | | 1120 | | } |