| | | 1 | | //======================================================================= |
| | | 2 | | // WideOrientedBox.TriangleCircleSlabSweep.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 | | /// <content> |
| | | 13 | | /// Computes swept-circle vs. triangle intersection within a bounded slab region, |
| | | 14 | | /// using high-precision wide arithmetic for exact edge and interior overlap tests. |
| | | 15 | | /// </content> |
| | | 16 | | internal static partial class WideOrientedBox |
| | | 17 | | { |
| | | 18 | | #region Nested Types |
| | | 19 | | |
| | | 20 | | private readonly struct TriangleSweepVertex |
| | | 21 | | { |
| | | 22 | | internal readonly Signed320 X; |
| | | 23 | | internal readonly Signed320 Y; |
| | | 24 | | internal readonly Signed320 Z; |
| | | 25 | | internal readonly Signed320 ReferenceYOffset; |
| | | 26 | | internal readonly Vector3d LocalPoint; |
| | | 27 | | |
| | | 28 | | internal TriangleSweepVertex( |
| | | 29 | | Signed320 x, |
| | | 30 | | Signed320 y, |
| | | 31 | | Signed320 z, |
| | | 32 | | Signed320 referenceYOffset, |
| | | 33 | | Vector3d localPoint) |
| | | 34 | | { |
| | 654 | 35 | | X = x; |
| | 654 | 36 | | Y = y; |
| | 654 | 37 | | Z = z; |
| | 654 | 38 | | ReferenceYOffset = referenceYOffset; |
| | 654 | 39 | | LocalPoint = localPoint; |
| | 654 | 40 | | } |
| | | 41 | | |
| | | 42 | | internal TriangleSweepPoint ToSweepPoint( |
| | | 43 | | Signed192 basisDenominator) => |
| | 1844 | 44 | | new( |
| | 1844 | 45 | | new SweepRationalPoint( |
| | 1844 | 46 | | Signed576.ExtendValue(X), |
| | 1844 | 47 | | Signed576.ExtendValue(Z), |
| | 1844 | 48 | | Signed576.ExtendValue( |
| | 1844 | 49 | | Signed320.ExtendValue( |
| | 1844 | 50 | | basisDenominator))), |
| | 1844 | 51 | | ReferenceYOffset, |
| | 1844 | 52 | | LocalPoint); |
| | | 53 | | |
| | | 54 | | } |
| | | 55 | | |
| | | 56 | | private readonly struct TriangleSweepPoint |
| | | 57 | | { |
| | | 58 | | internal readonly SweepRationalPoint RationalPoint; |
| | | 59 | | internal readonly Signed320 ReferenceYOffset; |
| | | 60 | | internal readonly Vector3d LocalPoint; |
| | | 61 | | |
| | | 62 | | internal TriangleSweepPoint( |
| | | 63 | | SweepRationalPoint rationalPoint, |
| | | 64 | | Signed320 referenceYOffset, |
| | | 65 | | Vector3d localPoint) |
| | | 66 | | { |
| | 2180 | 67 | | RationalPoint = rationalPoint; |
| | 2180 | 68 | | ReferenceYOffset = referenceYOffset; |
| | 2180 | 69 | | LocalPoint = localPoint; |
| | 2180 | 70 | | } |
| | | 71 | | } |
| | | 72 | | |
| | | 73 | | #endregion |
| | | 74 | | |
| | | 75 | | internal static bool TryGetFiniteSlabProjectedCircleSweep( |
| | | 76 | | FixedTriangle triangle, |
| | | 77 | | Vector3d triangleOrigin, |
| | | 78 | | FixedQuaternion triangleRotation, |
| | | 79 | | Vector2d circleStart, |
| | | 80 | | Vector2d direction, |
| | | 81 | | Fixed64 maximumDistance, |
| | | 82 | | Fixed64 circleRadius, |
| | | 83 | | Fixed64 slabCenterY, |
| | | 84 | | Fixed64 slabHalfThickness, |
| | | 85 | | out Fixed64 distance, |
| | | 86 | | out FixedPointAnchor triangleContact) |
| | 112 | 87 | | => TryGetFiniteSlabProjectedCircleSweepCore( |
| | 112 | 88 | | triangle, triangleOrigin, triangleRotation, circleStart, new SweepTrajectory(direction), |
| | 112 | 89 | | maximumDistance, circleRadius, slabCenterY, slabHalfThickness, out distance, out triangleContact); |
| | | 90 | | |
| | | 91 | | internal static bool TryGetFiniteSlabProjectedCircleSweep( |
| | | 92 | | FixedTriangle triangle, |
| | | 93 | | Vector3d triangleOrigin, |
| | | 94 | | FixedQuaternion triangleRotation, |
| | | 95 | | FixedSegment2d segment, |
| | | 96 | | Fixed64 totalDistance, |
| | | 97 | | Fixed64 circleRadius, |
| | | 98 | | Fixed64 slabCenterY, |
| | | 99 | | Fixed64 slabHalfThickness, |
| | | 100 | | out Fixed64 distance, |
| | | 101 | | out FixedPointAnchor triangleContact) |
| | 106 | 102 | | => TryGetFiniteSlabProjectedCircleSweepCore( |
| | 106 | 103 | | triangle, triangleOrigin, triangleRotation, segment.Start, new SweepTrajectory(segment, totalDistance), |
| | 106 | 104 | | totalDistance, circleRadius, slabCenterY, slabHalfThickness, out distance, out triangleContact); |
| | | 105 | | |
| | | 106 | | private static bool TryGetFiniteSlabProjectedCircleSweepCore( |
| | | 107 | | FixedTriangle triangle, |
| | | 108 | | Vector3d triangleOrigin, |
| | | 109 | | FixedQuaternion triangleRotation, |
| | | 110 | | Vector2d circleStart, |
| | | 111 | | in SweepTrajectory trajectory, |
| | | 112 | | Fixed64 maximumDistance, |
| | | 113 | | Fixed64 circleRadius, |
| | | 114 | | Fixed64 slabCenterY, |
| | | 115 | | Fixed64 slabHalfThickness, |
| | | 116 | | out Fixed64 distance, |
| | | 117 | | out FixedPointAnchor triangleContact) |
| | | 118 | | { |
| | 218 | 119 | | WideRationalBasis3d basis = new(triangleRotation); |
| | 218 | 120 | | Signed320 referenceYNumerator = |
| | 218 | 121 | | WideArithmetic.MultiplySigned192( |
| | 218 | 122 | | Signed192.Raw(slabCenterY), |
| | 218 | 123 | | basis.Denominator); |
| | 218 | 124 | | TriangleSweepVertex first = GetTriangleSweepVertex( |
| | 218 | 125 | | triangleOrigin, |
| | 218 | 126 | | basis, |
| | 218 | 127 | | triangle.A, |
| | 218 | 128 | | referenceYNumerator); |
| | 218 | 129 | | TriangleSweepVertex second = GetTriangleSweepVertex( |
| | 218 | 130 | | triangleOrigin, |
| | 218 | 131 | | basis, |
| | 218 | 132 | | triangle.B, |
| | 218 | 133 | | referenceYNumerator); |
| | 218 | 134 | | TriangleSweepVertex third = GetTriangleSweepVertex( |
| | 218 | 135 | | triangleOrigin, |
| | 218 | 136 | | basis, |
| | 218 | 137 | | triangle.C, |
| | 218 | 138 | | referenceYNumerator); |
| | 218 | 139 | | Signed192 lowerY = WideArithmetic.SubtractSigned192( |
| | 218 | 140 | | Signed192.Raw(slabCenterY), |
| | 218 | 141 | | Signed192.Raw(slabHalfThickness)); |
| | 218 | 142 | | Signed192 upperY = WideArithmetic.AddSigned192( |
| | 218 | 143 | | Signed192.Raw(slabCenterY), |
| | 218 | 144 | | Signed192.Raw(slabHalfThickness)); |
| | 218 | 145 | | Signed320 lowerNumerator = |
| | 218 | 146 | | WideArithmetic.MultiplySigned192(lowerY, basis.Denominator); |
| | 218 | 147 | | Signed320 upperNumerator = |
| | 218 | 148 | | WideArithmetic.MultiplySigned192(upperY, basis.Denominator); |
| | 218 | 149 | | if (TryGetTriangleProjectionInteriorPoint( |
| | 218 | 150 | | first, |
| | 218 | 151 | | second, |
| | 218 | 152 | | third, |
| | 218 | 153 | | circleStart, |
| | 218 | 154 | | lowerNumerator, |
| | 218 | 155 | | upperNumerator, |
| | 218 | 156 | | basis.Denominator, |
| | 218 | 157 | | out Vector3d interiorLocalPoint)) |
| | | 158 | | { |
| | 3 | 159 | | distance = Fixed64.Zero; |
| | 3 | 160 | | triangleContact = new FixedPointAnchor( |
| | 3 | 161 | | triangleOrigin, |
| | 3 | 162 | | triangleRotation, |
| | 3 | 163 | | interiorLocalPoint); |
| | 3 | 164 | | return true; |
| | | 165 | | } |
| | | 166 | | |
| | 215 | 167 | | bool found = false; |
| | 215 | 168 | | Fixed64 bestDistance = default; |
| | 215 | 169 | | Vector3d bestLocalPoint = default; |
| | 215 | 170 | | Signed576 bestReferenceYOffset = default; |
| | 215 | 171 | | bool firstSecondCollapsed = |
| | 215 | 172 | | AreSameProjectedPoint(first, second); |
| | 215 | 173 | | bool secondThirdCollapsed = |
| | 215 | 174 | | AreSameProjectedPoint(second, third); |
| | 215 | 175 | | bool thirdFirstCollapsed = |
| | 215 | 176 | | AreSameProjectedPoint(third, first); |
| | 215 | 177 | | bool allCollapsed = |
| | 215 | 178 | | firstSecondCollapsed |
| | 215 | 179 | | && secondThirdCollapsed |
| | 215 | 180 | | && thirdFirstCollapsed; |
| | 215 | 181 | | if (allCollapsed || !firstSecondCollapsed) |
| | | 182 | | { |
| | 192 | 183 | | KeepTriangleEdgeSweep( |
| | 192 | 184 | | first, |
| | 192 | 185 | | second, |
| | 192 | 186 | | lowerNumerator, |
| | 192 | 187 | | upperNumerator, |
| | 192 | 188 | | basis.Denominator, |
| | 192 | 189 | | referenceYNumerator, |
| | 192 | 190 | | circleStart, |
| | 192 | 191 | | trajectory, |
| | 192 | 192 | | maximumDistance, |
| | 192 | 193 | | circleRadius, |
| | 192 | 194 | | ref found, |
| | 192 | 195 | | ref bestDistance, |
| | 192 | 196 | | ref bestLocalPoint, |
| | 192 | 197 | | ref bestReferenceYOffset); |
| | | 198 | | } |
| | 215 | 199 | | if (allCollapsed || !secondThirdCollapsed) |
| | | 200 | | { |
| | 210 | 201 | | KeepTriangleEdgeSweep( |
| | 210 | 202 | | second, |
| | 210 | 203 | | third, |
| | 210 | 204 | | lowerNumerator, |
| | 210 | 205 | | upperNumerator, |
| | 210 | 206 | | basis.Denominator, |
| | 210 | 207 | | referenceYNumerator, |
| | 210 | 208 | | circleStart, |
| | 210 | 209 | | trajectory, |
| | 210 | 210 | | maximumDistance, |
| | 210 | 211 | | circleRadius, |
| | 210 | 212 | | ref found, |
| | 210 | 213 | | ref bestDistance, |
| | 210 | 214 | | ref bestLocalPoint, |
| | 210 | 215 | | ref bestReferenceYOffset); |
| | | 216 | | } |
| | 215 | 217 | | if (allCollapsed || !thirdFirstCollapsed) |
| | | 218 | | { |
| | 211 | 219 | | KeepTriangleEdgeSweep( |
| | 211 | 220 | | third, |
| | 211 | 221 | | first, |
| | 211 | 222 | | lowerNumerator, |
| | 211 | 223 | | upperNumerator, |
| | 211 | 224 | | basis.Denominator, |
| | 211 | 225 | | referenceYNumerator, |
| | 211 | 226 | | circleStart, |
| | 211 | 227 | | trajectory, |
| | 211 | 228 | | maximumDistance, |
| | 211 | 229 | | circleRadius, |
| | 211 | 230 | | ref found, |
| | 211 | 231 | | ref bestDistance, |
| | 211 | 232 | | ref bestLocalPoint, |
| | 211 | 233 | | ref bestReferenceYOffset); |
| | | 234 | | } |
| | 215 | 235 | | KeepTrianglePlaneEdgeSweep( |
| | 215 | 236 | | first, |
| | 215 | 237 | | second, |
| | 215 | 238 | | third, |
| | 215 | 239 | | lowerNumerator, |
| | 215 | 240 | | basis.Denominator, |
| | 215 | 241 | | referenceYNumerator, |
| | 215 | 242 | | circleStart, |
| | 215 | 243 | | trajectory, |
| | 215 | 244 | | maximumDistance, |
| | 215 | 245 | | circleRadius, |
| | 215 | 246 | | ref found, |
| | 215 | 247 | | ref bestDistance, |
| | 215 | 248 | | ref bestLocalPoint, |
| | 215 | 249 | | ref bestReferenceYOffset); |
| | 215 | 250 | | if (CompareSigned(lowerNumerator, upperNumerator) != 0) |
| | | 251 | | { |
| | 68 | 252 | | KeepTrianglePlaneEdgeSweep( |
| | 68 | 253 | | first, |
| | 68 | 254 | | second, |
| | 68 | 255 | | third, |
| | 68 | 256 | | upperNumerator, |
| | 68 | 257 | | basis.Denominator, |
| | 68 | 258 | | referenceYNumerator, |
| | 68 | 259 | | circleStart, |
| | 68 | 260 | | trajectory, |
| | 68 | 261 | | maximumDistance, |
| | 68 | 262 | | circleRadius, |
| | 68 | 263 | | ref found, |
| | 68 | 264 | | ref bestDistance, |
| | 68 | 265 | | ref bestLocalPoint, |
| | 68 | 266 | | ref bestReferenceYOffset); |
| | | 267 | | } |
| | | 268 | | |
| | 215 | 269 | | if (!found) |
| | | 270 | | { |
| | 17 | 271 | | distance = default; |
| | 17 | 272 | | triangleContact = default; |
| | 17 | 273 | | return false; |
| | | 274 | | } |
| | | 275 | | |
| | 198 | 276 | | distance = bestDistance; |
| | 198 | 277 | | triangleContact = new FixedPointAnchor( |
| | 198 | 278 | | triangleOrigin, |
| | 198 | 279 | | triangleRotation, |
| | 198 | 280 | | bestLocalPoint); |
| | 198 | 281 | | return true; |
| | | 282 | | } |
| | | 283 | | |
| | | 284 | | private static bool TryGetTriangleProjectionInteriorPoint( |
| | | 285 | | TriangleSweepVertex first, |
| | | 286 | | TriangleSweepVertex second, |
| | | 287 | | TriangleSweepVertex third, |
| | | 288 | | Vector2d point, |
| | | 289 | | Signed320 lowerY, |
| | | 290 | | Signed320 upperY, |
| | | 291 | | Signed192 basisDenominator, |
| | | 292 | | out Vector3d localPoint) |
| | | 293 | | { |
| | 218 | 294 | | Signed320 firstEdgeX = |
| | 218 | 295 | | WideArithmetic.SubtractSigned320(second.X, first.X); |
| | 218 | 296 | | Signed320 firstEdgeZ = |
| | 218 | 297 | | WideArithmetic.SubtractSigned320(second.Z, first.Z); |
| | 218 | 298 | | Signed320 secondEdgeX = |
| | 218 | 299 | | WideArithmetic.SubtractSigned320(third.X, first.X); |
| | 218 | 300 | | Signed320 secondEdgeZ = |
| | 218 | 301 | | WideArithmetic.SubtractSigned320(third.Z, first.Z); |
| | 218 | 302 | | Signed576 determinant = WideArithmetic.SubtractSigned576( |
| | 218 | 303 | | WideArithmetic.MultiplySigned320( |
| | 218 | 304 | | firstEdgeX, |
| | 218 | 305 | | secondEdgeZ), |
| | 218 | 306 | | WideArithmetic.MultiplySigned320( |
| | 218 | 307 | | firstEdgeZ, |
| | 218 | 308 | | secondEdgeX)); |
| | 218 | 309 | | if (determinant.IsZero) |
| | | 310 | | { |
| | 35 | 311 | | localPoint = default; |
| | 35 | 312 | | return false; |
| | | 313 | | } |
| | | 314 | | |
| | 183 | 315 | | Signed320 pointX = WideArithmetic.SubtractSigned320( |
| | 183 | 316 | | WideArithmetic.MultiplySigned192( |
| | 183 | 317 | | Signed192.Raw(point.X), |
| | 183 | 318 | | basisDenominator), |
| | 183 | 319 | | first.X); |
| | 183 | 320 | | Signed320 pointZ = WideArithmetic.SubtractSigned320( |
| | 183 | 321 | | WideArithmetic.MultiplySigned192( |
| | 183 | 322 | | Signed192.Raw(point.Y), |
| | 183 | 323 | | basisDenominator), |
| | 183 | 324 | | first.Z); |
| | 183 | 325 | | Signed576 firstParameter = WideArithmetic.SubtractSigned576( |
| | 183 | 326 | | WideArithmetic.MultiplySigned320( |
| | 183 | 327 | | pointX, |
| | 183 | 328 | | secondEdgeZ), |
| | 183 | 329 | | WideArithmetic.MultiplySigned320( |
| | 183 | 330 | | pointZ, |
| | 183 | 331 | | secondEdgeX)); |
| | 183 | 332 | | Signed576 secondParameter = WideArithmetic.SubtractSigned576( |
| | 183 | 333 | | WideArithmetic.MultiplySigned320( |
| | 183 | 334 | | firstEdgeX, |
| | 183 | 335 | | pointZ), |
| | 183 | 336 | | WideArithmetic.MultiplySigned320( |
| | 183 | 337 | | firstEdgeZ, |
| | 183 | 338 | | pointX)); |
| | 183 | 339 | | if (determinant.Sign < 0) |
| | | 340 | | { |
| | 155 | 341 | | determinant = |
| | 155 | 342 | | WideArithmetic.SubtractSigned576(default, determinant); |
| | 155 | 343 | | firstParameter = |
| | 155 | 344 | | WideArithmetic.SubtractSigned576(default, firstParameter); |
| | 155 | 345 | | secondParameter = |
| | 155 | 346 | | WideArithmetic.SubtractSigned576(default, secondParameter); |
| | | 347 | | } |
| | | 348 | | |
| | 183 | 349 | | if (firstParameter.Sign < 0 |
| | 183 | 350 | | || secondParameter.Sign < 0 |
| | 183 | 351 | | || WideArithmetic.SubtractSigned576( |
| | 183 | 352 | | WideArithmetic.AddSigned576( |
| | 183 | 353 | | firstParameter, |
| | 183 | 354 | | secondParameter), |
| | 183 | 355 | | determinant).Sign > 0) |
| | | 356 | | { |
| | 178 | 357 | | localPoint = default; |
| | 178 | 358 | | return false; |
| | | 359 | | } |
| | | 360 | | |
| | 5 | 361 | | Signed320 firstEdgeY = |
| | 5 | 362 | | WideArithmetic.SubtractSigned320(second.Y, first.Y); |
| | 5 | 363 | | Signed320 secondEdgeY = |
| | 5 | 364 | | WideArithmetic.SubtractSigned320(third.Y, first.Y); |
| | 5 | 365 | | Signed832 yNumerator = WideArithmetic.AddSigned832( |
| | 5 | 366 | | WideArithmetic.AddSigned832( |
| | 5 | 367 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 5 | 368 | | Signed576.ExtendValue(first.Y), |
| | 5 | 369 | | determinant), |
| | 5 | 370 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 5 | 371 | | Signed576.ExtendValue(firstEdgeY), |
| | 5 | 372 | | firstParameter)), |
| | 5 | 373 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 5 | 374 | | Signed576.ExtendValue(secondEdgeY), |
| | 5 | 375 | | secondParameter)); |
| | 5 | 376 | | Signed832 lowerNumerator = |
| | 5 | 377 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 5 | 378 | | Signed576.ExtendValue(lowerY), |
| | 5 | 379 | | determinant); |
| | 5 | 380 | | Signed832 upperNumerator = |
| | 5 | 381 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 5 | 382 | | Signed576.ExtendValue(upperY), |
| | 5 | 383 | | determinant); |
| | 5 | 384 | | if (WideArithmetic.SubtractSigned832( |
| | 5 | 385 | | yNumerator, |
| | 5 | 386 | | lowerNumerator).Sign < 0 |
| | 5 | 387 | | || WideArithmetic.SubtractSigned832( |
| | 5 | 388 | | yNumerator, |
| | 5 | 389 | | upperNumerator).Sign > 0) |
| | | 390 | | { |
| | 2 | 391 | | localPoint = default; |
| | 2 | 392 | | return false; |
| | | 393 | | } |
| | | 394 | | |
| | 3 | 395 | | Signed320 firstLocalX = |
| | 3 | 396 | | Signed320.ExtendValue( |
| | 3 | 397 | | WideArithmetic.SubtractSigned192( |
| | 3 | 398 | | Signed192.Raw(second.LocalPoint.X), |
| | 3 | 399 | | Signed192.Raw(first.LocalPoint.X))); |
| | 3 | 400 | | Signed320 firstLocalY = |
| | 3 | 401 | | Signed320.ExtendValue( |
| | 3 | 402 | | WideArithmetic.SubtractSigned192( |
| | 3 | 403 | | Signed192.Raw(second.LocalPoint.Y), |
| | 3 | 404 | | Signed192.Raw(first.LocalPoint.Y))); |
| | 3 | 405 | | Signed320 firstLocalZ = |
| | 3 | 406 | | Signed320.ExtendValue( |
| | 3 | 407 | | WideArithmetic.SubtractSigned192( |
| | 3 | 408 | | Signed192.Raw(second.LocalPoint.Z), |
| | 3 | 409 | | Signed192.Raw(first.LocalPoint.Z))); |
| | 3 | 410 | | Signed320 secondLocalX = |
| | 3 | 411 | | Signed320.ExtendValue( |
| | 3 | 412 | | WideArithmetic.SubtractSigned192( |
| | 3 | 413 | | Signed192.Raw(third.LocalPoint.X), |
| | 3 | 414 | | Signed192.Raw(first.LocalPoint.X))); |
| | 3 | 415 | | Signed320 secondLocalY = |
| | 3 | 416 | | Signed320.ExtendValue( |
| | 3 | 417 | | WideArithmetic.SubtractSigned192( |
| | 3 | 418 | | Signed192.Raw(third.LocalPoint.Y), |
| | 3 | 419 | | Signed192.Raw(first.LocalPoint.Y))); |
| | 3 | 420 | | Signed320 secondLocalZ = |
| | 3 | 421 | | Signed320.ExtendValue( |
| | 3 | 422 | | WideArithmetic.SubtractSigned192( |
| | 3 | 423 | | Signed192.Raw(third.LocalPoint.Z), |
| | 3 | 424 | | Signed192.Raw(first.LocalPoint.Z))); |
| | 3 | 425 | | Signed832 denominator = |
| | 3 | 426 | | Signed832.ExtendValue(determinant); |
| | | 427 | | // A barycentric point lies inside the convex hull of three |
| | | 428 | | // representable local vertices, so every final local component is |
| | | 429 | | // representable by construction. |
| | 3 | 430 | | _ = TryGetTriangleInteriorLocalCoordinate( |
| | 3 | 431 | | first.LocalPoint.X, |
| | 3 | 432 | | firstLocalX, |
| | 3 | 433 | | secondLocalX, |
| | 3 | 434 | | firstParameter, |
| | 3 | 435 | | secondParameter, |
| | 3 | 436 | | determinant, |
| | 3 | 437 | | denominator, |
| | 3 | 438 | | out Fixed64 localX); |
| | 3 | 439 | | _ = TryGetTriangleInteriorLocalCoordinate( |
| | 3 | 440 | | first.LocalPoint.Y, |
| | 3 | 441 | | firstLocalY, |
| | 3 | 442 | | secondLocalY, |
| | 3 | 443 | | firstParameter, |
| | 3 | 444 | | secondParameter, |
| | 3 | 445 | | determinant, |
| | 3 | 446 | | denominator, |
| | 3 | 447 | | out Fixed64 localY); |
| | 3 | 448 | | _ = TryGetTriangleInteriorLocalCoordinate( |
| | 3 | 449 | | first.LocalPoint.Z, |
| | 3 | 450 | | firstLocalZ, |
| | 3 | 451 | | secondLocalZ, |
| | 3 | 452 | | firstParameter, |
| | 3 | 453 | | secondParameter, |
| | 3 | 454 | | determinant, |
| | 3 | 455 | | denominator, |
| | 3 | 456 | | out Fixed64 localZ); |
| | 3 | 457 | | localPoint = new Vector3d(localX, localY, localZ); |
| | 3 | 458 | | return true; |
| | | 459 | | } |
| | | 460 | | |
| | | 461 | | private static bool TryGetTriangleInteriorLocalCoordinate( |
| | | 462 | | Fixed64 origin, |
| | | 463 | | Signed320 firstEdge, |
| | | 464 | | Signed320 secondEdge, |
| | | 465 | | Signed576 firstParameter, |
| | | 466 | | Signed576 secondParameter, |
| | | 467 | | Signed576 determinant, |
| | | 468 | | Signed832 denominator, |
| | | 469 | | out Fixed64 coordinate) |
| | | 470 | | { |
| | 9 | 471 | | Signed832 numerator = WideArithmetic.AddSigned832( |
| | 9 | 472 | | WideArithmetic.AddSigned832( |
| | 9 | 473 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 9 | 474 | | Signed576.ExtendValue( |
| | 9 | 475 | | Signed320.ExtendValue(Signed192.Raw(origin))), |
| | 9 | 476 | | determinant), |
| | 9 | 477 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 9 | 478 | | Signed576.ExtendValue(firstEdge), |
| | 9 | 479 | | firstParameter)), |
| | 9 | 480 | | WideArithmetic.MultiplySigned576ToSigned832( |
| | 9 | 481 | | Signed576.ExtendValue(secondEdge), |
| | 9 | 482 | | secondParameter)); |
| | 9 | 483 | | return Fixed64.TryGetSignedRawRatio( |
| | 9 | 484 | | numerator, |
| | 9 | 485 | | denominator, |
| | 9 | 486 | | numeratorLeftShift: 0, |
| | 9 | 487 | | out coordinate); |
| | | 488 | | } |
| | | 489 | | |
| | | 490 | | private static TriangleSweepVertex GetTriangleSweepVertex( |
| | | 491 | | Vector3d origin, |
| | | 492 | | WideRationalBasis3d basis, |
| | | 493 | | Vector3d localPoint, |
| | | 494 | | Signed320 referenceYNumerator) |
| | | 495 | | { |
| | 654 | 496 | | Signed320 x = WideArithmetic.AddSigned320( |
| | 654 | 497 | | WideArithmetic.MultiplySigned192( |
| | 654 | 498 | | Signed192.Raw(origin.X), |
| | 654 | 499 | | basis.Denominator), |
| | 654 | 500 | | GetLocalOffsetNumerator( |
| | 654 | 501 | | basis.Xx, |
| | 654 | 502 | | basis.Yx, |
| | 654 | 503 | | basis.Zx, |
| | 654 | 504 | | localPoint)); |
| | 654 | 505 | | Signed320 y = WideArithmetic.AddSigned320( |
| | 654 | 506 | | WideArithmetic.MultiplySigned192( |
| | 654 | 507 | | Signed192.Raw(origin.Y), |
| | 654 | 508 | | basis.Denominator), |
| | 654 | 509 | | GetLocalOffsetNumerator( |
| | 654 | 510 | | basis.Xy, |
| | 654 | 511 | | basis.Yy, |
| | 654 | 512 | | basis.Zy, |
| | 654 | 513 | | localPoint)); |
| | 654 | 514 | | Signed320 z = WideArithmetic.AddSigned320( |
| | 654 | 515 | | WideArithmetic.MultiplySigned192( |
| | 654 | 516 | | Signed192.Raw(origin.Z), |
| | 654 | 517 | | basis.Denominator), |
| | 654 | 518 | | GetLocalOffsetNumerator( |
| | 654 | 519 | | basis.Xz, |
| | 654 | 520 | | basis.Yz, |
| | 654 | 521 | | basis.Zz, |
| | 654 | 522 | | localPoint)); |
| | 654 | 523 | | return new TriangleSweepVertex( |
| | 654 | 524 | | x, |
| | 654 | 525 | | y, |
| | 654 | 526 | | z, |
| | 654 | 527 | | WideArithmetic.SubtractSigned320( |
| | 654 | 528 | | y, |
| | 654 | 529 | | referenceYNumerator), |
| | 654 | 530 | | localPoint); |
| | | 531 | | } |
| | | 532 | | |
| | | 533 | | private static bool AreSameProjectedPoint( |
| | | 534 | | TriangleSweepVertex first, |
| | | 535 | | TriangleSweepVertex second) => |
| | 645 | 536 | | CompareSigned(first.X, second.X) == 0 |
| | 645 | 537 | | && CompareSigned(first.Z, second.Z) == 0; |
| | | 538 | | |
| | | 539 | | private static void KeepTriangleEdgeSweep( |
| | | 540 | | TriangleSweepVertex first, |
| | | 541 | | TriangleSweepVertex second, |
| | | 542 | | Signed320 lowerY, |
| | | 543 | | Signed320 upperY, |
| | | 544 | | Signed192 basisDenominator, |
| | | 545 | | Signed320 referenceYNumerator, |
| | | 546 | | Vector2d circleStart, |
| | | 547 | | in SweepTrajectory trajectory, |
| | | 548 | | Fixed64 maximumDistance, |
| | | 549 | | Fixed64 radius, |
| | | 550 | | ref bool found, |
| | | 551 | | ref Fixed64 bestDistance, |
| | | 552 | | ref Vector3d bestLocalPoint, |
| | | 553 | | ref Signed576 bestReferenceYOffset) |
| | | 554 | | { |
| | 613 | 555 | | Signed320 minimum = CompareSigned(first.Y, second.Y) <= 0 |
| | 613 | 556 | | ? first.Y |
| | 613 | 557 | | : second.Y; |
| | 613 | 558 | | Signed320 maximum = CompareSigned(first.Y, second.Y) <= 0 |
| | 613 | 559 | | ? second.Y |
| | 613 | 560 | | : first.Y; |
| | 613 | 561 | | if (CompareSigned(maximum, lowerY) < 0 |
| | 613 | 562 | | || CompareSigned(minimum, upperY) > 0) |
| | | 563 | | { |
| | 57 | 564 | | return; |
| | | 565 | | } |
| | | 566 | | |
| | 556 | 567 | | TriangleSweepPoint clippedFirst = |
| | 556 | 568 | | GetClippedTriangleEdgeEndpoint( |
| | 556 | 569 | | first, |
| | 556 | 570 | | second, |
| | 556 | 571 | | lowerY, |
| | 556 | 572 | | upperY, |
| | 556 | 573 | | basisDenominator, |
| | 556 | 574 | | referenceYNumerator); |
| | 556 | 575 | | TriangleSweepPoint clippedSecond = |
| | 556 | 576 | | GetClippedTriangleEdgeEndpoint( |
| | 556 | 577 | | second, |
| | 556 | 578 | | first, |
| | 556 | 579 | | lowerY, |
| | 556 | 580 | | upperY, |
| | 556 | 581 | | basisDenominator, |
| | 556 | 582 | | referenceYNumerator); |
| | 556 | 583 | | KeepTriangleSweepFeature( |
| | 556 | 584 | | clippedFirst, |
| | 556 | 585 | | clippedSecond, |
| | 556 | 586 | | circleStart, |
| | 556 | 587 | | trajectory, |
| | 556 | 588 | | maximumDistance, |
| | 556 | 589 | | radius, |
| | 556 | 590 | | ref found, |
| | 556 | 591 | | ref bestDistance, |
| | 556 | 592 | | ref bestLocalPoint, |
| | 556 | 593 | | ref bestReferenceYOffset); |
| | 556 | 594 | | } |
| | | 595 | | |
| | | 596 | | private static TriangleSweepPoint GetClippedTriangleEdgeEndpoint( |
| | | 597 | | TriangleSweepVertex endpoint, |
| | | 598 | | TriangleSweepVertex other, |
| | | 599 | | Signed320 lowerY, |
| | | 600 | | Signed320 upperY, |
| | | 601 | | Signed192 basisDenominator, |
| | | 602 | | Signed320 referenceYNumerator) |
| | | 603 | | { |
| | 1112 | 604 | | if (CompareSigned(endpoint.Y, lowerY) >= 0 |
| | 1112 | 605 | | && CompareSigned(endpoint.Y, upperY) <= 0) |
| | | 606 | | { |
| | 960 | 607 | | return endpoint.ToSweepPoint(basisDenominator); |
| | | 608 | | } |
| | | 609 | | |
| | 152 | 610 | | Signed320 plane = CompareSigned(endpoint.Y, lowerY) < 0 |
| | 152 | 611 | | ? lowerY |
| | 152 | 612 | | : upperY; |
| | 152 | 613 | | return GetTrianglePlaneIntersection( |
| | 152 | 614 | | endpoint, |
| | 152 | 615 | | other, |
| | 152 | 616 | | plane, |
| | 152 | 617 | | basisDenominator, |
| | 152 | 618 | | referenceYNumerator); |
| | | 619 | | } |
| | | 620 | | |
| | | 621 | | private static void KeepTrianglePlaneEdgeSweep( |
| | | 622 | | TriangleSweepVertex first, |
| | | 623 | | TriangleSweepVertex second, |
| | | 624 | | TriangleSweepVertex third, |
| | | 625 | | Signed320 planeY, |
| | | 626 | | Signed192 basisDenominator, |
| | | 627 | | Signed320 referenceYNumerator, |
| | | 628 | | Vector2d circleStart, |
| | | 629 | | in SweepTrajectory trajectory, |
| | | 630 | | Fixed64 maximumDistance, |
| | | 631 | | Fixed64 radius, |
| | | 632 | | ref bool found, |
| | | 633 | | ref Fixed64 bestDistance, |
| | | 634 | | ref Vector3d bestLocalPoint, |
| | | 635 | | ref Signed576 bestReferenceYOffset) |
| | | 636 | | { |
| | 283 | 637 | | Span<TriangleSweepPoint> intersections = |
| | 283 | 638 | | stackalloc TriangleSweepPoint[3]; |
| | 283 | 639 | | int count = 0; |
| | 283 | 640 | | TryAddTrianglePlaneIntersection( |
| | 283 | 641 | | first, |
| | 283 | 642 | | second, |
| | 283 | 643 | | planeY, |
| | 283 | 644 | | basisDenominator, |
| | 283 | 645 | | referenceYNumerator, |
| | 283 | 646 | | intersections, |
| | 283 | 647 | | ref count); |
| | 283 | 648 | | TryAddTrianglePlaneIntersection( |
| | 283 | 649 | | second, |
| | 283 | 650 | | third, |
| | 283 | 651 | | planeY, |
| | 283 | 652 | | basisDenominator, |
| | 283 | 653 | | referenceYNumerator, |
| | 283 | 654 | | intersections, |
| | 283 | 655 | | ref count); |
| | 283 | 656 | | TryAddTrianglePlaneIntersection( |
| | 283 | 657 | | third, |
| | 283 | 658 | | first, |
| | 283 | 659 | | planeY, |
| | 283 | 660 | | basisDenominator, |
| | 283 | 661 | | referenceYNumerator, |
| | 283 | 662 | | intersections, |
| | 283 | 663 | | ref count); |
| | 283 | 664 | | if (count < 2) |
| | 49 | 665 | | return; |
| | | 666 | | |
| | 234 | 667 | | KeepTriangleSweepFeature( |
| | 234 | 668 | | intersections[0], |
| | 234 | 669 | | intersections[1], |
| | 234 | 670 | | circleStart, |
| | 234 | 671 | | trajectory, |
| | 234 | 672 | | maximumDistance, |
| | 234 | 673 | | radius, |
| | 234 | 674 | | ref found, |
| | 234 | 675 | | ref bestDistance, |
| | 234 | 676 | | ref bestLocalPoint, |
| | 234 | 677 | | ref bestReferenceYOffset); |
| | 234 | 678 | | } |
| | | 679 | | |
| | | 680 | | private static void TryAddTrianglePlaneIntersection( |
| | | 681 | | TriangleSweepVertex first, |
| | | 682 | | TriangleSweepVertex second, |
| | | 683 | | Signed320 planeY, |
| | | 684 | | Signed192 basisDenominator, |
| | | 685 | | Signed320 referenceYNumerator, |
| | | 686 | | Span<TriangleSweepPoint> intersections, |
| | | 687 | | ref int count) |
| | | 688 | | { |
| | 849 | 689 | | int firstSide = CompareSigned(first.Y, planeY); |
| | 849 | 690 | | int secondSide = CompareSigned(second.Y, planeY); |
| | 849 | 691 | | if (firstSide == 0) |
| | | 692 | | { |
| | 442 | 693 | | AddUniqueTriangleSweepPoint( |
| | 442 | 694 | | first.ToSweepPoint(basisDenominator), |
| | 442 | 695 | | intersections, |
| | 442 | 696 | | ref count); |
| | | 697 | | } |
| | 849 | 698 | | if (secondSide == 0) |
| | | 699 | | { |
| | 442 | 700 | | AddUniqueTriangleSweepPoint( |
| | 442 | 701 | | second.ToSweepPoint(basisDenominator), |
| | 442 | 702 | | intersections, |
| | 442 | 703 | | ref count); |
| | | 704 | | } |
| | 849 | 705 | | if (firstSide == 0 |
| | 849 | 706 | | || secondSide == 0 |
| | 849 | 707 | | || (firstSide < 0) == (secondSide < 0)) |
| | | 708 | | { |
| | 665 | 709 | | return; |
| | | 710 | | } |
| | | 711 | | |
| | 184 | 712 | | AddUniqueTriangleSweepPoint( |
| | 184 | 713 | | GetTrianglePlaneIntersection( |
| | 184 | 714 | | first, |
| | 184 | 715 | | second, |
| | 184 | 716 | | planeY, |
| | 184 | 717 | | basisDenominator, |
| | 184 | 718 | | referenceYNumerator), |
| | 184 | 719 | | intersections, |
| | 184 | 720 | | ref count); |
| | 184 | 721 | | } |
| | | 722 | | |
| | | 723 | | private static TriangleSweepPoint GetTrianglePlaneIntersection( |
| | | 724 | | TriangleSweepVertex first, |
| | | 725 | | TriangleSweepVertex second, |
| | | 726 | | Signed320 planeY, |
| | | 727 | | Signed192 basisDenominator, |
| | | 728 | | Signed320 referenceYNumerator) |
| | | 729 | | { |
| | 336 | 730 | | Signed320 deltaY = |
| | 336 | 731 | | WideArithmetic.SubtractSigned320(second.Y, first.Y); |
| | 336 | 732 | | Signed320 firstWeight = |
| | 336 | 733 | | WideArithmetic.SubtractSigned320(second.Y, planeY); |
| | 336 | 734 | | Signed320 secondWeight = |
| | 336 | 735 | | WideArithmetic.SubtractSigned320(planeY, first.Y); |
| | 336 | 736 | | Signed576 x = WideArithmetic.AddSigned576( |
| | 336 | 737 | | WideArithmetic.MultiplySigned320(first.X, firstWeight), |
| | 336 | 738 | | WideArithmetic.MultiplySigned320(second.X, secondWeight)); |
| | 336 | 739 | | Signed576 z = WideArithmetic.AddSigned576( |
| | 336 | 740 | | WideArithmetic.MultiplySigned320(first.Z, firstWeight), |
| | 336 | 741 | | WideArithmetic.MultiplySigned320(second.Z, secondWeight)); |
| | 336 | 742 | | Signed576 denominator = WideArithmetic.MultiplySigned320( |
| | 336 | 743 | | Signed320.ExtendValue(basisDenominator), |
| | 336 | 744 | | deltaY); |
| | 336 | 745 | | if (denominator.Sign < 0) |
| | | 746 | | { |
| | 169 | 747 | | x = WideArithmetic.SubtractSigned576(default, x); |
| | 169 | 748 | | z = WideArithmetic.SubtractSigned576(default, z); |
| | 169 | 749 | | denominator = |
| | 169 | 750 | | WideArithmetic.SubtractSigned576(default, denominator); |
| | 169 | 751 | | firstWeight = |
| | 169 | 752 | | WideArithmetic.SubtractSigned320(default, firstWeight); |
| | 169 | 753 | | secondWeight = |
| | 169 | 754 | | WideArithmetic.SubtractSigned320(default, secondWeight); |
| | 169 | 755 | | deltaY = WideArithmetic.SubtractSigned320(default, deltaY); |
| | | 756 | | } |
| | | 757 | | |
| | 336 | 758 | | Vector3d localPoint = GetWeightedLocalPoint( |
| | 336 | 759 | | first.LocalPoint, |
| | 336 | 760 | | second.LocalPoint, |
| | 336 | 761 | | secondWeight, |
| | 336 | 762 | | deltaY); |
| | 336 | 763 | | return new TriangleSweepPoint( |
| | 336 | 764 | | new SweepRationalPoint(x, z, denominator), |
| | 336 | 765 | | WideArithmetic.SubtractSigned320( |
| | 336 | 766 | | planeY, |
| | 336 | 767 | | referenceYNumerator), |
| | 336 | 768 | | localPoint); |
| | | 769 | | } |
| | | 770 | | |
| | | 771 | | private static Vector3d GetWeightedLocalPoint( |
| | | 772 | | Vector3d first, |
| | | 773 | | Vector3d second, |
| | | 774 | | Signed320 secondWeight, |
| | | 775 | | Signed320 denominator) |
| | | 776 | | { |
| | 336 | 777 | | Span<Vector3d> points = stackalloc Vector3d[2] |
| | 336 | 778 | | { |
| | 336 | 779 | | first, |
| | 336 | 780 | | second, |
| | 336 | 781 | | }; |
| | | 782 | | // This ratio is a dimensionless interpolation parameter, so promote |
| | | 783 | | // it to Q32.32 rather than treating the integer quotient as raw. |
| | 336 | 784 | | _ = Fixed64.TryGetSignedRawRatio( |
| | 336 | 785 | | Signed832.ExtendValue( |
| | 336 | 786 | | Signed576.ExtendValue(secondWeight)), |
| | 336 | 787 | | Signed832.ExtendValue( |
| | 336 | 788 | | Signed576.ExtendValue(denominator)), |
| | 336 | 789 | | FixedMath.SHIFT_AMOUNT_I, |
| | 336 | 790 | | out Fixed64 parameter); |
| | 336 | 791 | | Span<Fixed64> weights = stackalloc Fixed64[2] |
| | 336 | 792 | | { |
| | 336 | 793 | | Fixed64.One - parameter, |
| | 336 | 794 | | parameter, |
| | 336 | 795 | | }; |
| | 336 | 796 | | _ = Vector3d.TryGetWeightedAverage( |
| | 336 | 797 | | points, |
| | 336 | 798 | | weights, |
| | 336 | 799 | | out Vector3d localPoint); |
| | 336 | 800 | | return localPoint; |
| | | 801 | | } |
| | | 802 | | |
| | | 803 | | private static void AddUniqueTriangleSweepPoint( |
| | | 804 | | TriangleSweepPoint point, |
| | | 805 | | Span<TriangleSweepPoint> points, |
| | | 806 | | ref int count) |
| | | 807 | | { |
| | 4024 | 808 | | for (int index = 0; index < count; index++) |
| | | 809 | | { |
| | 1388 | 810 | | if (AreSameSweepPoint( |
| | 1388 | 811 | | points[index].RationalPoint, |
| | 1388 | 812 | | point.RationalPoint)) |
| | | 813 | | { |
| | 444 | 814 | | return; |
| | | 815 | | } |
| | | 816 | | } |
| | | 817 | | |
| | 624 | 818 | | points[count++] = point; |
| | 624 | 819 | | } |
| | | 820 | | |
| | | 821 | | private static void KeepTriangleSweepFeature( |
| | | 822 | | TriangleSweepPoint first, |
| | | 823 | | TriangleSweepPoint second, |
| | | 824 | | Vector2d circleStart, |
| | | 825 | | in SweepTrajectory trajectory, |
| | | 826 | | Fixed64 maximumDistance, |
| | | 827 | | Fixed64 radius, |
| | | 828 | | ref bool found, |
| | | 829 | | ref Fixed64 bestDistance, |
| | | 830 | | ref Vector3d bestLocalPoint, |
| | | 831 | | ref Signed576 bestReferenceYOffset) |
| | | 832 | | { |
| | 790 | 833 | | if (TryGetRationalPointSweepDistance( |
| | 790 | 834 | | first.RationalPoint, |
| | 790 | 835 | | circleStart, |
| | 790 | 836 | | trajectory, |
| | 790 | 837 | | maximumDistance, |
| | 790 | 838 | | radius, |
| | 790 | 839 | | out Fixed64 firstDistance)) |
| | | 840 | | { |
| | 211 | 841 | | KeepTriangleSweepCandidate( |
| | 211 | 842 | | firstDistance, |
| | 211 | 843 | | first.LocalPoint, |
| | 211 | 844 | | ScaleReferenceYOffset(first.ReferenceYOffset), |
| | 211 | 845 | | ref found, |
| | 211 | 846 | | ref bestDistance, |
| | 211 | 847 | | ref bestLocalPoint, |
| | 211 | 848 | | ref bestReferenceYOffset); |
| | | 849 | | } |
| | 790 | 850 | | if (!AreSameSweepPoint( |
| | 790 | 851 | | first.RationalPoint, |
| | 790 | 852 | | second.RationalPoint) |
| | 790 | 853 | | && TryGetRationalPointSweepDistance( |
| | 790 | 854 | | second.RationalPoint, |
| | 790 | 855 | | circleStart, |
| | 790 | 856 | | trajectory, |
| | 790 | 857 | | maximumDistance, |
| | 790 | 858 | | radius, |
| | 790 | 859 | | out Fixed64 secondDistance)) |
| | | 860 | | { |
| | 175 | 861 | | KeepTriangleSweepCandidate( |
| | 175 | 862 | | secondDistance, |
| | 175 | 863 | | second.LocalPoint, |
| | 175 | 864 | | ScaleReferenceYOffset(second.ReferenceYOffset), |
| | 175 | 865 | | ref found, |
| | 175 | 866 | | ref bestDistance, |
| | 175 | 867 | | ref bestLocalPoint, |
| | 175 | 868 | | ref bestReferenceYOffset); |
| | | 869 | | } |
| | 790 | 870 | | if (!TryGetRationalSegmentSweepDistance( |
| | 790 | 871 | | first, |
| | 790 | 872 | | second, |
| | 790 | 873 | | circleStart, |
| | 790 | 874 | | trajectory, |
| | 790 | 875 | | maximumDistance, |
| | 790 | 876 | | radius, |
| | 790 | 877 | | out Fixed64 segmentDistance, |
| | 790 | 878 | | out Vector3d segmentLocalPoint, |
| | 790 | 879 | | out Signed576 segmentReferenceYOffset)) |
| | | 880 | | { |
| | 182 | 881 | | return; |
| | | 882 | | } |
| | | 883 | | |
| | 608 | 884 | | KeepTriangleSweepCandidate( |
| | 608 | 885 | | segmentDistance, |
| | 608 | 886 | | segmentLocalPoint, |
| | 608 | 887 | | segmentReferenceYOffset, |
| | 608 | 888 | | ref found, |
| | 608 | 889 | | ref bestDistance, |
| | 608 | 890 | | ref bestLocalPoint, |
| | 608 | 891 | | ref bestReferenceYOffset); |
| | 608 | 892 | | } |
| | | 893 | | |
| | | 894 | | private static void KeepTriangleSweepCandidate( |
| | | 895 | | Fixed64 candidateDistance, |
| | | 896 | | Vector3d candidateLocalPoint, |
| | | 897 | | Signed576 candidateReferenceYOffset, |
| | | 898 | | ref bool found, |
| | | 899 | | ref Fixed64 bestDistance, |
| | | 900 | | ref Vector3d bestLocalPoint, |
| | | 901 | | ref Signed576 bestReferenceYOffset) |
| | | 902 | | { |
| | 994 | 903 | | if (found) |
| | | 904 | | { |
| | 796 | 905 | | if (candidateDistance > bestDistance) |
| | 565 | 906 | | return; |
| | 231 | 907 | | if ((candidateDistance == bestDistance) |
| | 231 | 908 | | & (WideArithmetic.CompareNonNegative( |
| | 231 | 909 | | GetAbsoluteReferenceYOffset(candidateReferenceYOffset), |
| | 231 | 910 | | GetAbsoluteReferenceYOffset(bestReferenceYOffset)) >= 0)) |
| | | 911 | | { |
| | 218 | 912 | | return; |
| | | 913 | | } |
| | | 914 | | } |
| | | 915 | | |
| | 211 | 916 | | found = true; |
| | 211 | 917 | | bestDistance = candidateDistance; |
| | 211 | 918 | | bestLocalPoint = candidateLocalPoint; |
| | 211 | 919 | | bestReferenceYOffset = candidateReferenceYOffset; |
| | 211 | 920 | | } |
| | | 921 | | |
| | | 922 | | private static Signed576 ScaleReferenceYOffset( |
| | | 923 | | Signed320 value) => |
| | 386 | 924 | | WideArithmetic.MultiplySigned320( |
| | 386 | 925 | | value, |
| | 386 | 926 | | Signed320.ExtendValue( |
| | 386 | 927 | | Signed192.One)); |
| | | 928 | | |
| | | 929 | | private static Signed576 GetAbsoluteReferenceYOffset(Signed576 value) => |
| | 462 | 930 | | value.Sign < 0 |
| | 462 | 931 | | ? WideArithmetic.SubtractSigned576(default, value) |
| | 462 | 932 | | : value; |
| | | 933 | | // Exact affine geometry retains the authored frame, while final witnesses |
| | | 934 | | // use a caller-supplied nearby origin rather than a potentially distant host. |
| | | 935 | | internal static bool TryGetFiniteSlabProjectedCircleSweep(in FixedTriangle triangle, |
| | | 936 | | in ScaledRigidFrame3d frame, Vector3d materializationOrigin, in FixedSegment2d segment, Fixed64 length, Fixed64 |
| | | 937 | | Fixed64 slabCenterY, Fixed64 halfThickness, out Fixed64 distance, out FixedPointAnchor contact) |
| | | 938 | | { |
| | 40 | 939 | | if (frame.IsRigid) |
| | 6 | 940 | | return TryGetFiniteSlabProjectedCircleSweep(triangle, frame.RigidOrigin, frame.Rotation, |
| | 6 | 941 | | segment, length, radius, slabCenterY, halfThickness, out distance, out contact); |
| | 34 | 942 | | Vector3d origin = new(segment.Start.X, slabCenterY, segment.Start.Y); |
| | 34 | 943 | | Span<WideDirection3d> points = stackalloc WideDirection3d[3]; |
| | 34 | 944 | | Signed320 denominator = frame.Denominator; |
| | 272 | 945 | | for (int i = 0; i < 3; i++) |
| | | 946 | | { |
| | 102 | 947 | | frame.GetPoint(i == 0 ? triangle.A : i == 1 ? triangle.B : triangle.C) |
| | 102 | 948 | | .GetLocalPointNumerators(origin, FixedQuaternion.Identity, |
| | 102 | 949 | | out Signed576 x, out Signed576 y, out Signed576 z, out denominator); |
| | 102 | 950 | | points[i] = new(x, y, z); |
| | | 951 | | } |
| | | 952 | | // All point coordinates share one denominator. Remove only their |
| | | 953 | | // exact common integer factor before forming quadratic products. |
| | 34 | 954 | | Span<Signed576> reduced = stackalloc Signed576[10]; |
| | 578 | 955 | | for (int i = 0; i < 3; i++) { reduced[3 * i] = points[i].X; reduced[3 * i + 1] = points[i].Y; reduced[3 * i + 2] |
| | 68 | 956 | | reduced[9] = Signed576.ExtendValue(denominator); WideArithmetic.ReduceCommonScale(reduced); |
| | 34 | 957 | | denominator = Signed320.NarrowValue(reduced[9]); |
| | 374 | 958 | | for (int i = 0; i < 3; i++) points[i] = new(reduced[3 * i], reduced[3 * i + 1], reduced[3 * i + 2]); |
| | 34 | 959 | | if (TryGetAffineSlabSweepInterior(points, denominator, halfThickness, origin, |
| | 50 | 960 | | materializationOrigin, out contact)) { distance = Fixed64.Zero; return true; } |
| | 26 | 961 | | Signed192 dx = WideArithmetic.Difference(segment.End.X, segment.Start.X), |
| | 26 | 962 | | dz = WideArithmetic.Difference(segment.End.Y, segment.Start.Y); |
| | 29 | 963 | | if (length == Fixed64.Zero) dx = dz = default; |
| | | 964 | | // Admitted local vertices/placements retain <262 coordinate bits and |
| | | 965 | | // <400-bit wall coefficients. Clipped point coefficients are <525 bits; |
| | | 966 | | // edge/face roots and finite witnesses remain <2700 coefficient bits. |
| | | 967 | | // Keep these banks only on the affine path. The ordinary owner retains |
| | | 968 | | // its original fixed-width points, trajectory and compact root banks. |
| | | 969 | | const int words = 64; |
| | 52 | 970 | | Span<ulong> values = stackalloc ulong[14 * words]; Span<int> signs = stackalloc int[14]; |
| | 26 | 971 | | Span<ulong> bestRoot = stackalloc ulong[2 * words]; |
| | 26 | 972 | | bool found = false; |
| | 26 | 973 | | WideDirection3d localNormal = WideTriangleRelations.GetScaledTriangleLocalNormal(triangle, frame); |
| | 26 | 974 | | WideDirection3d normal = ReduceAffineSlabDirection(localNormal.Transform(frame.GeometryBasis)); |
| | 26 | 975 | | Span<WideDirection3d> walls = stackalloc WideDirection3d[3]; |
| | 208 | 976 | | for (int i = 0; i < 3; i++) |
| | | 977 | | { |
| | 78 | 978 | | FixedSegment edge = triangle.GetEdge(i); |
| | 78 | 979 | | walls[i] = ReduceAffineSlabDirection(WideDirection3d.Cross(localNormal, |
| | 78 | 980 | | WideDirection3d.GetLocalEdge(frame, edge.Start, edge.End)).Transform(frame.GeometryBasis)); |
| | | 981 | | } |
| | 754 | 982 | | for (int feature = 0; feature < 14; feature++) |
| | | 983 | | { |
| | 352 | 984 | | KeepAffineSlabSweepFeature(points, denominator, normal, walls, dx, dz, radius, |
| | 352 | 985 | | halfThickness, feature, values, signs, bestRoot, ref found); |
| | | 986 | | // Zero time and zero height offset are both global lower bounds. |
| | 352 | 987 | | if (found && AffineSlabField(values, signs, 0).Sign(bestRoot) == 0 |
| | 352 | 988 | | && AffineSlabField(values, signs, 3).Sign(bestRoot) == 0) break; |
| | | 989 | | } |
| | 52 | 990 | | distance = default; contact = default; |
| | 37 | 991 | | if (!found) return false; |
| | 30 | 992 | | var n = AffineSlabField(values, signs, 0); var d = AffineSlabField(values, signs, 1); |
| | 30 | 993 | | Span<ulong> workValues = stackalloc ulong[2 * words]; Span<int> workSigns = stackalloc int[2]; |
| | 15 | 994 | | var work = AffineSlabField(workValues, workSigns, 0); |
| | 15 | 995 | | ContactQuadratic.Scale(n, Signed576.ExtendValue(Signed192.Raw(length)), work); |
| | 15 | 996 | | distance = ContactQuadratic.RoundRatio(work, d, bestRoot, 0, length.m_rawValue); |
| | 15 | 997 | | if (!QuadraticPointMaterialization.TryGetPointInFrame(origin, FixedQuaternion.Identity, |
| | 15 | 998 | | AffineSlabField(values, signs, 2), AffineSlabField(values, signs, 3), |
| | 15 | 999 | | AffineSlabField(values, signs, 4), AffineSlabField(values, signs, 5), bestRoot, |
| | 16 | 1000 | | materializationOrigin, out Vector3d local)) return false; |
| | 14 | 1001 | | contact = new FixedPointAnchor(materializationOrigin, FixedQuaternion.Identity, local); |
| | 14 | 1002 | | return true; |
| | | 1003 | | } |
| | | 1004 | | |
| | | 1005 | | private static ContactQuadratic AffineSlabField(Span<ulong> values, Span<int> signs, int index) => |
| | 7566 | 1006 | | ContactQuadratic.At(values, signs, index, 64); |
| | | 1007 | | |
| | | 1008 | | private static bool TryGetAffineSlabSweepInterior(ReadOnlySpan<WideDirection3d> p, |
| | | 1009 | | Signed320 denominator, Fixed64 halfThickness, Vector3d origin, Vector3d anchorOrigin, |
| | | 1010 | | out FixedPointAnchor contact) |
| | | 1011 | | { |
| | 34 | 1012 | | contact = default; |
| | 68 | 1013 | | WideDirection3d e = AffineSlabDifference(p[1], p[0]), f = AffineSlabDifference(p[2], p[0]); |
| | 34 | 1014 | | Signed832 det = AffineSlabCross(e.X, e.Z, f.X, f.Z); |
| | 49 | 1015 | | if (det.IsZero) return false; |
| | 19 | 1016 | | Signed832 n = AffineSlabCross(WideArithmetic.SubtractSigned576(default, p[0].X), |
| | 19 | 1017 | | WideArithmetic.SubtractSigned576(default, p[0].Z), f.X, f.Z), |
| | 19 | 1018 | | m = AffineSlabCross(e.X, e.Z, WideArithmetic.SubtractSigned576(default, p[0].X), |
| | 19 | 1019 | | WideArithmetic.SubtractSigned576(default, p[0].Z)); |
| | 40 | 1020 | | if (det.Sign < 0) { det = AffineSlabNegate(det); n = AffineSlabNegate(n); m = AffineSlabNegate(m); } |
| | 19 | 1021 | | if (n.Sign < 0 || m.Sign < 0 || WideArithmetic.SubtractSigned832(WideArithmetic.AddSigned832(n, m), det).Sign > |
| | 6 | 1022 | | return false; |
| | 26 | 1023 | | Span<ulong> values = stackalloc ulong[12 * 64]; Span<int> signs = stackalloc int[12]; |
| | 26 | 1024 | | var y = AffineSlabField(values, signs, 0); var d = AffineSlabField(values, signs, 1); |
| | 26 | 1025 | | var term = AffineSlabField(values, signs, 2); var query = AffineSlabField(values, signs, 3); |
| | 26 | 1026 | | var zero = AffineSlabField(values, signs, 4); zero.Clear(); |
| | 39 | 1027 | | y.Set(det); ContactQuadratic.Scale(y, p[0].Y, query); query.CopyTo(y); |
| | 39 | 1028 | | term.Set(n); ContactQuadratic.Scale(term, e.Y, query); y.Add(query); |
| | 39 | 1029 | | term.Set(m); ContactQuadratic.Scale(term, f.Y, query); y.Add(query); |
| | 26 | 1030 | | term.Set(det); ContactQuadratic.Scale(term, Signed576.ExtendValue(denominator), d); |
| | 13 | 1031 | | ContactQuadratic.Scale(d, Signed576.ExtendValue(Signed192.Raw(halfThickness)), term); |
| | 41 | 1032 | | term.CopyTo(query); query.Add(y, -1); if (query.Signs[0] < 0) return false; |
| | 35 | 1033 | | term.CopyTo(query); query.Add(y); if (query.Signs[0] < 0) return false; |
| | 9 | 1034 | | if (!QuadraticPointMaterialization.TryGetPointInFrame(origin, FixedQuaternion.Identity, |
| | 10 | 1035 | | zero, y, zero, d, ReadOnlySpan<ulong>.Empty, anchorOrigin, out Vector3d point)) return false; |
| | 16 | 1036 | | contact = new FixedPointAnchor(anchorOrigin, FixedQuaternion.Identity, point); return true; |
| | | 1037 | | } |
| | | 1038 | | |
| | | 1039 | | private static void KeepAffineSlabSweepFeature(ReadOnlySpan<WideDirection3d> points, Signed320 denominator, |
| | | 1040 | | in WideDirection3d normal, ReadOnlySpan<WideDirection3d> walls, Signed192 dx, Signed192 dz, |
| | | 1041 | | Fixed64 radius, Fixed64 halfThickness, int feature, Span<ulong> bestValues, |
| | | 1042 | | Span<int> bestSigns, Span<ulong> bestRoot, ref bool found) |
| | | 1043 | | { |
| | | 1044 | | const int words = 64; |
| | 704 | 1045 | | Span<ulong> values = stackalloc ulong[48 * words]; Span<int> signs = stackalloc int[48]; |
| | 704 | 1046 | | var a = AffineSlabField(values, signs, 0); var b = AffineSlabField(values, signs, 1); |
| | 704 | 1047 | | var c = AffineSlabField(values, signs, 2); var n = AffineSlabField(values, signs, 3); |
| | 704 | 1048 | | var d = AffineSlabField(values, signs, 4); var work = AffineSlabField(values, signs, 5); |
| | 704 | 1049 | | var term = AffineSlabField(values, signs, 6); var temporary = AffineSlabField(values, signs, 7); |
| | 704 | 1050 | | var px = AffineSlabField(values, signs, 8); var py = AffineSlabField(values, signs, 9); |
| | 704 | 1051 | | var pz = AffineSlabField(values, signs, 10); var pd = AffineSlabField(values, signs, 11); |
| | 704 | 1052 | | var vx = AffineSlabField(values, signs, 12); var vz = AffineSlabField(values, signs, 13); |
| | 704 | 1053 | | var tx = AffineSlabField(values, signs, 14); var ty = AffineSlabField(values, signs, 15); |
| | 704 | 1054 | | var tz = AffineSlabField(values, signs, 16); var td = AffineSlabField(values, signs, 17); |
| | 704 | 1055 | | var t = AffineSlabField(values, signs, 18); var metric = AffineSlabField(values, signs, 19); |
| | 352 | 1056 | | WideDirection3d velocity = new(WideArithmetic.MultiplySigned320(denominator, dx), default, |
| | 352 | 1057 | | WideArithmetic.MultiplySigned320(denominator, dz)); |
| | 352 | 1058 | | int vertex = feature < 3 ? feature : feature < 6 ? feature - 3 : feature < 12 ? (feature - 6) / 2 : 0; |
| | 704 | 1059 | | WideDirection3d point = points[vertex], edge = AffineSlabDifference(points[(vertex + 1) % 3], point); |
| | 352 | 1060 | | Signed576 cap = WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(halfThickness)); |
| | 1760 | 1061 | | Signed832 ee = default, ev = default, ep = default, planeK = default, planeV = default; |
| | 704 | 1062 | | bool line = feature is >= 3 and < 6, face = feature >= 12; |
| | 352 | 1063 | | if (line) |
| | | 1064 | | { |
| | 168 | 1065 | | ee = AffineSlabDot(edge, edge); if (ee.IsZero) return; |
| | 114 | 1066 | | ev = AffineSlabDot(edge, velocity); ep = AffineSlabDot(edge, point); |
| | 171 | 1067 | | Signed832 vv = AffineSlabDot(velocity, velocity), vp = AffineSlabDot(velocity, point), pp = AffineSlabDot(po |
| | 57 | 1068 | | Signed576 expanded = WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(radius)); |
| | 57 | 1069 | | Signed832 r2 = WideArithmetic.MultiplySigned576ToSigned832(expanded, expanded); |
| | 171 | 1070 | | AffineSlabProduct(ee, vv, work, a); AffineSlabProduct(ev, ev, work, term); a.Add(term, -1); |
| | 171 | 1071 | | AffineSlabProduct(ev, ep, work, b); AffineSlabProduct(ee, vp, work, term); b.Add(term, -1); |
| | 57 | 1072 | | AffineSlabProduct(ee, WideArithmetic.SubtractSigned832(pp, r2), work, c); |
| | 114 | 1073 | | AffineSlabProduct(ep, ep, work, term); c.Add(term, -1); |
| | | 1074 | | } |
| | 277 | 1075 | | else if (face) |
| | | 1076 | | { |
| | 126 | 1077 | | ee = AffineSlabDot(normal, normal); if (ee.IsZero) return; |
| | 24 | 1078 | | Signed576 height = feature == 12 ? WideArithmetic.SubtractSigned576(default, cap) : cap; |
| | 24 | 1079 | | planeK = WideArithmetic.SubtractSigned832(WideDirection3d.Dot(normal, point), |
| | 24 | 1080 | | WideArithmetic.MultiplySigned576ToSigned832(normal.Y, height)); |
| | 24 | 1081 | | planeV = AffineSlabDot(normal, velocity); |
| | 24 | 1082 | | Signed576 expanded = WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(radius)); |
| | 24 | 1083 | | Signed832 r2 = WideArithmetic.MultiplySigned576ToSigned832(expanded, expanded); |
| | 24 | 1084 | | AffineSlabProduct(planeV, planeV, work, a); |
| | 48 | 1085 | | AffineSlabProduct(planeV, planeK, work, b); b.MultiplySign(-1); |
| | 72 | 1086 | | AffineSlabProduct(planeK, planeK, work, c); AffineSlabProduct(ee, r2, work, term); c.Add(term, -1); |
| | | 1087 | | } |
| | | 1088 | | else |
| | | 1089 | | { |
| | 227 | 1090 | | if (feature < 3) |
| | | 1091 | | { |
| | 123 | 1092 | | if (WideArithmetic.SubtractSigned576(point.Y, cap).Sign > 0 || WideArithmetic.AddSigned576(point.Y, cap) |
| | 124 | 1093 | | px.Set(point.X); py.Set(point.Y); pz.Set(point.Z); pd.Set(Signed576.ExtendValue(denominator)); |
| | | 1094 | | } |
| | | 1095 | | else |
| | | 1096 | | { |
| | 150 | 1097 | | Signed576 height = (feature & 1) == 0 ? WideArithmetic.SubtractSigned576(default, cap) : cap; |
| | 300 | 1098 | | Signed576 weight = WideArithmetic.SubtractSigned576(height, point.Y), divisor = edge.Y; |
| | 242 | 1099 | | if (divisor.IsZero) return; |
| | 122 | 1100 | | if (divisor.Sign < 0) { weight = WideArithmetic.SubtractSigned576(default, weight); divisor = WideArithm |
| | 76 | 1101 | | if (weight.Sign < 0 || WideArithmetic.SubtractSigned576(weight, divisor).Sign > 0) return; |
| | 120 | 1102 | | pd.Set(divisor); ContactQuadratic.Scale(pd, Signed576.ExtendValue(denominator), work); work.CopyTo(pd); |
| | | 1103 | | // Weight/divisor uses the original edge orientation even after |
| | | 1104 | | // its signs are normalized above. |
| | 80 | 1105 | | px.Set(divisor); ContactQuadratic.Scale(px, point.X, work); |
| | 160 | 1106 | | temporary.Set(weight); ContactQuadratic.Scale(temporary, edge.X, term); work.Add(term); work.CopyTo(px); |
| | 120 | 1107 | | py.Set(divisor); ContactQuadratic.Scale(py, height, work); work.CopyTo(py); |
| | 80 | 1108 | | pz.Set(divisor); ContactQuadratic.Scale(pz, point.Z, work); |
| | 160 | 1109 | | temporary.Set(weight); ContactQuadratic.Scale(temporary, edge.Z, term); work.Add(term); work.CopyTo(pz); |
| | | 1110 | | } |
| | 142 | 1111 | | ContactQuadratic.Scale(pd, Signed576.ExtendValue(dx), vx); ContactQuadratic.Scale(pd, Signed576.ExtendValue( |
| | 71 | 1112 | | ContactQuadratic.Multiply(vx, vx, ReadOnlySpan<ulong>.Empty, a); |
| | 142 | 1113 | | ContactQuadratic.Multiply(vz, vz, ReadOnlySpan<ulong>.Empty, term); a.Add(term); |
| | 71 | 1114 | | ContactQuadratic.Multiply(vx, px, ReadOnlySpan<ulong>.Empty, b); |
| | 213 | 1115 | | ContactQuadratic.Multiply(vz, pz, ReadOnlySpan<ulong>.Empty, term); b.Add(term); b.MultiplySign(-1); |
| | 71 | 1116 | | ContactQuadratic.Multiply(px, px, ReadOnlySpan<ulong>.Empty, c); |
| | 142 | 1117 | | ContactQuadratic.Multiply(pz, pz, ReadOnlySpan<ulong>.Empty, term); c.Add(term); |
| | 71 | 1118 | | ContactQuadratic.Multiply(pd, pd, ReadOnlySpan<ulong>.Empty, work); |
| | 142 | 1119 | | ContactQuadratic.Scale(work, Signed576.ExtendValue(Fixed64.GetExactRawProduct(radius, radius)), term); c.Add |
| | | 1120 | | } |
| | 152 | 1121 | | Span<ulong> root = stackalloc ulong[2 * words]; |
| | 1204 | 1122 | | for (int branch = 0; branch < 3; branch++) |
| | | 1123 | | { |
| | 452 | 1124 | | if (branch == 0) |
| | 158 | 1125 | | { if (c.Signs[0] > 0) continue; n.Clear(); d.Set(Signed576.ExtendValue(Signed192.Signed(1))); root.Clear(); |
| | 300 | 1126 | | else if (!ContactQuadratic.TryGetParameter(a, b, c, branch == 1 ? -1 : 1, root, n, d)) continue; |
| | 158 | 1127 | | if (n.Sign(root) < 0) continue; |
| | 450 | 1128 | | d.CopyTo(work); work.Add(n, -1); if (work.Sign(root) < 0) continue; |
| | 104 | 1129 | | if (line) |
| | | 1130 | | { |
| | 156 | 1131 | | ContactQuadratic.Scale(n, ev, t); ContactQuadratic.Scale(d, ep, term); t.Add(term, -1); |
| | 52 | 1132 | | if (t.Sign(root) < 0) continue; |
| | 200 | 1133 | | ContactQuadratic.Scale(d, ee, metric); metric.CopyTo(work); work.Add(t, -1); if (work.Sign(root) < 0) co |
| | 96 | 1134 | | ContactQuadratic.Scale(d, ee, work); ContactQuadratic.Scale(work, point.Y, ty); |
| | 96 | 1135 | | ContactQuadratic.Scale(t, edge.Y, term); ty.Add(term); |
| | 192 | 1136 | | ContactQuadratic.Scale(metric, cap, work); work.CopyTo(term); term.Add(ty, -1); if (term.Sign(root) < 0) |
| | 108 | 1137 | | work.CopyTo(term); term.Add(ty); if (term.Sign(root) < 0) continue; |
| | 16 | 1138 | | ContactQuadratic.Scale(metric, Signed576.ExtendValue(denominator), td); |
| | 48 | 1139 | | ContactQuadratic.Scale(metric, point.X, tx); ContactQuadratic.Scale(t, edge.X, term); tx.Add(term); |
| | 48 | 1140 | | ContactQuadratic.Scale(metric, point.Z, tz); ContactQuadratic.Scale(t, edge.Z, term); tz.Add(term); |
| | | 1141 | | } |
| | 52 | 1142 | | else if (face) |
| | | 1143 | | { |
| | 96 | 1144 | | ContactQuadratic.Scale(n, planeV, t); ContactQuadratic.Scale(d, planeK, term); t.Add(term, -1); |
| | 64 | 1145 | | metric.Set(ee); ContactQuadratic.Scale(metric, Signed576.ExtendValue(denominator), work); |
| | 32 | 1146 | | ContactQuadratic.Multiply(work, d, root, td); |
| | 64 | 1147 | | ContactQuadratic.Scale(n, Signed576.ExtendValue(dx), temporary); ContactQuadratic.Multiply(temporary, wo |
| | 64 | 1148 | | ContactQuadratic.Scale(t, normal.X, term); tx.Add(term, -1); |
| | 64 | 1149 | | ContactQuadratic.Scale(n, Signed576.ExtendValue(dz), temporary); ContactQuadratic.Multiply(temporary, wo |
| | 64 | 1150 | | ContactQuadratic.Scale(t, normal.Z, term); tz.Add(term, -1); |
| | 32 | 1151 | | ContactQuadratic.Multiply(metric, d, root, work); |
| | 32 | 1152 | | ContactQuadratic.Scale(work, feature == 12 ? WideArithmetic.SubtractSigned576(default, cap) : cap, ty); |
| | 40 | 1153 | | if (!ContainsAffineSlabSweepFace(walls, points, denominator, tx, ty, tz, td, root, work, term, temporary |
| | | 1154 | | } |
| | | 1155 | | else |
| | | 1156 | | { |
| | 40 | 1157 | | ContactQuadratic.Multiply(px, d, root, tx); ContactQuadratic.Multiply(py, d, root, ty); |
| | 40 | 1158 | | ContactQuadratic.Multiply(pz, d, root, tz); ContactQuadratic.Multiply(pd, d, root, td); |
| | | 1159 | | } |
| | 60 | 1160 | | if (found) |
| | | 1161 | | { |
| | 45 | 1162 | | int order = ContactQuadratic.CompareRatios(n, d, root, |
| | 45 | 1163 | | AffineSlabField(bestValues, bestSigns, 0), AffineSlabField(bestValues, bestSigns, 1), bestRoot); |
| | 45 | 1164 | | if (order > 0) continue; |
| | 19 | 1165 | | if (order == 0) |
| | | 1166 | | { |
| | | 1167 | | // As in the rigid owner, equal-time witnesses prefer the |
| | | 1168 | | // closest height to the slab center. Retain the original |
| | | 1169 | | // quadratic ratios instead of comparing rounded anchors. |
| | 13 | 1170 | | ty.CopyTo(work, ty.Sign(root) < 0 ? -1 : 1); |
| | 13 | 1171 | | var bestY = AffineSlabField(bestValues, bestSigns, 3); |
| | 13 | 1172 | | bestY.CopyTo(term, bestY.Sign(bestRoot) < 0 ? -1 : 1); |
| | 13 | 1173 | | if (ContactQuadratic.CompareRatios(work, td, root, term, |
| | 13 | 1174 | | AffineSlabField(bestValues, bestSigns, 5), bestRoot) >= 0) continue; |
| | | 1175 | | } |
| | | 1176 | | } |
| | 46 | 1177 | | n.CopyTo(AffineSlabField(bestValues, bestSigns, 0)); d.CopyTo(AffineSlabField(bestValues, bestSigns, 1)); |
| | 46 | 1178 | | tx.CopyTo(AffineSlabField(bestValues, bestSigns, 2)); ty.CopyTo(AffineSlabField(bestValues, bestSigns, 3)); |
| | 46 | 1179 | | tz.CopyTo(AffineSlabField(bestValues, bestSigns, 4)); td.CopyTo(AffineSlabField(bestValues, bestSigns, 5)); |
| | 46 | 1180 | | root.CopyTo(bestRoot); found = true; |
| | 25 | 1181 | | if (branch == 0) return; |
| | | 1182 | | } |
| | 150 | 1183 | | } |
| | | 1184 | | |
| | | 1185 | | private static bool ContainsAffineSlabSweepFace(ReadOnlySpan<WideDirection3d> walls, |
| | | 1186 | | ReadOnlySpan<WideDirection3d> points, Signed320 denominator, scoped ContactQuadratic x, |
| | | 1187 | | scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic d, |
| | | 1188 | | ReadOnlySpan<ulong> root, scoped ContactQuadratic work, scoped ContactQuadratic term, |
| | | 1189 | | scoped ContactQuadratic temporary) |
| | | 1190 | | { |
| | 216 | 1191 | | for (int i = 0; i < 3; i++) |
| | | 1192 | | { |
| | 84 | 1193 | | WideDirection3d wall = walls[i]; |
| | 252 | 1194 | | ContactQuadratic.Scale(x, wall.X, work); ContactQuadratic.Scale(y, wall.Y, term); work.Add(term); |
| | 168 | 1195 | | ContactQuadratic.Scale(z, wall.Z, term); work.Add(term); |
| | 84 | 1196 | | ContactQuadratic.Scale(work, Signed576.ExtendValue(denominator), temporary); |
| | 168 | 1197 | | ContactQuadratic.Scale(d, WideDirection3d.Dot(wall, points[i]), term); temporary.Add(term, -1); |
| | 92 | 1198 | | if (temporary.Sign(root) < 0) return false; |
| | | 1199 | | } |
| | 24 | 1200 | | return true; |
| | | 1201 | | } |
| | | 1202 | | |
| | | 1203 | | private static WideDirection3d ReduceAffineSlabDirection(in WideDirection3d value) |
| | | 1204 | | { |
| | 116 | 1205 | | if (value.IsZero) return value; |
| | 92 | 1206 | | Span<Signed576> components = stackalloc Signed576[3] { value.X, value.Y, value.Z }; |
| | 92 | 1207 | | WideArithmetic.ReduceCommonScale(components); |
| | 92 | 1208 | | return new(components[0], components[1], components[2]); |
| | | 1209 | | } |
| | | 1210 | | |
| | | 1211 | | private static WideDirection3d AffineSlabDifference(in WideDirection3d a, in WideDirection3d b) => |
| | 420 | 1212 | | new(WideArithmetic.SubtractSigned576(a.X, b.X), WideArithmetic.SubtractSigned576(a.Y, b.Y), WideArithmetic.Subtr |
| | | 1213 | | private static Signed832 AffineSlabDot(in WideDirection3d a, in WideDirection3d b) => |
| | 434 | 1214 | | WideArithmetic.AddSigned832(WideArithmetic.MultiplySigned576ToSigned832(a.X, b.X), WideArithmetic.MultiplySigned |
| | | 1215 | | private static Signed832 AffineSlabCross(Signed576 ax, Signed576 az, Signed576 bx, Signed576 bz) => |
| | 72 | 1216 | | WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(ax, bz), WideArithmetic.MultiplySig |
| | 21 | 1217 | | private static Signed832 AffineSlabNegate(Signed832 value) => WideArithmetic.SubtractSigned832(default, value); |
| | | 1218 | | private static void AffineSlabProduct(Signed832 a, Signed832 b, scoped ContactQuadratic work, scoped ContactQuadrati |
| | 1314 | 1219 | | { work.Set(a); ContactQuadratic.Scale(work, b, result); } |
| | | 1220 | | |
| | | 1221 | | } |