| | | 1 | | //======================================================================= |
| | | 2 | | // WidePointAnchor3d.Exact.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 | | /// Exact three-dimensional point-anchor term operations. |
| | | 14 | | /// </content> |
| | | 15 | | internal static partial class WidePointAnchor3d |
| | | 16 | | { |
| | | 17 | | internal static bool TryGetPointInFrame(in FixedPointAnchor anchor, Vector3d samplingOrigin, out Vector3d point) |
| | | 18 | | { |
| | 353 | 19 | | WideRationalBasis3d basis = new(anchor.Rotation); |
| | 353 | 20 | | Signed320 denominator = GetAnchorDenominator(basis); |
| | 706 | 21 | | Span<ulong> denominatorWords = stackalloc ulong[9], numeratorWords = stackalloc ulong[9]; |
| | 353 | 22 | | WideArithmetic.GetMagnitude(Signed576.ExtendValue(denominator), denominatorWords); |
| | 353 | 23 | | Span<Fixed64> coordinates = stackalloc Fixed64[3]; |
| | 2806 | 24 | | for (int axis = 0; axis < 3; axis++) |
| | | 25 | | { |
| | 1053 | 26 | | Signed320 numerator = GetAnchorDeltaNumerator(anchor.Origin[axis], samplingOrigin[axis], |
| | 1053 | 27 | | axis == 0 ? basis.Xx : axis == 1 ? basis.Xy : basis.Xz, |
| | 1053 | 28 | | axis == 0 ? basis.Yx : axis == 1 ? basis.Yy : basis.Yz, |
| | 1053 | 29 | | axis == 0 ? basis.Zx : axis == 1 ? basis.Zy : basis.Zz, |
| | 1053 | 30 | | basis, anchor.LocalPoint, anchor.LocalDisplacement, anchor.LocalTranslation, anchor.ExactLocalTerm, deno |
| | 1053 | 31 | | WideArithmetic.GetMagnitude(Signed576.ExtendValue(numerator), numeratorWords); |
| | | 32 | | // Preserve global half-tie parity without requiring a representable |
| | | 33 | | // absolute coordinate. No intermediate origin difference is narrowed. |
| | 1053 | 34 | | if (!Fixed64.TryGetSignedRawRatio(numeratorWords, denominatorWords, numerator.Sign < 0, |
| | 1053 | 35 | | out coordinates[axis], samplingOrigin[axis].m_rawValue)) |
| | | 36 | | { |
| | 3 | 37 | | point = default; |
| | 3 | 38 | | return false; |
| | | 39 | | } |
| | | 40 | | } |
| | 350 | 41 | | point = new Vector3d(coordinates[0], coordinates[1], coordinates[2]); |
| | 350 | 42 | | return true; |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | // Compare dot(first-second, direction) with dot(third-fourth, direction). |
| | | 46 | | // Keep both signed ratios exact: materializing either projection would |
| | | 47 | | // lose sub-raw distinctions or saturate unrelated large offsets equally. |
| | | 48 | | internal static int CompareProjectedOffsets( |
| | | 49 | | in FixedPointAnchor first, |
| | | 50 | | in FixedPointAnchor second, |
| | | 51 | | in FixedPointAnchor third, |
| | | 52 | | in FixedPointAnchor fourth, |
| | | 53 | | Vector3d direction) => |
| | 18 | 54 | | CompareProjectedOffsets(first, second, third, fourth, direction, direction); |
| | | 55 | | |
| | | 56 | | internal static int CompareProjectedOffsets( |
| | | 57 | | in FixedPointAnchor first, |
| | | 58 | | in FixedPointAnchor second, |
| | | 59 | | in FixedPointAnchor third, |
| | | 60 | | in FixedPointAnchor fourth, |
| | | 61 | | Vector3d firstDirection, |
| | | 62 | | Vector3d secondDirection) |
| | | 63 | | { |
| | 20 | 64 | | GetExactProjectedOffsetRatio( |
| | 20 | 65 | | first.Origin, first.Rotation, first.LocalPoint, |
| | 20 | 66 | | first.LocalDisplacement, first.LocalTranslation, first.ExactLocalTerm, |
| | 20 | 67 | | second.Origin, second.Rotation, second.LocalPoint, |
| | 20 | 68 | | second.LocalDisplacement, second.LocalTranslation, second.ExactLocalTerm, |
| | 20 | 69 | | firstDirection, out Signed704 left, out Signed704 leftDenominator); |
| | 20 | 70 | | GetExactProjectedOffsetRatio( |
| | 20 | 71 | | third.Origin, third.Rotation, third.LocalPoint, |
| | 20 | 72 | | third.LocalDisplacement, third.LocalTranslation, third.ExactLocalTerm, |
| | 20 | 73 | | fourth.Origin, fourth.Rotation, fourth.LocalPoint, |
| | 20 | 74 | | fourth.LocalDisplacement, fourth.LocalTranslation, fourth.ExactLocalTerm, |
| | 20 | 75 | | secondDirection, out Signed704 right, out Signed704 rightDenominator); |
| | 20 | 76 | | int sign = left.Sign; |
| | 20 | 77 | | int signComparison = sign.CompareTo(right.Sign); |
| | 20 | 78 | | if (signComparison != 0) |
| | 6 | 79 | | return signComparison; |
| | 14 | 80 | | if (sign == 0) |
| | 2 | 81 | | return 0; |
| | | 82 | | |
| | | 83 | | // The positive denominators and 704-bit signed numerators extend |
| | | 84 | | // losslessly; the existing full products retain all 1408 bits. |
| | 12 | 85 | | return sign * WideArithmetic.CompareNonNegativeProducts( |
| | 12 | 86 | | Signed832.ExtendValue(left), Signed832.ExtendValue(rightDenominator), |
| | 12 | 87 | | Signed832.ExtendValue(right), Signed832.ExtendValue(leftDenominator)); |
| | | 88 | | } |
| | | 89 | | |
| | | 90 | | internal static bool TryGetPoint( |
| | | 91 | | Vector3d origin, |
| | | 92 | | FixedQuaternion rotation, |
| | | 93 | | Vector3d localPoint, |
| | | 94 | | Vector3d localDisplacement, |
| | | 95 | | Vector3d localTranslation, |
| | | 96 | | FixedPointAnchorTerm3d exactLocalTerm, |
| | | 97 | | out Vector3d point) |
| | | 98 | | { |
| | 5143 | 99 | | if (exactLocalTerm.IsZero |
| | 5143 | 100 | | && localTranslation == Vector3d.Zero) |
| | | 101 | | { |
| | 5033 | 102 | | return TryGetPoint( |
| | 5033 | 103 | | origin, |
| | 5033 | 104 | | rotation, |
| | 5033 | 105 | | localPoint, |
| | 5033 | 106 | | localDisplacement, |
| | 5033 | 107 | | out point); |
| | | 108 | | } |
| | | 109 | | |
| | 110 | 110 | | WideRationalBasis3d basis = new(rotation); |
| | 110 | 111 | | Signed320 denominator = GetAnchorDenominator(basis); |
| | 110 | 112 | | Signed576 denominatorWide = |
| | 110 | 113 | | Signed576.ExtendValue(denominator); |
| | 110 | 114 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 110 | 115 | | Signed576.ExtendValue( |
| | 110 | 116 | | GetAnchorCoordinateNumerator( |
| | 110 | 117 | | origin.X, |
| | 110 | 118 | | basis.Xx, |
| | 110 | 119 | | basis.Yx, |
| | 110 | 120 | | basis.Zx, |
| | 110 | 121 | | basis, |
| | 110 | 122 | | localPoint, |
| | 110 | 123 | | localDisplacement, |
| | 110 | 124 | | localTranslation, |
| | 110 | 125 | | exactLocalTerm)), |
| | 110 | 126 | | denominatorWide, |
| | 110 | 127 | | out Fixed64 x) |
| | 110 | 128 | | & Fixed64.TryGetSignedRawRatio( |
| | 110 | 129 | | Signed576.ExtendValue( |
| | 110 | 130 | | GetAnchorCoordinateNumerator( |
| | 110 | 131 | | origin.Y, |
| | 110 | 132 | | basis.Xy, |
| | 110 | 133 | | basis.Yy, |
| | 110 | 134 | | basis.Zy, |
| | 110 | 135 | | basis, |
| | 110 | 136 | | localPoint, |
| | 110 | 137 | | localDisplacement, |
| | 110 | 138 | | localTranslation, |
| | 110 | 139 | | exactLocalTerm)), |
| | 110 | 140 | | denominatorWide, |
| | 110 | 141 | | out Fixed64 y) |
| | 110 | 142 | | & Fixed64.TryGetSignedRawRatio( |
| | 110 | 143 | | Signed576.ExtendValue( |
| | 110 | 144 | | GetAnchorCoordinateNumerator( |
| | 110 | 145 | | origin.Z, |
| | 110 | 146 | | basis.Xz, |
| | 110 | 147 | | basis.Yz, |
| | 110 | 148 | | basis.Zz, |
| | 110 | 149 | | basis, |
| | 110 | 150 | | localPoint, |
| | 110 | 151 | | localDisplacement, |
| | 110 | 152 | | localTranslation, |
| | 110 | 153 | | exactLocalTerm)), |
| | 110 | 154 | | denominatorWide, |
| | 110 | 155 | | out Fixed64 z); |
| | 110 | 156 | | point = representable ? new Vector3d(x, y, z) : default; |
| | 110 | 157 | | return representable; |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | internal static bool TryGetRelativeOffset( |
| | | 161 | | Vector3d firstOrigin, |
| | | 162 | | FixedQuaternion firstRotation, |
| | | 163 | | Vector3d firstLocalPoint, |
| | | 164 | | Vector3d firstLocalDisplacement, |
| | | 165 | | Vector3d firstLocalTranslation, |
| | | 166 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 167 | | Vector3d secondOrigin, |
| | | 168 | | FixedQuaternion secondRotation, |
| | | 169 | | Vector3d secondLocalPoint, |
| | | 170 | | Vector3d secondLocalDisplacement, |
| | | 171 | | Vector3d secondLocalTranslation, |
| | | 172 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 173 | | Fixed64 scale, |
| | | 174 | | out Vector3d offset) |
| | | 175 | | { |
| | 1834 | 176 | | if (firstExactLocalTerm.IsZero |
| | 1834 | 177 | | && secondExactLocalTerm.IsZero |
| | 1834 | 178 | | && firstLocalTranslation == Vector3d.Zero |
| | 1834 | 179 | | && secondLocalTranslation == Vector3d.Zero) |
| | | 180 | | { |
| | 1782 | 181 | | return TryGetRelativeOffset( |
| | 1782 | 182 | | firstOrigin, |
| | 1782 | 183 | | firstRotation, |
| | 1782 | 184 | | firstLocalPoint, |
| | 1782 | 185 | | firstLocalDisplacement, |
| | 1782 | 186 | | secondOrigin, |
| | 1782 | 187 | | secondRotation, |
| | 1782 | 188 | | secondLocalPoint, |
| | 1782 | 189 | | secondLocalDisplacement, |
| | 1782 | 190 | | scale, |
| | 1782 | 191 | | out offset); |
| | | 192 | | } |
| | | 193 | | |
| | 52 | 194 | | GetExactRelativeOffsetRatio( |
| | 52 | 195 | | firstOrigin, |
| | 52 | 196 | | firstRotation, |
| | 52 | 197 | | firstLocalPoint, |
| | 52 | 198 | | firstLocalDisplacement, |
| | 52 | 199 | | firstLocalTranslation, |
| | 52 | 200 | | firstExactLocalTerm, |
| | 52 | 201 | | secondOrigin, |
| | 52 | 202 | | secondRotation, |
| | 52 | 203 | | secondLocalPoint, |
| | 52 | 204 | | secondLocalDisplacement, |
| | 52 | 205 | | secondLocalTranslation, |
| | 52 | 206 | | secondExactLocalTerm, |
| | 52 | 207 | | out Signed576 xNumerator, |
| | 52 | 208 | | out Signed576 yNumerator, |
| | 52 | 209 | | out Signed576 zNumerator, |
| | 52 | 210 | | out Signed576 denominator); |
| | 52 | 211 | | bool representable = TryGetScaledExactCoordinate( |
| | 52 | 212 | | xNumerator, |
| | 52 | 213 | | denominator, |
| | 52 | 214 | | scale, |
| | 52 | 215 | | out Fixed64 x) |
| | 52 | 216 | | & TryGetScaledExactCoordinate( |
| | 52 | 217 | | yNumerator, |
| | 52 | 218 | | denominator, |
| | 52 | 219 | | scale, |
| | 52 | 220 | | out Fixed64 y) |
| | 52 | 221 | | & TryGetScaledExactCoordinate( |
| | 52 | 222 | | zNumerator, |
| | 52 | 223 | | denominator, |
| | 52 | 224 | | scale, |
| | 52 | 225 | | out Fixed64 z); |
| | 52 | 226 | | offset = representable ? new Vector3d(x, y, z) : default; |
| | 52 | 227 | | return representable; |
| | | 228 | | } |
| | | 229 | | |
| | | 230 | | internal static bool RepresentsSamePoint( |
| | | 231 | | in FixedPointAnchor first, |
| | | 232 | | in FixedPointAnchor second) |
| | | 233 | | { |
| | 3 | 234 | | GetExactRelativeOffsetRatio( |
| | 3 | 235 | | first.Origin, |
| | 3 | 236 | | first.Rotation, |
| | 3 | 237 | | first.LocalPoint, |
| | 3 | 238 | | first.LocalDisplacement, |
| | 3 | 239 | | first.LocalTranslation, |
| | 3 | 240 | | first.ExactLocalTerm, |
| | 3 | 241 | | second.Origin, |
| | 3 | 242 | | second.Rotation, |
| | 3 | 243 | | second.LocalPoint, |
| | 3 | 244 | | second.LocalDisplacement, |
| | 3 | 245 | | second.LocalTranslation, |
| | 3 | 246 | | second.ExactLocalTerm, |
| | 3 | 247 | | out Signed576 x, |
| | 3 | 248 | | out Signed576 y, |
| | 3 | 249 | | out Signed576 z, |
| | 3 | 250 | | out _); |
| | 3 | 251 | | return x.IsZero && y.IsZero && z.IsZero; |
| | | 252 | | } |
| | | 253 | | |
| | | 254 | | internal static bool TryGetLocalPointIn( |
| | | 255 | | Vector3d pointOrigin, |
| | | 256 | | FixedQuaternion pointRotation, |
| | | 257 | | Vector3d pointLocalPoint, |
| | | 258 | | Vector3d pointLocalDisplacement, |
| | | 259 | | Vector3d pointLocalTranslation, |
| | | 260 | | FixedPointAnchorTerm3d exactLocalTerm, |
| | | 261 | | Vector3d frameOrigin, |
| | | 262 | | FixedQuaternion frameRotation, |
| | | 263 | | out Vector3d localPoint) |
| | | 264 | | { |
| | 4810 | 265 | | if (exactLocalTerm.IsZero |
| | 4810 | 266 | | && pointLocalTranslation == Vector3d.Zero) |
| | | 267 | | { |
| | 4798 | 268 | | return TryGetLocalPointIn( |
| | 4798 | 269 | | pointOrigin, |
| | 4798 | 270 | | pointRotation, |
| | 4798 | 271 | | pointLocalPoint, |
| | 4798 | 272 | | pointLocalDisplacement, |
| | 4798 | 273 | | frameOrigin, |
| | 4798 | 274 | | frameRotation, |
| | 4798 | 275 | | out localPoint); |
| | | 276 | | } |
| | | 277 | | |
| | 12 | 278 | | WideRationalBasis3d pointBasis = new(pointRotation); |
| | 12 | 279 | | WideRationalBasis3d frameBasis = new(frameRotation); |
| | 12 | 280 | | Signed320 pointDenominator = GetAnchorDenominator(pointBasis); |
| | 12 | 281 | | Signed320 deltaX = GetAnchorDeltaNumerator( |
| | 12 | 282 | | pointOrigin.X, |
| | 12 | 283 | | frameOrigin.X, |
| | 12 | 284 | | pointBasis.Xx, |
| | 12 | 285 | | pointBasis.Yx, |
| | 12 | 286 | | pointBasis.Zx, |
| | 12 | 287 | | pointBasis, |
| | 12 | 288 | | pointLocalPoint, |
| | 12 | 289 | | pointLocalDisplacement, |
| | 12 | 290 | | pointLocalTranslation, |
| | 12 | 291 | | exactLocalTerm, |
| | 12 | 292 | | pointDenominator); |
| | 12 | 293 | | Signed320 deltaY = GetAnchorDeltaNumerator( |
| | 12 | 294 | | pointOrigin.Y, |
| | 12 | 295 | | frameOrigin.Y, |
| | 12 | 296 | | pointBasis.Xy, |
| | 12 | 297 | | pointBasis.Yy, |
| | 12 | 298 | | pointBasis.Zy, |
| | 12 | 299 | | pointBasis, |
| | 12 | 300 | | pointLocalPoint, |
| | 12 | 301 | | pointLocalDisplacement, |
| | 12 | 302 | | pointLocalTranslation, |
| | 12 | 303 | | exactLocalTerm, |
| | 12 | 304 | | pointDenominator); |
| | 12 | 305 | | Signed320 deltaZ = GetAnchorDeltaNumerator( |
| | 12 | 306 | | pointOrigin.Z, |
| | 12 | 307 | | frameOrigin.Z, |
| | 12 | 308 | | pointBasis.Xz, |
| | 12 | 309 | | pointBasis.Yz, |
| | 12 | 310 | | pointBasis.Zz, |
| | 12 | 311 | | pointBasis, |
| | 12 | 312 | | pointLocalPoint, |
| | 12 | 313 | | pointLocalDisplacement, |
| | 12 | 314 | | pointLocalTranslation, |
| | 12 | 315 | | exactLocalTerm, |
| | 12 | 316 | | pointDenominator); |
| | 12 | 317 | | Signed576 denominator = WideArithmetic.MultiplySigned320( |
| | 12 | 318 | | pointDenominator, |
| | 12 | 319 | | Signed320.ExtendValue(frameBasis.Denominator)); |
| | 12 | 320 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 12 | 321 | | GetWideProjection( |
| | 12 | 322 | | deltaX, |
| | 12 | 323 | | deltaY, |
| | 12 | 324 | | deltaZ, |
| | 12 | 325 | | frameBasis.Xx, |
| | 12 | 326 | | frameBasis.Xy, |
| | 12 | 327 | | frameBasis.Xz), |
| | 12 | 328 | | denominator, |
| | 12 | 329 | | out Fixed64 x) |
| | 12 | 330 | | & Fixed64.TryGetSignedRawRatio( |
| | 12 | 331 | | GetWideProjection( |
| | 12 | 332 | | deltaX, |
| | 12 | 333 | | deltaY, |
| | 12 | 334 | | deltaZ, |
| | 12 | 335 | | frameBasis.Yx, |
| | 12 | 336 | | frameBasis.Yy, |
| | 12 | 337 | | frameBasis.Yz), |
| | 12 | 338 | | denominator, |
| | 12 | 339 | | out Fixed64 y) |
| | 12 | 340 | | & Fixed64.TryGetSignedRawRatio( |
| | 12 | 341 | | GetWideProjection( |
| | 12 | 342 | | deltaX, |
| | 12 | 343 | | deltaY, |
| | 12 | 344 | | deltaZ, |
| | 12 | 345 | | frameBasis.Zx, |
| | 12 | 346 | | frameBasis.Zy, |
| | 12 | 347 | | frameBasis.Zz), |
| | 12 | 348 | | denominator, |
| | 12 | 349 | | out Fixed64 z); |
| | 12 | 350 | | localPoint = representable ? new Vector3d(x, y, z) : default; |
| | 12 | 351 | | return representable; |
| | | 352 | | } |
| | | 353 | | |
| | | 354 | | internal static bool TryGetProjectedOffset( |
| | | 355 | | Vector3d firstOrigin, |
| | | 356 | | FixedQuaternion firstRotation, |
| | | 357 | | Vector3d firstLocalPoint, |
| | | 358 | | Vector3d firstLocalDisplacement, |
| | | 359 | | Vector3d firstLocalTranslation, |
| | | 360 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 361 | | Vector3d secondOrigin, |
| | | 362 | | FixedQuaternion secondRotation, |
| | | 363 | | Vector3d secondLocalPoint, |
| | | 364 | | Vector3d secondLocalDisplacement, |
| | | 365 | | Vector3d secondLocalTranslation, |
| | | 366 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 367 | | Vector3d direction, |
| | | 368 | | out Fixed64 projection) |
| | | 369 | | { |
| | 278 | 370 | | if (firstExactLocalTerm.IsZero |
| | 278 | 371 | | && secondExactLocalTerm.IsZero |
| | 278 | 372 | | && firstLocalTranslation == Vector3d.Zero |
| | 278 | 373 | | && secondLocalTranslation == Vector3d.Zero) |
| | | 374 | | { |
| | 262 | 375 | | return TryGetProjectedOffset( |
| | 262 | 376 | | firstOrigin, |
| | 262 | 377 | | firstRotation, |
| | 262 | 378 | | firstLocalPoint, |
| | 262 | 379 | | firstLocalDisplacement, |
| | 262 | 380 | | secondOrigin, |
| | 262 | 381 | | secondRotation, |
| | 262 | 382 | | secondLocalPoint, |
| | 262 | 383 | | secondLocalDisplacement, |
| | 262 | 384 | | direction, |
| | 262 | 385 | | out projection); |
| | | 386 | | } |
| | | 387 | | |
| | 16 | 388 | | GetExactProjectedOffsetRatio( |
| | 16 | 389 | | firstOrigin, |
| | 16 | 390 | | firstRotation, |
| | 16 | 391 | | firstLocalPoint, |
| | 16 | 392 | | firstLocalDisplacement, |
| | 16 | 393 | | firstLocalTranslation, |
| | 16 | 394 | | firstExactLocalTerm, |
| | 16 | 395 | | secondOrigin, |
| | 16 | 396 | | secondRotation, |
| | 16 | 397 | | secondLocalPoint, |
| | 16 | 398 | | secondLocalDisplacement, |
| | 16 | 399 | | secondLocalTranslation, |
| | 16 | 400 | | secondExactLocalTerm, |
| | 16 | 401 | | direction, |
| | 16 | 402 | | out Signed704 numerator, |
| | 16 | 403 | | out Signed704 denominator); |
| | 16 | 404 | | return Fixed64.TryGetSignedRawRatio( |
| | 16 | 405 | | numerator, |
| | 16 | 406 | | denominator, |
| | 16 | 407 | | out projection); |
| | | 408 | | } |
| | | 409 | | |
| | | 410 | | internal static Fixed64 ProjectNonNegativeOffset( |
| | | 411 | | Vector3d firstOrigin, |
| | | 412 | | FixedQuaternion firstRotation, |
| | | 413 | | Vector3d firstLocalPoint, |
| | | 414 | | Vector3d firstLocalDisplacement, |
| | | 415 | | Vector3d firstLocalTranslation, |
| | | 416 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 417 | | Vector3d secondOrigin, |
| | | 418 | | FixedQuaternion secondRotation, |
| | | 419 | | Vector3d secondLocalPoint, |
| | | 420 | | Vector3d secondLocalDisplacement, |
| | | 421 | | Vector3d secondLocalTranslation, |
| | | 422 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 423 | | Vector3d direction) |
| | | 424 | | { |
| | 289 | 425 | | if (firstExactLocalTerm.IsZero |
| | 289 | 426 | | && secondExactLocalTerm.IsZero |
| | 289 | 427 | | && firstLocalTranslation == Vector3d.Zero |
| | 289 | 428 | | && secondLocalTranslation == Vector3d.Zero) |
| | | 429 | | { |
| | 286 | 430 | | return ProjectNonNegativeOffset( |
| | 286 | 431 | | firstOrigin, |
| | 286 | 432 | | firstRotation, |
| | 286 | 433 | | firstLocalPoint, |
| | 286 | 434 | | firstLocalDisplacement, |
| | 286 | 435 | | secondOrigin, |
| | 286 | 436 | | secondRotation, |
| | 286 | 437 | | secondLocalPoint, |
| | 286 | 438 | | secondLocalDisplacement, |
| | 286 | 439 | | direction); |
| | | 440 | | } |
| | | 441 | | |
| | 3 | 442 | | GetExactProjectedOffsetRatio( |
| | 3 | 443 | | firstOrigin, |
| | 3 | 444 | | firstRotation, |
| | 3 | 445 | | firstLocalPoint, |
| | 3 | 446 | | firstLocalDisplacement, |
| | 3 | 447 | | firstLocalTranslation, |
| | 3 | 448 | | firstExactLocalTerm, |
| | 3 | 449 | | secondOrigin, |
| | 3 | 450 | | secondRotation, |
| | 3 | 451 | | secondLocalPoint, |
| | 3 | 452 | | secondLocalDisplacement, |
| | 3 | 453 | | secondLocalTranslation, |
| | 3 | 454 | | secondExactLocalTerm, |
| | 3 | 455 | | direction, |
| | 3 | 456 | | out Signed704 numerator, |
| | 3 | 457 | | out Signed704 denominator); |
| | 3 | 458 | | return Fixed64.GetNonNegativeRawRatioFloor( |
| | 3 | 459 | | numerator, |
| | 3 | 460 | | denominator); |
| | | 461 | | } |
| | | 462 | | |
| | | 463 | | private static void GetExactProjectedOffsetRatio( |
| | | 464 | | Vector3d firstOrigin, |
| | | 465 | | FixedQuaternion firstRotation, |
| | | 466 | | Vector3d firstLocalPoint, |
| | | 467 | | Vector3d firstLocalDisplacement, |
| | | 468 | | Vector3d firstLocalTranslation, |
| | | 469 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 470 | | Vector3d secondOrigin, |
| | | 471 | | FixedQuaternion secondRotation, |
| | | 472 | | Vector3d secondLocalPoint, |
| | | 473 | | Vector3d secondLocalDisplacement, |
| | | 474 | | Vector3d secondLocalTranslation, |
| | | 475 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 476 | | Vector3d direction, |
| | | 477 | | out Signed704 numerator, |
| | | 478 | | out Signed704 denominator) |
| | | 479 | | { |
| | 59 | 480 | | GetExactRelativeOffsetRatio( |
| | 59 | 481 | | firstOrigin, |
| | 59 | 482 | | firstRotation, |
| | 59 | 483 | | firstLocalPoint, |
| | 59 | 484 | | firstLocalDisplacement, |
| | 59 | 485 | | firstLocalTranslation, |
| | 59 | 486 | | firstExactLocalTerm, |
| | 59 | 487 | | secondOrigin, |
| | 59 | 488 | | secondRotation, |
| | 59 | 489 | | secondLocalPoint, |
| | 59 | 490 | | secondLocalDisplacement, |
| | 59 | 491 | | secondLocalTranslation, |
| | 59 | 492 | | secondExactLocalTerm, |
| | 59 | 493 | | out Signed576 x, |
| | 59 | 494 | | out Signed576 y, |
| | 59 | 495 | | out Signed576 z, |
| | 59 | 496 | | out Signed576 coordinateDenominator); |
| | 59 | 497 | | numerator = WideArithmetic.AddSigned704( |
| | 59 | 498 | | WideArithmetic.AddSigned704( |
| | 59 | 499 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 59 | 500 | | x, |
| | 59 | 501 | | Signed320.ExtendValue(Signed192.Raw(direction.X))), |
| | 59 | 502 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 59 | 503 | | y, |
| | 59 | 504 | | Signed320.ExtendValue(Signed192.Raw(direction.Y)))), |
| | 59 | 505 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 59 | 506 | | z, |
| | 59 | 507 | | Signed320.ExtendValue(Signed192.Raw(direction.Z)))); |
| | 59 | 508 | | denominator = WideArithmetic.MultiplySigned576ToSigned704( |
| | 59 | 509 | | coordinateDenominator, |
| | 59 | 510 | | Signed320.ExtendValue(Signed192.One)); |
| | 59 | 511 | | } |
| | | 512 | | |
| | | 513 | | internal static void GetExactRelativeOffsetRatio( |
| | | 514 | | Vector3d firstOrigin, |
| | | 515 | | FixedQuaternion firstRotation, |
| | | 516 | | Vector3d firstLocalPoint, |
| | | 517 | | Vector3d firstLocalDisplacement, |
| | | 518 | | Vector3d firstLocalTranslation, |
| | | 519 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 520 | | Vector3d secondOrigin, |
| | | 521 | | FixedQuaternion secondRotation, |
| | | 522 | | Vector3d secondLocalPoint, |
| | | 523 | | Vector3d secondLocalDisplacement, |
| | | 524 | | Vector3d secondLocalTranslation, |
| | | 525 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 526 | | out Signed576 x, |
| | | 527 | | out Signed576 y, |
| | | 528 | | out Signed576 z, |
| | | 529 | | out Signed576 denominator) |
| | | 530 | | { |
| | 147 | 531 | | WideRationalBasis3d firstBasis = new(firstRotation); |
| | 147 | 532 | | WideRationalBasis3d secondBasis = new(secondRotation); |
| | 147 | 533 | | Signed320 firstDenominator = GetAnchorDenominator(firstBasis); |
| | 147 | 534 | | Signed320 secondDenominator = GetAnchorDenominator(secondBasis); |
| | 147 | 535 | | denominator = WideArithmetic.MultiplySigned320( |
| | 147 | 536 | | firstDenominator, |
| | 147 | 537 | | secondDenominator); |
| | 147 | 538 | | x = GetExactRelativeOffsetNumerator( |
| | 147 | 539 | | firstOrigin.X, |
| | 147 | 540 | | firstBasis.Xx, |
| | 147 | 541 | | firstBasis.Yx, |
| | 147 | 542 | | firstBasis.Zx, |
| | 147 | 543 | | firstBasis, |
| | 147 | 544 | | firstLocalPoint, |
| | 147 | 545 | | firstLocalDisplacement, |
| | 147 | 546 | | firstLocalTranslation, |
| | 147 | 547 | | firstExactLocalTerm, |
| | 147 | 548 | | firstDenominator, |
| | 147 | 549 | | secondOrigin.X, |
| | 147 | 550 | | secondBasis.Xx, |
| | 147 | 551 | | secondBasis.Yx, |
| | 147 | 552 | | secondBasis.Zx, |
| | 147 | 553 | | secondBasis, |
| | 147 | 554 | | secondLocalPoint, |
| | 147 | 555 | | secondLocalDisplacement, |
| | 147 | 556 | | secondLocalTranslation, |
| | 147 | 557 | | secondExactLocalTerm, |
| | 147 | 558 | | secondDenominator); |
| | 147 | 559 | | y = GetExactRelativeOffsetNumerator( |
| | 147 | 560 | | firstOrigin.Y, |
| | 147 | 561 | | firstBasis.Xy, |
| | 147 | 562 | | firstBasis.Yy, |
| | 147 | 563 | | firstBasis.Zy, |
| | 147 | 564 | | firstBasis, |
| | 147 | 565 | | firstLocalPoint, |
| | 147 | 566 | | firstLocalDisplacement, |
| | 147 | 567 | | firstLocalTranslation, |
| | 147 | 568 | | firstExactLocalTerm, |
| | 147 | 569 | | firstDenominator, |
| | 147 | 570 | | secondOrigin.Y, |
| | 147 | 571 | | secondBasis.Xy, |
| | 147 | 572 | | secondBasis.Yy, |
| | 147 | 573 | | secondBasis.Zy, |
| | 147 | 574 | | secondBasis, |
| | 147 | 575 | | secondLocalPoint, |
| | 147 | 576 | | secondLocalDisplacement, |
| | 147 | 577 | | secondLocalTranslation, |
| | 147 | 578 | | secondExactLocalTerm, |
| | 147 | 579 | | secondDenominator); |
| | 147 | 580 | | z = GetExactRelativeOffsetNumerator( |
| | 147 | 581 | | firstOrigin.Z, |
| | 147 | 582 | | firstBasis.Xz, |
| | 147 | 583 | | firstBasis.Yz, |
| | 147 | 584 | | firstBasis.Zz, |
| | 147 | 585 | | firstBasis, |
| | 147 | 586 | | firstLocalPoint, |
| | 147 | 587 | | firstLocalDisplacement, |
| | 147 | 588 | | firstLocalTranslation, |
| | 147 | 589 | | firstExactLocalTerm, |
| | 147 | 590 | | firstDenominator, |
| | 147 | 591 | | secondOrigin.Z, |
| | 147 | 592 | | secondBasis.Xz, |
| | 147 | 593 | | secondBasis.Yz, |
| | 147 | 594 | | secondBasis.Zz, |
| | 147 | 595 | | secondBasis, |
| | 147 | 596 | | secondLocalPoint, |
| | 147 | 597 | | secondLocalDisplacement, |
| | 147 | 598 | | secondLocalTranslation, |
| | 147 | 599 | | secondExactLocalTerm, |
| | 147 | 600 | | secondDenominator); |
| | 147 | 601 | | } |
| | | 602 | | |
| | | 603 | | private static Signed576 GetExactRelativeOffsetNumerator( |
| | | 604 | | Fixed64 firstOrigin, |
| | | 605 | | Signed192 firstAxisX, |
| | | 606 | | Signed192 firstAxisY, |
| | | 607 | | Signed192 firstAxisZ, |
| | | 608 | | WideRationalBasis3d firstBasis, |
| | | 609 | | Vector3d firstLocalPoint, |
| | | 610 | | Vector3d firstLocalDisplacement, |
| | | 611 | | Vector3d firstLocalTranslation, |
| | | 612 | | FixedPointAnchorTerm3d firstExactLocalTerm, |
| | | 613 | | Signed320 firstDenominator, |
| | | 614 | | Fixed64 secondOrigin, |
| | | 615 | | Signed192 secondAxisX, |
| | | 616 | | Signed192 secondAxisY, |
| | | 617 | | Signed192 secondAxisZ, |
| | | 618 | | WideRationalBasis3d secondBasis, |
| | | 619 | | Vector3d secondLocalPoint, |
| | | 620 | | Vector3d secondLocalDisplacement, |
| | | 621 | | Vector3d secondLocalTranslation, |
| | | 622 | | FixedPointAnchorTerm3d secondExactLocalTerm, |
| | | 623 | | Signed320 secondDenominator) => |
| | 441 | 624 | | WideArithmetic.SubtractSigned576( |
| | 441 | 625 | | WideArithmetic.MultiplySigned320( |
| | 441 | 626 | | GetAnchorCoordinateNumerator( |
| | 441 | 627 | | firstOrigin, |
| | 441 | 628 | | firstAxisX, |
| | 441 | 629 | | firstAxisY, |
| | 441 | 630 | | firstAxisZ, |
| | 441 | 631 | | firstBasis, |
| | 441 | 632 | | firstLocalPoint, |
| | 441 | 633 | | firstLocalDisplacement, |
| | 441 | 634 | | firstLocalTranslation, |
| | 441 | 635 | | firstExactLocalTerm), |
| | 441 | 636 | | secondDenominator), |
| | 441 | 637 | | WideArithmetic.MultiplySigned320( |
| | 441 | 638 | | GetAnchorCoordinateNumerator( |
| | 441 | 639 | | secondOrigin, |
| | 441 | 640 | | secondAxisX, |
| | 441 | 641 | | secondAxisY, |
| | 441 | 642 | | secondAxisZ, |
| | 441 | 643 | | secondBasis, |
| | 441 | 644 | | secondLocalPoint, |
| | 441 | 645 | | secondLocalDisplacement, |
| | 441 | 646 | | secondLocalTranslation, |
| | 441 | 647 | | secondExactLocalTerm), |
| | 441 | 648 | | firstDenominator)); |
| | | 649 | | |
| | | 650 | | private static Signed320 GetAnchorCoordinateNumerator( |
| | | 651 | | Fixed64 origin, |
| | | 652 | | Signed192 axisX, |
| | | 653 | | Signed192 axisY, |
| | | 654 | | Signed192 axisZ, |
| | | 655 | | WideRationalBasis3d basis, |
| | | 656 | | Vector3d localPoint, |
| | | 657 | | Vector3d localDisplacement, |
| | | 658 | | Vector3d localTranslation, |
| | | 659 | | FixedPointAnchorTerm3d exactLocalTerm) |
| | | 660 | | { |
| | 2301 | 661 | | Signed320 roundedNumerator = WideArithmetic.AddSigned320( |
| | 2301 | 662 | | WideArithmetic.MultiplySigned192( |
| | 2301 | 663 | | Signed192.Raw(origin), |
| | 2301 | 664 | | basis.Denominator), |
| | 2301 | 665 | | WideArithmetic.AddSigned320( |
| | 2301 | 666 | | WideArithmetic.AddSigned320( |
| | 2301 | 667 | | WideArithmetic.GetDotProduct3D( |
| | 2301 | 668 | | Signed192.Raw(localPoint.X), |
| | 2301 | 669 | | Signed192.Raw(localPoint.Y), |
| | 2301 | 670 | | Signed192.Raw(localPoint.Z), |
| | 2301 | 671 | | axisX, |
| | 2301 | 672 | | axisY, |
| | 2301 | 673 | | axisZ), |
| | 2301 | 674 | | WideArithmetic.GetDotProduct3D( |
| | 2301 | 675 | | Signed192.Raw(localDisplacement.X), |
| | 2301 | 676 | | Signed192.Raw(localDisplacement.Y), |
| | 2301 | 677 | | Signed192.Raw(localDisplacement.Z), |
| | 2301 | 678 | | axisX, |
| | 2301 | 679 | | axisY, |
| | 2301 | 680 | | axisZ)), |
| | 2301 | 681 | | WideArithmetic.GetDotProduct3D( |
| | 2301 | 682 | | Signed192.Raw(localTranslation.X), |
| | 2301 | 683 | | Signed192.Raw(localTranslation.Y), |
| | 2301 | 684 | | Signed192.Raw(localTranslation.Z), |
| | 2301 | 685 | | axisX, |
| | 2301 | 686 | | axisY, |
| | 2301 | 687 | | axisZ))); |
| | 2301 | 688 | | Signed576 scaledRounded = WideArithmetic.MultiplySigned320( |
| | 2301 | 689 | | roundedNumerator, |
| | 2301 | 690 | | Signed320.ExtendValue( |
| | 2301 | 691 | | FixedPointAnchorTerm3d.Denominator)); |
| | 2301 | 692 | | Signed320 residualProjection = WideArithmetic.GetDotProduct3D( |
| | 2301 | 693 | | Signed192.Signed(exactLocalTerm.X), |
| | 2301 | 694 | | Signed192.Signed(exactLocalTerm.Y), |
| | 2301 | 695 | | Signed192.Signed(exactLocalTerm.Z), |
| | 2301 | 696 | | axisX, |
| | 2301 | 697 | | axisY, |
| | 2301 | 698 | | axisZ); |
| | 2301 | 699 | | return WideArithmetic.AddSigned320( |
| | 2301 | 700 | | Signed320.NarrowValue(scaledRounded), |
| | 2301 | 701 | | residualProjection); |
| | | 702 | | } |
| | | 703 | | |
| | | 704 | | private static Signed320 GetAnchorDenominator(WideRationalBasis3d basis) => |
| | 769 | 705 | | WideArithmetic.MultiplySigned192( |
| | 769 | 706 | | basis.Denominator, |
| | 769 | 707 | | FixedPointAnchorTerm3d.Denominator); |
| | | 708 | | |
| | | 709 | | private static Signed320 GetAnchorDeltaNumerator( |
| | | 710 | | Fixed64 pointOrigin, |
| | | 711 | | Fixed64 frameOrigin, |
| | | 712 | | Signed192 axisX, |
| | | 713 | | Signed192 axisY, |
| | | 714 | | Signed192 axisZ, |
| | | 715 | | WideRationalBasis3d pointBasis, |
| | | 716 | | Vector3d pointLocalPoint, |
| | | 717 | | Vector3d pointLocalDisplacement, |
| | | 718 | | Vector3d pointLocalTranslation, |
| | | 719 | | FixedPointAnchorTerm3d exactLocalTerm, |
| | | 720 | | Signed320 pointDenominator) => |
| | 1089 | 721 | | WideArithmetic.SubtractSigned320( |
| | 1089 | 722 | | GetAnchorCoordinateNumerator( |
| | 1089 | 723 | | pointOrigin, |
| | 1089 | 724 | | axisX, |
| | 1089 | 725 | | axisY, |
| | 1089 | 726 | | axisZ, |
| | 1089 | 727 | | pointBasis, |
| | 1089 | 728 | | pointLocalPoint, |
| | 1089 | 729 | | pointLocalDisplacement, |
| | 1089 | 730 | | pointLocalTranslation, |
| | 1089 | 731 | | exactLocalTerm), |
| | 1089 | 732 | | WideArithmetic.MultiplySigned192( |
| | 1089 | 733 | | Signed192.Raw(frameOrigin), |
| | 1089 | 734 | | Signed192.NarrowValue(pointDenominator))); |
| | | 735 | | |
| | | 736 | | private static Signed576 GetWideProjection( |
| | | 737 | | Signed320 x, |
| | | 738 | | Signed320 y, |
| | | 739 | | Signed320 z, |
| | | 740 | | Signed192 axisX, |
| | | 741 | | Signed192 axisY, |
| | | 742 | | Signed192 axisZ) => |
| | 36 | 743 | | WideArithmetic.AddSigned576( |
| | 36 | 744 | | WideArithmetic.AddSigned576( |
| | 36 | 745 | | WideArithmetic.MultiplySigned320( |
| | 36 | 746 | | x, |
| | 36 | 747 | | Signed320.ExtendValue(axisX)), |
| | 36 | 748 | | WideArithmetic.MultiplySigned320( |
| | 36 | 749 | | y, |
| | 36 | 750 | | Signed320.ExtendValue(axisY))), |
| | 36 | 751 | | WideArithmetic.MultiplySigned320( |
| | 36 | 752 | | z, |
| | 36 | 753 | | Signed320.ExtendValue(axisZ))); |
| | | 754 | | |
| | | 755 | | private static bool TryGetScaledExactCoordinate( |
| | | 756 | | Signed576 numerator, |
| | | 757 | | Signed576 denominator, |
| | | 758 | | Fixed64 scale, |
| | | 759 | | out Fixed64 coordinate) |
| | | 760 | | { |
| | 156 | 761 | | if (scale == Fixed64.One) |
| | | 762 | | { |
| | 144 | 763 | | return Fixed64.TryGetSignedRawRatio( |
| | 144 | 764 | | numerator, |
| | 144 | 765 | | denominator, |
| | 144 | 766 | | out coordinate); |
| | | 767 | | } |
| | 12 | 768 | | return Fixed64.TryGetSignedRawRatio( |
| | 12 | 769 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 12 | 770 | | numerator, |
| | 12 | 771 | | Signed320.ExtendValue(Signed192.Raw(scale))), |
| | 12 | 772 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 12 | 773 | | denominator, |
| | 12 | 774 | | Signed320.ExtendValue(Signed192.One)), |
| | 12 | 775 | | out coordinate); |
| | | 776 | | } |
| | | 777 | | } |