| | | 1 | | //======================================================================= |
| | | 2 | | // WideTriangleConeIntersection.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 | | namespace FixedMathSharp.Geometry; |
| | | 9 | | |
| | | 10 | | /// <summary> |
| | | 11 | | /// Owns exact full-domain triangle-face reduction against finite cones. |
| | | 12 | | /// </summary> |
| | | 13 | | internal static class WideTriangleConeIntersection |
| | | 14 | | { |
| | 1 | 15 | | private static readonly Signed192 Scale = Signed192.Signed(Fixed64.One.m_rawValue); |
| | 1 | 16 | | private static readonly Signed192 MaximumParameter = Signed192.Signed(Fixed64.MaxValue.m_rawValue); |
| | | 17 | | |
| | | 18 | | internal static bool TryGetFaceMinimumAxialPoint( |
| | | 19 | | FixedTriangle triangle, |
| | | 20 | | Signed192 normalX, |
| | | 21 | | Signed192 normalY, |
| | | 22 | | Signed192 normalZ, |
| | | 23 | | Signed320 normalSquared, |
| | | 24 | | Vector3d apex, |
| | | 25 | | Vector3d axisDirection, |
| | | 26 | | Fixed64 height, |
| | | 27 | | Fixed64 baseRadius, |
| | | 28 | | bool hasEdgeIntersection, |
| | | 29 | | out Vector3d point, |
| | | 30 | | out Fixed64 axialParameter) |
| | | 31 | | { |
| | 24 | 32 | | Signed192 q = GetDot(axisDirection, Vector3d.Zero, axisDirection, Vector3d.Zero); |
| | 24 | 33 | | Signed320 bWide = GetDot(normalX, normalY, normalZ, axisDirection, Vector3d.Zero); |
| | 24 | 34 | | Signed192 b = Signed192.NarrowValue(bWide); |
| | 24 | 35 | | Signed320 c = GetDot(normalX, normalY, normalZ, triangle.A, apex); |
| | 24 | 36 | | Signed576 p = WideArithmetic.SubtractSigned576( |
| | 24 | 37 | | WideArithmetic.MultiplySigned576(Signed576.ExtendValue(normalSquared), q), |
| | 24 | 38 | | WideArithmetic.MultiplySigned320(bWide, bWide)); |
| | | 39 | | |
| | 24 | 40 | | if (p.IsZero) |
| | 8 | 41 | | return TryGetPerpendicularPlanePoint( |
| | 8 | 42 | | triangle, |
| | 8 | 43 | | normalX, |
| | 8 | 44 | | normalY, |
| | 8 | 45 | | normalZ, |
| | 8 | 46 | | apex, |
| | 8 | 47 | | axisDirection, |
| | 8 | 48 | | height, |
| | 8 | 49 | | b, |
| | 8 | 50 | | c, |
| | 8 | 51 | | hasEdgeIntersection, |
| | 8 | 52 | | out point, |
| | 8 | 53 | | out axialParameter); |
| | | 54 | | |
| | 16 | 55 | | Signed320 vX = GetRadialNormalComponent(q, normalX, b, axisDirection.X); |
| | 16 | 56 | | Signed320 vY = GetRadialNormalComponent(q, normalY, b, axisDirection.Y); |
| | 16 | 57 | | Signed320 vZ = GetRadialNormalComponent(q, normalZ, b, axisDirection.Z); |
| | 16 | 58 | | Signed576 wX = GetStationaryDirectionComponent(p, axisDirection.X, bWide, vX); |
| | 16 | 59 | | Signed576 wY = GetStationaryDirectionComponent(p, axisDirection.Y, bWide, vY); |
| | 16 | 60 | | Signed576 wZ = GetStationaryDirectionComponent(p, axisDirection.Z, bWide, vZ); |
| | | 61 | | |
| | 16 | 62 | | CreatePolynomial( |
| | 16 | 63 | | q, |
| | 16 | 64 | | bWide, |
| | 16 | 65 | | c, |
| | 16 | 66 | | p, |
| | 16 | 67 | | height, |
| | 16 | 68 | | baseRadius, |
| | 16 | 69 | | out Signed576 coefficient, |
| | 16 | 70 | | out Signed576 projection, |
| | 16 | 71 | | out Signed576 constant); |
| | 16 | 72 | | if (!TryGetMinimumScaledParameter( |
| | 16 | 73 | | coefficient, |
| | 16 | 74 | | projection, |
| | 16 | 75 | | constant, |
| | 16 | 76 | | b, |
| | 16 | 77 | | c, |
| | 16 | 78 | | height, |
| | 16 | 79 | | out Fixed64 entry)) |
| | | 80 | | { |
| | 2 | 81 | | point = default; |
| | 2 | 82 | | axialParameter = default; |
| | 2 | 83 | | return false; |
| | | 84 | | } |
| | | 85 | | |
| | | 86 | | bool keepFace; |
| | 14 | 87 | | if (!hasEdgeIntersection) |
| | | 88 | | { |
| | 9 | 89 | | keepFace = ContainsNoEdgeProbe( |
| | 9 | 90 | | triangle, |
| | 9 | 91 | | normalX, |
| | 9 | 92 | | normalY, |
| | 9 | 93 | | normalZ, |
| | 9 | 94 | | normalSquared, |
| | 9 | 95 | | apex, |
| | 9 | 96 | | axisDirection, |
| | 9 | 97 | | height, |
| | 9 | 98 | | q, |
| | 9 | 99 | | b, |
| | 9 | 100 | | bWide, |
| | 9 | 101 | | c, |
| | 9 | 102 | | p, |
| | 9 | 103 | | coefficient, |
| | 9 | 104 | | projection, |
| | 9 | 105 | | constant); |
| | | 106 | | } |
| | | 107 | | else |
| | | 108 | | { |
| | 5 | 109 | | GetLowerRootLatticeBracket( |
| | 5 | 110 | | coefficient, |
| | 5 | 111 | | projection, |
| | 5 | 112 | | constant, |
| | 5 | 113 | | entry, |
| | 5 | 114 | | out Fixed64 lowerParameter, |
| | 5 | 115 | | out Fixed64 upperParameter); |
| | 5 | 116 | | keepFace = ContainsStationaryPoint( |
| | 5 | 117 | | triangle, |
| | 5 | 118 | | normalX, |
| | 5 | 119 | | normalY, |
| | 5 | 120 | | normalZ, |
| | 5 | 121 | | normalSquared, |
| | 5 | 122 | | apex, |
| | 5 | 123 | | axisDirection, |
| | 5 | 124 | | height, |
| | 5 | 125 | | q, |
| | 5 | 126 | | bWide, |
| | 5 | 127 | | c, |
| | 5 | 128 | | p, |
| | 5 | 129 | | lowerParameter) |
| | 5 | 130 | | && (upperParameter == lowerParameter |
| | 5 | 131 | | || ContainsStationaryPoint( |
| | 5 | 132 | | triangle, |
| | 5 | 133 | | normalX, |
| | 5 | 134 | | normalY, |
| | 5 | 135 | | normalZ, |
| | 5 | 136 | | normalSquared, |
| | 5 | 137 | | apex, |
| | 5 | 138 | | axisDirection, |
| | 5 | 139 | | height, |
| | 5 | 140 | | q, |
| | 5 | 141 | | bWide, |
| | 5 | 142 | | c, |
| | 5 | 143 | | p, |
| | 5 | 144 | | upperParameter)); |
| | | 145 | | } |
| | 14 | 146 | | if (!keepFace) |
| | | 147 | | { |
| | 4 | 148 | | point = default; |
| | 4 | 149 | | axialParameter = default; |
| | 4 | 150 | | return false; |
| | | 151 | | } |
| | | 152 | | |
| | 10 | 153 | | point = new Vector3d( |
| | 10 | 154 | | GetWitnessCoordinate(apex.X, vX, wX, c, p, height, entry), |
| | 10 | 155 | | GetWitnessCoordinate(apex.Y, vY, wY, c, p, height, entry), |
| | 10 | 156 | | GetWitnessCoordinate(apex.Z, vZ, wZ, c, p, height, entry)); |
| | 10 | 157 | | axialParameter = Fixed64.GetSignedRawRatio( |
| | 10 | 158 | | WideArithmetic.MultiplySigned192( |
| | 10 | 159 | | Signed192.Signed(height.m_rawValue), |
| | 10 | 160 | | Signed192.Signed(entry.m_rawValue)), |
| | 10 | 161 | | MaximumParameter); |
| | 10 | 162 | | return true; |
| | | 163 | | } |
| | | 164 | | |
| | | 165 | | private static bool TryGetMinimumScaledParameter( |
| | | 166 | | Signed576 coefficient, |
| | | 167 | | Signed576 projection, |
| | | 168 | | Signed576 constant, |
| | | 169 | | Signed192 b, |
| | | 170 | | Signed320 c, |
| | | 171 | | Fixed64 height, |
| | | 172 | | out Fixed64 entry) |
| | | 173 | | { |
| | 16 | 174 | | if (FitsSigned832Product(projection, projection) |
| | 16 | 175 | | && FitsSigned832Product(coefficient, constant)) |
| | | 176 | | { |
| | 12 | 177 | | return WideFiniteConeIntersection.TrySolveUnitPolynomial( |
| | 12 | 178 | | coefficient, |
| | 12 | 179 | | projection, |
| | 12 | 180 | | constant, |
| | 12 | 181 | | Fixed64.MaxValue, |
| | 12 | 182 | | out entry, |
| | 12 | 183 | | out _); |
| | | 184 | | } |
| | | 185 | | |
| | 4 | 186 | | if (!TryGetContainedUpperBound( |
| | 4 | 187 | | coefficient, |
| | 4 | 188 | | projection, |
| | 4 | 189 | | constant, |
| | 4 | 190 | | b, |
| | 4 | 191 | | c, |
| | 4 | 192 | | height, |
| | 4 | 193 | | out Signed576 upperNumerator, |
| | 4 | 194 | | out Signed576 upperDenominator)) |
| | | 195 | | { |
| | 1 | 196 | | entry = default; |
| | 1 | 197 | | return false; |
| | | 198 | | } |
| | | 199 | | |
| | 3 | 200 | | ulong low = 0UL; |
| | 3 | 201 | | ulong high = (ulong)Fixed64.MaxValue.m_rawValue; |
| | 3 | 202 | | Signed192 midpointDenominator = new(0UL, 0UL, high << 1); |
| | 192 | 203 | | while (low < high) |
| | | 204 | | { |
| | 189 | 205 | | ulong candidate = low + ((high - low) >> 1); |
| | 189 | 206 | | Signed192 midpointNumerator = new(0UL, 0UL, (candidate << 1) | 1UL); |
| | 189 | 207 | | int upperComparison = CompareRationals( |
| | 189 | 208 | | midpointNumerator, |
| | 189 | 209 | | midpointDenominator, |
| | 189 | 210 | | upperNumerator, |
| | 189 | 211 | | upperDenominator); |
| | 189 | 212 | | int polynomialSign = upperComparison > 0 |
| | 189 | 213 | | ? -1 |
| | 189 | 214 | | : WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter( |
| | 189 | 215 | | coefficient, |
| | 189 | 216 | | projection, |
| | 189 | 217 | | constant, |
| | 189 | 218 | | midpointNumerator, |
| | 189 | 219 | | midpointDenominator); |
| | 189 | 220 | | bool midpointPrecedesRoot = polynomialSign > 0 |
| | 189 | 221 | | || (polynomialSign == 0 && (candidate & 1UL) != 0UL); |
| | 189 | 222 | | if (midpointPrecedesRoot) |
| | 56 | 223 | | low = candidate + 1UL; |
| | | 224 | | else |
| | 133 | 225 | | high = candidate; |
| | | 226 | | } |
| | | 227 | | |
| | 3 | 228 | | entry = Fixed64.FromRaw((long)low); |
| | 3 | 229 | | return true; |
| | | 230 | | } |
| | | 231 | | |
| | | 232 | | private static bool TryGetContainedUpperBound( |
| | | 233 | | Signed576 coefficient, |
| | | 234 | | Signed576 projection, |
| | | 235 | | Signed576 constant, |
| | | 236 | | Signed192 b, |
| | | 237 | | Signed320 c, |
| | | 238 | | Fixed64 height, |
| | | 239 | | out Signed576 numerator, |
| | | 240 | | out Signed576 denominator) |
| | | 241 | | { |
| | 4 | 242 | | if (TryGetNonNegativeAxisPlaneRatio( |
| | 4 | 243 | | b, |
| | 4 | 244 | | c, |
| | 4 | 245 | | height, |
| | 4 | 246 | | out Signed192 axisDenominator, |
| | 4 | 247 | | out Signed320 axisNumerator)) |
| | | 248 | | { |
| | 1 | 249 | | numerator = WideArithmetic.MultiplySigned576( |
| | 1 | 250 | | Signed576.ExtendValue(axisNumerator), |
| | 1 | 251 | | Scale); |
| | 1 | 252 | | denominator = Signed576.ExtendValue( |
| | 1 | 253 | | WideArithmetic.MultiplySigned192( |
| | 1 | 254 | | axisDenominator, |
| | 1 | 255 | | Signed192.Signed(height.m_rawValue))); |
| | 1 | 256 | | return true; |
| | | 257 | | } |
| | | 258 | | |
| | | 259 | | // Without an in-range axis crossing, the absolute plane distance is |
| | | 260 | | // linear on [0, 1]; any admitted interval therefore reaches the cap. |
| | 3 | 261 | | if (WideFiniteConeIntersection.GetPolynomialSignAtScaledParameter( |
| | 3 | 262 | | coefficient, |
| | 3 | 263 | | projection, |
| | 3 | 264 | | constant, |
| | 3 | 265 | | Fixed64.One, |
| | 3 | 266 | | Fixed64.One) > 0) |
| | | 267 | | { |
| | 1 | 268 | | numerator = default; |
| | 1 | 269 | | denominator = default; |
| | 1 | 270 | | return false; |
| | | 271 | | } |
| | | 272 | | |
| | 2 | 273 | | numerator = Signed576.ExtendValue( |
| | 2 | 274 | | Signed320.ExtendValue(Signed192.Signed(1L))); |
| | 2 | 275 | | denominator = numerator; |
| | 2 | 276 | | return true; |
| | | 277 | | } |
| | | 278 | | |
| | | 279 | | private static int CompareRationals( |
| | | 280 | | Signed192 leftNumerator, |
| | | 281 | | Signed192 leftDenominator, |
| | | 282 | | Signed576 rightNumerator, |
| | | 283 | | Signed576 rightDenominator) => |
| | 189 | 284 | | WideArithmetic.CompareNonNegative( |
| | 189 | 285 | | WideArithmetic.MultiplySigned576(rightDenominator, leftNumerator), |
| | 189 | 286 | | WideArithmetic.MultiplySigned576(rightNumerator, leftDenominator)); |
| | | 287 | | |
| | | 288 | | private static bool FitsSigned832Product(Signed576 left, Signed576 right) |
| | | 289 | | { |
| | 31 | 290 | | int leftBits = GetMagnitudeBitLength(left); |
| | 31 | 291 | | int rightBits = GetMagnitudeBitLength(right); |
| | 31 | 292 | | return leftBits == 0 || rightBits == 0 || leftBits + rightBits <= 830; |
| | | 293 | | } |
| | | 294 | | |
| | | 295 | | private static int GetMagnitudeBitLength(Signed576 value) |
| | | 296 | | { |
| | 62 | 297 | | System.Span<ulong> magnitude = stackalloc ulong[9]; |
| | 62 | 298 | | WideArithmetic.GetMagnitude(value, magnitude); |
| | 62 | 299 | | return WideArithmetic.GetMagnitudeBitLength(magnitude); |
| | | 300 | | } |
| | | 301 | | |
| | | 302 | | private static void CreatePolynomial( |
| | | 303 | | Signed192 q, |
| | | 304 | | Signed320 b, |
| | | 305 | | Signed320 c, |
| | | 306 | | Signed576 p, |
| | | 307 | | Fixed64 height, |
| | | 308 | | Fixed64 baseRadius, |
| | | 309 | | out Signed576 coefficient, |
| | | 310 | | out Signed576 projection, |
| | | 311 | | out Signed576 constant) |
| | | 312 | | { |
| | 16 | 313 | | Signed192 heightRaw = Signed192.Signed(height.m_rawValue); |
| | 16 | 314 | | Signed192 radiusRaw = Signed192.Signed(baseRadius.m_rawValue); |
| | 16 | 315 | | Signed576 bSquared = WideArithmetic.MultiplySigned320(b, b); |
| | 16 | 316 | | Signed576 firstCoefficient = WideArithmetic.MultiplySigned576(bSquared, q, heightRaw, heightRaw); |
| | 16 | 317 | | Signed576 secondCoefficient = WideArithmetic.MultiplySigned576(p, radiusRaw, radiusRaw, Scale, Scale); |
| | 16 | 318 | | coefficient = WideArithmetic.SubtractSigned576(firstCoefficient, secondCoefficient); |
| | | 319 | | |
| | 16 | 320 | | Signed576 bc = WideArithmetic.MultiplySigned320(b, c); |
| | 16 | 321 | | projection = WideArithmetic.SubtractSigned576( |
| | 16 | 322 | | default, |
| | 16 | 323 | | WideArithmetic.MultiplySigned576(bc, q, Scale, heightRaw)); |
| | 16 | 324 | | constant = WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(c, c), q, Scale, Scale); |
| | 16 | 325 | | } |
| | | 326 | | |
| | | 327 | | private static void GetLowerRootLatticeBracket( |
| | | 328 | | Signed576 coefficient, |
| | | 329 | | Signed576 projection, |
| | | 330 | | Signed576 constant, |
| | | 331 | | Fixed64 entry, |
| | | 332 | | out Fixed64 lowerParameter, |
| | | 333 | | out Fixed64 upperParameter) |
| | | 334 | | { |
| | 5 | 335 | | int entrySign = WideFiniteConeIntersection.GetPolynomialSignAtScaledParameter( |
| | 5 | 336 | | coefficient, |
| | 5 | 337 | | projection, |
| | 5 | 338 | | constant, |
| | 5 | 339 | | entry, |
| | 5 | 340 | | Fixed64.MaxValue); |
| | 5 | 341 | | if (entrySign == 0) |
| | | 342 | | { |
| | 1 | 343 | | lowerParameter = entry; |
| | 1 | 344 | | upperParameter = entry; |
| | 1 | 345 | | return; |
| | | 346 | | } |
| | | 347 | | |
| | 4 | 348 | | if (entrySign > 0) |
| | | 349 | | { |
| | 3 | 350 | | lowerParameter = entry; |
| | 3 | 351 | | upperParameter = Fixed64.FromRaw(entry.m_rawValue + 1L); |
| | 3 | 352 | | return; |
| | | 353 | | } |
| | | 354 | | |
| | 1 | 355 | | upperParameter = entry; |
| | 1 | 356 | | lowerParameter = Fixed64.FromRaw(entry.m_rawValue - 1L); |
| | 1 | 357 | | } |
| | | 358 | | |
| | | 359 | | private static bool ContainsNoEdgeProbe( |
| | | 360 | | FixedTriangle triangle, |
| | | 361 | | Signed192 normalX, |
| | | 362 | | Signed192 normalY, |
| | | 363 | | Signed192 normalZ, |
| | | 364 | | Signed320 normalSquared, |
| | | 365 | | Vector3d apex, |
| | | 366 | | Vector3d axisDirection, |
| | | 367 | | Fixed64 height, |
| | | 368 | | Signed192 q, |
| | | 369 | | Signed192 b, |
| | | 370 | | Signed320 bWide, |
| | | 371 | | Signed320 c, |
| | | 372 | | Signed576 p, |
| | | 373 | | Signed576 coefficient, |
| | | 374 | | Signed576 projection, |
| | | 375 | | Signed576 constant) |
| | | 376 | | { |
| | 9 | 377 | | if (TryGetNonNegativeAxisPlaneRatio(b, c, height, out Signed192 denominator, out Signed320 numerator)) |
| | | 378 | | { |
| | 2 | 379 | | return ContainsAxisPlanePoint( |
| | 2 | 380 | | triangle, |
| | 2 | 381 | | normalX, |
| | 2 | 382 | | normalY, |
| | 2 | 383 | | normalZ, |
| | 2 | 384 | | apex, |
| | 2 | 385 | | axisDirection, |
| | 2 | 386 | | denominator, |
| | 2 | 387 | | numerator); |
| | | 388 | | } |
| | | 389 | | |
| | 7 | 390 | | return ContainsStationaryPoint( |
| | 7 | 391 | | triangle, |
| | 7 | 392 | | normalX, |
| | 7 | 393 | | normalY, |
| | 7 | 394 | | normalZ, |
| | 7 | 395 | | normalSquared, |
| | 7 | 396 | | apex, |
| | 7 | 397 | | axisDirection, |
| | 7 | 398 | | height, |
| | 7 | 399 | | q, |
| | 7 | 400 | | bWide, |
| | 7 | 401 | | c, |
| | 7 | 402 | | p, |
| | 7 | 403 | | Fixed64.MaxValue); |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | private static bool TryGetPerpendicularPlanePoint( |
| | | 407 | | FixedTriangle triangle, |
| | | 408 | | Signed192 normalX, |
| | | 409 | | Signed192 normalY, |
| | | 410 | | Signed192 normalZ, |
| | | 411 | | Vector3d apex, |
| | | 412 | | Vector3d axisDirection, |
| | | 413 | | Fixed64 height, |
| | | 414 | | Signed192 b, |
| | | 415 | | Signed320 c, |
| | | 416 | | bool hasEdgeIntersection, |
| | | 417 | | out Vector3d point, |
| | | 418 | | out Fixed64 axialParameter) |
| | | 419 | | { |
| | 8 | 420 | | if (hasEdgeIntersection |
| | 8 | 421 | | || !TryGetNonNegativeAxisPlaneRatio(b, c, height, out Signed192 denominator, out Signed320 numerator) |
| | 8 | 422 | | || !ContainsAxisPlanePoint( |
| | 8 | 423 | | triangle, |
| | 8 | 424 | | normalX, |
| | 8 | 425 | | normalY, |
| | 8 | 426 | | normalZ, |
| | 8 | 427 | | apex, |
| | 8 | 428 | | axisDirection, |
| | 8 | 429 | | denominator, |
| | 8 | 430 | | numerator)) |
| | | 431 | | { |
| | 6 | 432 | | point = default; |
| | 6 | 433 | | axialParameter = default; |
| | 6 | 434 | | return false; |
| | | 435 | | } |
| | | 436 | | |
| | 2 | 437 | | point = new Vector3d( |
| | 2 | 438 | | GetAxisPlaneCoordinate(apex.X, axisDirection.X, denominator, numerator), |
| | 2 | 439 | | GetAxisPlaneCoordinate(apex.Y, axisDirection.Y, denominator, numerator), |
| | 2 | 440 | | GetAxisPlaneCoordinate(apex.Z, axisDirection.Z, denominator, numerator)); |
| | 2 | 441 | | Signed576 axialNumerator = WideArithmetic.MultiplySigned576( |
| | 2 | 442 | | Signed576.ExtendValue(numerator), |
| | 2 | 443 | | Scale); |
| | 2 | 444 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 2 | 445 | | axialNumerator, |
| | 2 | 446 | | Signed576.ExtendValue(Signed320.ExtendValue(denominator)), |
| | 2 | 447 | | out axialParameter); |
| | 2 | 448 | | return true; |
| | | 449 | | } |
| | | 450 | | |
| | | 451 | | private static bool TryGetNonNegativeAxisPlaneRatio( |
| | | 452 | | Signed192 b, |
| | | 453 | | Signed320 c, |
| | | 454 | | Fixed64 height, |
| | | 455 | | out Signed192 denominator, |
| | | 456 | | out Signed320 numerator) |
| | | 457 | | { |
| | 20 | 458 | | if (b.Sign == 0) |
| | | 459 | | { |
| | 10 | 460 | | denominator = default; |
| | 10 | 461 | | numerator = default; |
| | 10 | 462 | | return false; |
| | | 463 | | } |
| | | 464 | | |
| | 10 | 465 | | denominator = b.Sign > 0 |
| | 10 | 466 | | ? b |
| | 10 | 467 | | : WideArithmetic.SubtractSigned192(default, b); |
| | 10 | 468 | | numerator = b.Sign > 0 |
| | 10 | 469 | | ? c |
| | 10 | 470 | | : WideArithmetic.SubtractSigned320(default, c); |
| | 10 | 471 | | if (numerator.Sign < 0) |
| | 1 | 472 | | return false; |
| | | 473 | | |
| | 9 | 474 | | Signed576 scaledNumerator = WideArithmetic.MultiplySigned576( |
| | 9 | 475 | | Signed576.ExtendValue(numerator), |
| | 9 | 476 | | Scale); |
| | 9 | 477 | | Signed576 maximumNumerator = Signed576.ExtendValue( |
| | 9 | 478 | | WideArithmetic.MultiplySigned192( |
| | 9 | 479 | | denominator, |
| | 9 | 480 | | Signed192.Signed(height.m_rawValue))); |
| | 9 | 481 | | return WideArithmetic.CompareNonNegative(scaledNumerator, maximumNumerator) <= 0; |
| | | 482 | | } |
| | | 483 | | |
| | | 484 | | private static bool ContainsAxisPlanePoint( |
| | | 485 | | FixedTriangle triangle, |
| | | 486 | | Signed192 normalX, |
| | | 487 | | Signed192 normalY, |
| | | 488 | | Signed192 normalZ, |
| | | 489 | | Vector3d apex, |
| | | 490 | | Vector3d axisDirection, |
| | | 491 | | Signed192 denominator, |
| | | 492 | | Signed320 numerator) => |
| | 7 | 493 | | GetAxisPlaneHalfspaceSign(triangle.A, triangle.B, normalX, normalY, normalZ, apex, axisDirection, denominator, n |
| | 7 | 494 | | && GetAxisPlaneHalfspaceSign(triangle.B, triangle.C, normalX, normalY, normalZ, apex, axisDirection, denominator |
| | 7 | 495 | | && GetAxisPlaneHalfspaceSign(triangle.C, triangle.A, normalX, normalY, normalZ, apex, axisDirection, denominator |
| | | 496 | | |
| | | 497 | | private static int GetAxisPlaneHalfspaceSign( |
| | | 498 | | Vector3d edgeStart, |
| | | 499 | | Vector3d edgeEnd, |
| | | 500 | | Signed192 normalX, |
| | | 501 | | Signed192 normalY, |
| | | 502 | | Signed192 normalZ, |
| | | 503 | | Vector3d apex, |
| | | 504 | | Vector3d axisDirection, |
| | | 505 | | Signed192 denominator, |
| | | 506 | | Signed320 numerator) |
| | | 507 | | { |
| | 18 | 508 | | GetEdgeProducts( |
| | 18 | 509 | | edgeStart, |
| | 18 | 510 | | edgeEnd, |
| | 18 | 511 | | normalX, |
| | 18 | 512 | | normalY, |
| | 18 | 513 | | normalZ, |
| | 18 | 514 | | apex, |
| | 18 | 515 | | axisDirection, |
| | 18 | 516 | | out Signed320 hO, |
| | 18 | 517 | | out Signed320 l); |
| | 18 | 518 | | Signed576 first = WideArithmetic.MultiplySigned576( |
| | 18 | 519 | | Signed576.ExtendValue(hO), |
| | 18 | 520 | | denominator); |
| | 18 | 521 | | Signed576 second = WideArithmetic.MultiplySigned320(numerator, l); |
| | 18 | 522 | | return WideArithmetic.AddSigned576(first, second).Sign; |
| | | 523 | | } |
| | | 524 | | |
| | | 525 | | private static bool ContainsStationaryPoint( |
| | | 526 | | FixedTriangle triangle, |
| | | 527 | | Signed192 normalX, |
| | | 528 | | Signed192 normalY, |
| | | 529 | | Signed192 normalZ, |
| | | 530 | | Signed320 normalSquared, |
| | | 531 | | Vector3d apex, |
| | | 532 | | Vector3d axisDirection, |
| | | 533 | | Fixed64 height, |
| | | 534 | | Signed192 q, |
| | | 535 | | Signed320 b, |
| | | 536 | | Signed320 c, |
| | | 537 | | Signed576 p, |
| | | 538 | | Fixed64 scaledParameter) => |
| | 14 | 539 | | GetStationaryHalfspaceSign(triangle.A, triangle.B, normalX, normalY, normalZ, normalSquared, apex, axisDirection |
| | 14 | 540 | | && GetStationaryHalfspaceSign(triangle.B, triangle.C, normalX, normalY, normalZ, normalSquared, apex, axisDirect |
| | 14 | 541 | | && GetStationaryHalfspaceSign(triangle.C, triangle.A, normalX, normalY, normalZ, normalSquared, apex, axisDirect |
| | | 542 | | |
| | | 543 | | private static int GetStationaryHalfspaceSign( |
| | | 544 | | Vector3d edgeStart, |
| | | 545 | | Vector3d edgeEnd, |
| | | 546 | | Signed192 normalX, |
| | | 547 | | Signed192 normalY, |
| | | 548 | | Signed192 normalZ, |
| | | 549 | | Signed320 normalSquared, |
| | | 550 | | Vector3d apex, |
| | | 551 | | Vector3d axisDirection, |
| | | 552 | | Fixed64 height, |
| | | 553 | | Signed192 q, |
| | | 554 | | Signed320 b, |
| | | 555 | | Signed320 c, |
| | | 556 | | Signed576 p, |
| | | 557 | | Fixed64 scaledParameter) |
| | | 558 | | { |
| | 34 | 559 | | GetEdgeProducts( |
| | 34 | 560 | | edgeStart, |
| | 34 | 561 | | edgeEnd, |
| | 34 | 562 | | normalX, |
| | 34 | 563 | | normalY, |
| | 34 | 564 | | normalZ, |
| | 34 | 565 | | apex, |
| | 34 | 566 | | axisDirection, |
| | 34 | 567 | | out Signed320 hO, |
| | 34 | 568 | | out Signed320 l); |
| | 34 | 569 | | Signed192 scaleParameter = Signed192.Signed(Fixed64.MaxValue.m_rawValue); |
| | 34 | 570 | | Signed192 parameter = Signed192.Signed(scaledParameter.m_rawValue); |
| | 34 | 571 | | Signed192 heightRaw = Signed192.Signed(height.m_rawValue); |
| | | 572 | | |
| | 34 | 573 | | Signed576 pScale = WideArithmetic.MultiplySigned576(p, scaleParameter, Scale); |
| | 34 | 574 | | Signed704 first = WideArithmetic.MultiplySigned576ToSigned704(pScale, hO); |
| | | 575 | | |
| | 34 | 576 | | Signed576 negativeBC = WideArithmetic.SubtractSigned576( |
| | 34 | 577 | | default, |
| | 34 | 578 | | WideArithmetic.MultiplySigned320(b, c)); |
| | 34 | 579 | | Signed704 second = WideArithmetic.MultiplySigned576ToSigned704( |
| | 34 | 580 | | WideArithmetic.MultiplySigned576(negativeBC, scaleParameter, Scale), |
| | 34 | 581 | | l); |
| | | 582 | | |
| | 34 | 583 | | Signed576 normalEdge = WideArithmetic.MultiplySigned320(normalSquared, l); |
| | 34 | 584 | | Signed192 qHeight = Signed192.NarrowValue( |
| | 34 | 585 | | WideArithmetic.MultiplySigned192(q, heightRaw)); |
| | 34 | 586 | | Signed320 qHeightParameter = WideArithmetic.MultiplySigned192(qHeight, parameter); |
| | 34 | 587 | | Signed704 third = WideArithmetic.MultiplySigned576ToSigned704( |
| | 34 | 588 | | normalEdge, |
| | 34 | 589 | | qHeightParameter); |
| | 34 | 590 | | return WideArithmetic.AddSigned704( |
| | 34 | 591 | | WideArithmetic.AddSigned704(first, second), |
| | 34 | 592 | | third).Sign; |
| | | 593 | | } |
| | | 594 | | |
| | | 595 | | private static void GetEdgeProducts( |
| | | 596 | | Vector3d edgeStart, |
| | | 597 | | Vector3d edgeEnd, |
| | | 598 | | Signed192 normalX, |
| | | 599 | | Signed192 normalY, |
| | | 600 | | Signed192 normalZ, |
| | | 601 | | Vector3d apex, |
| | | 602 | | Vector3d axisDirection, |
| | | 603 | | out Signed320 hO, |
| | | 604 | | out Signed320 l) |
| | | 605 | | { |
| | 52 | 606 | | Signed192 edgeX = WideArithmetic.Difference(edgeEnd.X, edgeStart.X); |
| | 52 | 607 | | Signed192 edgeY = WideArithmetic.Difference(edgeEnd.Y, edgeStart.Y); |
| | 52 | 608 | | Signed192 edgeZ = WideArithmetic.Difference(edgeEnd.Z, edgeStart.Z); |
| | 52 | 609 | | GetCross( |
| | 52 | 610 | | edgeX, |
| | 52 | 611 | | edgeY, |
| | 52 | 612 | | edgeZ, |
| | 52 | 613 | | WideArithmetic.Difference(apex.X, edgeStart.X), |
| | 52 | 614 | | WideArithmetic.Difference(apex.Y, edgeStart.Y), |
| | 52 | 615 | | WideArithmetic.Difference(apex.Z, edgeStart.Z), |
| | 52 | 616 | | out Signed192 originCrossX, |
| | 52 | 617 | | out Signed192 originCrossY, |
| | 52 | 618 | | out Signed192 originCrossZ); |
| | 52 | 619 | | GetCross( |
| | 52 | 620 | | edgeX, |
| | 52 | 621 | | edgeY, |
| | 52 | 622 | | edgeZ, |
| | 52 | 623 | | Signed192.Signed(axisDirection.X.m_rawValue), |
| | 52 | 624 | | Signed192.Signed(axisDirection.Y.m_rawValue), |
| | 52 | 625 | | Signed192.Signed(axisDirection.Z.m_rawValue), |
| | 52 | 626 | | out Signed192 axisCrossX, |
| | 52 | 627 | | out Signed192 axisCrossY, |
| | 52 | 628 | | out Signed192 axisCrossZ); |
| | 52 | 629 | | hO = GetDot(normalX, normalY, normalZ, originCrossX, originCrossY, originCrossZ); |
| | 52 | 630 | | l = GetDot(normalX, normalY, normalZ, axisCrossX, axisCrossY, axisCrossZ); |
| | 52 | 631 | | } |
| | | 632 | | |
| | | 633 | | private static Fixed64 GetWitnessCoordinate( |
| | | 634 | | Fixed64 apexCoordinate, |
| | | 635 | | Signed320 v, |
| | | 636 | | Signed576 w, |
| | | 637 | | Signed320 c, |
| | | 638 | | Signed576 p, |
| | | 639 | | Fixed64 height, |
| | | 640 | | Fixed64 scaledParameter) |
| | | 641 | | { |
| | 30 | 642 | | Signed576 baseValue = WideArithmetic.AddSigned576( |
| | 30 | 643 | | WideArithmetic.MultiplySigned576( |
| | 30 | 644 | | p, |
| | 30 | 645 | | Signed192.Signed(apexCoordinate.m_rawValue)), |
| | 30 | 646 | | WideArithmetic.MultiplySigned320(c, v)); |
| | 30 | 647 | | Signed576 first = WideArithmetic.MultiplySigned576(baseValue, MaximumParameter, Scale); |
| | 30 | 648 | | Signed576 second = WideArithmetic.MultiplySigned576( |
| | 30 | 649 | | w, |
| | 30 | 650 | | Signed192.Signed(scaledParameter.m_rawValue), |
| | 30 | 651 | | Signed192.Signed(height.m_rawValue)); |
| | 30 | 652 | | Signed576 denominator = WideArithmetic.MultiplySigned576(p, MaximumParameter, Scale); |
| | 30 | 653 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 30 | 654 | | WideArithmetic.AddSigned576(first, second), |
| | 30 | 655 | | denominator, |
| | 30 | 656 | | out Fixed64 coordinate); |
| | 30 | 657 | | return coordinate; |
| | | 658 | | } |
| | | 659 | | |
| | | 660 | | private static Fixed64 GetAxisPlaneCoordinate( |
| | | 661 | | Fixed64 apexCoordinate, |
| | | 662 | | Fixed64 axisCoordinate, |
| | | 663 | | Signed192 denominator, |
| | | 664 | | Signed320 numerator) |
| | | 665 | | { |
| | 6 | 666 | | Signed576 first = Signed576.ExtendValue( |
| | 6 | 667 | | WideArithmetic.MultiplySigned192( |
| | 6 | 668 | | denominator, |
| | 6 | 669 | | Signed192.Signed(apexCoordinate.m_rawValue))); |
| | 6 | 670 | | Signed576 second = WideArithmetic.MultiplySigned576( |
| | 6 | 671 | | Signed576.ExtendValue(numerator), |
| | 6 | 672 | | Signed192.Signed(axisCoordinate.m_rawValue)); |
| | 6 | 673 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 6 | 674 | | WideArithmetic.AddSigned576(first, second), |
| | 6 | 675 | | Signed576.ExtendValue(Signed320.ExtendValue(denominator)), |
| | 6 | 676 | | out Fixed64 coordinate); |
| | 6 | 677 | | return coordinate; |
| | | 678 | | } |
| | | 679 | | |
| | | 680 | | private static Signed320 GetRadialNormalComponent( |
| | | 681 | | Signed192 q, |
| | | 682 | | Signed192 normal, |
| | | 683 | | Signed192 b, |
| | | 684 | | Fixed64 axis) => |
| | 48 | 685 | | WideArithmetic.SubtractSigned320( |
| | 48 | 686 | | WideArithmetic.MultiplySigned192(q, normal), |
| | 48 | 687 | | WideArithmetic.MultiplySigned192( |
| | 48 | 688 | | b, |
| | 48 | 689 | | Signed192.Signed(axis.m_rawValue))); |
| | | 690 | | |
| | | 691 | | private static Signed576 GetStationaryDirectionComponent( |
| | | 692 | | Signed576 p, |
| | | 693 | | Fixed64 axis, |
| | | 694 | | Signed320 b, |
| | | 695 | | Signed320 v) => |
| | 48 | 696 | | WideArithmetic.SubtractSigned576( |
| | 48 | 697 | | WideArithmetic.MultiplySigned576( |
| | 48 | 698 | | p, |
| | 48 | 699 | | Signed192.Signed(axis.m_rawValue)), |
| | 48 | 700 | | WideArithmetic.MultiplySigned320(b, v)); |
| | | 701 | | |
| | | 702 | | private static Signed192 GetDot( |
| | | 703 | | Vector3d leftEnd, |
| | | 704 | | Vector3d leftStart, |
| | | 705 | | Vector3d rightEnd, |
| | | 706 | | Vector3d rightStart) => |
| | 24 | 707 | | WideGeometry.GetDifferenceDotProduct3D( |
| | 24 | 708 | | leftEnd.X, leftStart.X, leftEnd.Y, leftStart.Y, leftEnd.Z, leftStart.Z, |
| | 24 | 709 | | rightEnd.X, rightStart.X, rightEnd.Y, rightStart.Y, rightEnd.Z, rightStart.Z); |
| | | 710 | | |
| | | 711 | | private static Signed320 GetDot( |
| | | 712 | | Signed192 leftX, |
| | | 713 | | Signed192 leftY, |
| | | 714 | | Signed192 leftZ, |
| | | 715 | | Vector3d rightEnd, |
| | | 716 | | Vector3d rightStart) => |
| | 48 | 717 | | GetDot( |
| | 48 | 718 | | leftX, |
| | 48 | 719 | | leftY, |
| | 48 | 720 | | leftZ, |
| | 48 | 721 | | WideArithmetic.Difference(rightEnd.X, rightStart.X), |
| | 48 | 722 | | WideArithmetic.Difference(rightEnd.Y, rightStart.Y), |
| | 48 | 723 | | WideArithmetic.Difference(rightEnd.Z, rightStart.Z)); |
| | | 724 | | |
| | | 725 | | private static Signed320 GetDot( |
| | | 726 | | Signed192 leftX, |
| | | 727 | | Signed192 leftY, |
| | | 728 | | Signed192 leftZ, |
| | | 729 | | Signed192 rightX, |
| | | 730 | | Signed192 rightY, |
| | | 731 | | Signed192 rightZ) => |
| | 152 | 732 | | WideArithmetic.AddSigned320( |
| | 152 | 733 | | WideArithmetic.AddSigned320( |
| | 152 | 734 | | WideArithmetic.MultiplySigned192(leftX, rightX), |
| | 152 | 735 | | WideArithmetic.MultiplySigned192(leftY, rightY)), |
| | 152 | 736 | | WideArithmetic.MultiplySigned192(leftZ, rightZ)); |
| | | 737 | | |
| | | 738 | | private static void GetCross( |
| | | 739 | | Signed192 leftX, |
| | | 740 | | Signed192 leftY, |
| | | 741 | | Signed192 leftZ, |
| | | 742 | | Signed192 rightX, |
| | | 743 | | Signed192 rightY, |
| | | 744 | | Signed192 rightZ, |
| | | 745 | | out Signed192 x, |
| | | 746 | | out Signed192 y, |
| | | 747 | | out Signed192 z) |
| | | 748 | | { |
| | 104 | 749 | | x = Signed192.NarrowValue(WideArithmetic.MultiplySubtract(leftY, rightZ, leftZ, rightY)); |
| | 104 | 750 | | y = Signed192.NarrowValue(WideArithmetic.MultiplySubtract(leftZ, rightX, leftX, rightZ)); |
| | 104 | 751 | | z = Signed192.NarrowValue(WideArithmetic.MultiplySubtract(leftX, rightY, leftY, rightX)); |
| | 104 | 752 | | } |
| | | 753 | | } |