| | | 1 | | //======================================================================= |
| | | 2 | | // WidePointAnchor3d.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | namespace FixedMathSharp.Geometry; |
| | | 9 | | |
| | | 10 | | /// <summary> |
| | | 11 | | /// Provides high-precision (wide/extended) arithmetic helpers for resolving anchored 3D points |
| | | 12 | | /// and their relative offsets under rotation, avoiding precision loss from repeated fixed-point operations. |
| | | 13 | | /// </summary> |
| | | 14 | | internal static partial class WidePointAnchor3d |
| | | 15 | | { |
| | | 16 | | internal static bool TryGetPoint( |
| | | 17 | | Vector3d origin, |
| | | 18 | | FixedQuaternion rotation, |
| | | 19 | | Vector3d localPoint, |
| | | 20 | | Vector3d localDisplacement, |
| | | 21 | | out Vector3d point) |
| | | 22 | | { |
| | 604 | 23 | | WideRationalBasis3d basis = new(rotation); |
| | 604 | 24 | | Signed576 denominator = |
| | 604 | 25 | | Signed576.ExtendValue( |
| | 604 | 26 | | Signed320.ExtendValue(basis.Denominator)); |
| | 604 | 27 | | bool representable = TryGetPointCoordinate( |
| | 604 | 28 | | origin.X, |
| | 604 | 29 | | basis.Xx, |
| | 604 | 30 | | basis.Yx, |
| | 604 | 31 | | basis.Zx, |
| | 604 | 32 | | basis.Denominator, |
| | 604 | 33 | | denominator, |
| | 604 | 34 | | localPoint, |
| | 604 | 35 | | localDisplacement, |
| | 604 | 36 | | out Fixed64 x) |
| | 604 | 37 | | & TryGetPointCoordinate( |
| | 604 | 38 | | origin.Y, |
| | 604 | 39 | | basis.Xy, |
| | 604 | 40 | | basis.Yy, |
| | 604 | 41 | | basis.Zy, |
| | 604 | 42 | | basis.Denominator, |
| | 604 | 43 | | denominator, |
| | 604 | 44 | | localPoint, |
| | 604 | 45 | | localDisplacement, |
| | 604 | 46 | | out Fixed64 y) |
| | 604 | 47 | | & TryGetPointCoordinate( |
| | 604 | 48 | | origin.Z, |
| | 604 | 49 | | basis.Xz, |
| | 604 | 50 | | basis.Yz, |
| | 604 | 51 | | basis.Zz, |
| | 604 | 52 | | basis.Denominator, |
| | 604 | 53 | | denominator, |
| | 604 | 54 | | localPoint, |
| | 604 | 55 | | localDisplacement, |
| | 604 | 56 | | out Fixed64 z); |
| | 604 | 57 | | if (!representable) |
| | | 58 | | { |
| | 32 | 59 | | point = default; |
| | 32 | 60 | | return false; |
| | | 61 | | } |
| | | 62 | | |
| | 572 | 63 | | point = new Vector3d(x, y, z); |
| | 572 | 64 | | return true; |
| | | 65 | | } |
| | | 66 | | |
| | | 67 | | internal static bool TryGetRelativeOffset( |
| | | 68 | | Vector3d firstOrigin, |
| | | 69 | | FixedQuaternion firstRotation, |
| | | 70 | | Vector3d firstLocalPoint, |
| | | 71 | | Vector3d firstLocalDisplacement, |
| | | 72 | | Vector3d secondOrigin, |
| | | 73 | | FixedQuaternion secondRotation, |
| | | 74 | | Vector3d secondLocalPoint, |
| | | 75 | | Vector3d secondLocalDisplacement, |
| | | 76 | | Fixed64 scale, |
| | | 77 | | out Vector3d offset) |
| | | 78 | | { |
| | 1396 | 79 | | if (firstOrigin == secondOrigin |
| | 1396 | 80 | | && firstRotation == secondRotation) |
| | | 81 | | { |
| | | 82 | | bool localRepresentable; |
| | | 83 | | Vector3d localOffset; |
| | 1049 | 84 | | if (firstLocalDisplacement == Vector3d.Zero |
| | 1049 | 85 | | && secondLocalDisplacement == Vector3d.Zero) |
| | | 86 | | { |
| | 1028 | 87 | | localRepresentable = Vector3d.TrySubtract( |
| | 1028 | 88 | | firstLocalPoint, |
| | 1028 | 89 | | secondLocalPoint, |
| | 1028 | 90 | | out localOffset); |
| | | 91 | | } |
| | | 92 | | else |
| | | 93 | | { |
| | 21 | 94 | | localRepresentable = Vector3d.TrySubtractSums( |
| | 21 | 95 | | firstLocalPoint, |
| | 21 | 96 | | firstLocalDisplacement, |
| | 21 | 97 | | secondLocalPoint, |
| | 21 | 98 | | secondLocalDisplacement, |
| | 21 | 99 | | out localOffset); |
| | | 100 | | } |
| | | 101 | | |
| | 1049 | 102 | | if (localRepresentable) |
| | | 103 | | { |
| | 1048 | 104 | | if (firstRotation != FixedQuaternion.Identity) |
| | | 105 | | { |
| | 3 | 106 | | if (scale == Fixed64.One) |
| | 2 | 107 | | return firstRotation.TryRotate(localOffset, out offset); |
| | | 108 | | } |
| | | 109 | | else |
| | | 110 | | { |
| | 1045 | 111 | | return TryScaleOffset(localOffset, scale, out offset); |
| | | 112 | | } |
| | | 113 | | } |
| | | 114 | | } |
| | | 115 | | |
| | 349 | 116 | | if (firstRotation == FixedQuaternion.Identity |
| | 349 | 117 | | && secondRotation == FixedQuaternion.Identity) |
| | | 118 | | { |
| | 68 | 119 | | bool compactRepresentable = Vector3d.TryAddSubtract( |
| | 68 | 120 | | firstOrigin, |
| | 68 | 121 | | firstLocalPoint, |
| | 68 | 122 | | secondOrigin, |
| | 68 | 123 | | out Vector3d firstStep); |
| | 68 | 124 | | compactRepresentable &= Vector3d.TryAddSubtract( |
| | 68 | 125 | | firstStep, |
| | 68 | 126 | | firstLocalDisplacement, |
| | 68 | 127 | | secondLocalPoint, |
| | 68 | 128 | | out Vector3d secondStep); |
| | 68 | 129 | | compactRepresentable &= Vector3d.TrySubtract( |
| | 68 | 130 | | secondStep, |
| | 68 | 131 | | secondLocalDisplacement, |
| | 68 | 132 | | out Vector3d compactOffset); |
| | 68 | 133 | | if (compactRepresentable) |
| | 55 | 134 | | return TryScaleOffset(compactOffset, scale, out offset); |
| | | 135 | | } |
| | | 136 | | |
| | 294 | 137 | | WideRationalBasis3d firstBasis = new(firstRotation); |
| | 294 | 138 | | WideRationalBasis3d secondBasis = new(secondRotation); |
| | 294 | 139 | | Signed320 denominator = WideArithmetic.MultiplySigned192( |
| | 294 | 140 | | firstBasis.Denominator, |
| | 294 | 141 | | secondBasis.Denominator); |
| | 294 | 142 | | Signed576 denominatorWide = Signed576.ExtendValue(denominator); |
| | 294 | 143 | | bool representable = TryGetRelativeOffsetCoordinate( |
| | 294 | 144 | | firstOrigin.X, |
| | 294 | 145 | | firstBasis.Xx, |
| | 294 | 146 | | firstBasis.Yx, |
| | 294 | 147 | | firstBasis.Zx, |
| | 294 | 148 | | firstBasis.Denominator, |
| | 294 | 149 | | firstLocalPoint, |
| | 294 | 150 | | firstLocalDisplacement, |
| | 294 | 151 | | secondOrigin.X, |
| | 294 | 152 | | secondBasis.Xx, |
| | 294 | 153 | | secondBasis.Yx, |
| | 294 | 154 | | secondBasis.Zx, |
| | 294 | 155 | | secondBasis.Denominator, |
| | 294 | 156 | | secondLocalPoint, |
| | 294 | 157 | | secondLocalDisplacement, |
| | 294 | 158 | | denominator, |
| | 294 | 159 | | denominatorWide, |
| | 294 | 160 | | scale, |
| | 294 | 161 | | out Fixed64 x) |
| | 294 | 162 | | & TryGetRelativeOffsetCoordinate( |
| | 294 | 163 | | firstOrigin.Y, |
| | 294 | 164 | | firstBasis.Xy, |
| | 294 | 165 | | firstBasis.Yy, |
| | 294 | 166 | | firstBasis.Zy, |
| | 294 | 167 | | firstBasis.Denominator, |
| | 294 | 168 | | firstLocalPoint, |
| | 294 | 169 | | firstLocalDisplacement, |
| | 294 | 170 | | secondOrigin.Y, |
| | 294 | 171 | | secondBasis.Xy, |
| | 294 | 172 | | secondBasis.Yy, |
| | 294 | 173 | | secondBasis.Zy, |
| | 294 | 174 | | secondBasis.Denominator, |
| | 294 | 175 | | secondLocalPoint, |
| | 294 | 176 | | secondLocalDisplacement, |
| | 294 | 177 | | denominator, |
| | 294 | 178 | | denominatorWide, |
| | 294 | 179 | | scale, |
| | 294 | 180 | | out Fixed64 y) |
| | 294 | 181 | | & TryGetRelativeOffsetCoordinate( |
| | 294 | 182 | | firstOrigin.Z, |
| | 294 | 183 | | firstBasis.Xz, |
| | 294 | 184 | | firstBasis.Yz, |
| | 294 | 185 | | firstBasis.Zz, |
| | 294 | 186 | | firstBasis.Denominator, |
| | 294 | 187 | | firstLocalPoint, |
| | 294 | 188 | | firstLocalDisplacement, |
| | 294 | 189 | | secondOrigin.Z, |
| | 294 | 190 | | secondBasis.Xz, |
| | 294 | 191 | | secondBasis.Yz, |
| | 294 | 192 | | secondBasis.Zz, |
| | 294 | 193 | | secondBasis.Denominator, |
| | 294 | 194 | | secondLocalPoint, |
| | 294 | 195 | | secondLocalDisplacement, |
| | 294 | 196 | | denominator, |
| | 294 | 197 | | denominatorWide, |
| | 294 | 198 | | scale, |
| | 294 | 199 | | out Fixed64 z); |
| | 294 | 200 | | if (!representable) |
| | | 201 | | { |
| | 8 | 202 | | offset = default; |
| | 8 | 203 | | return false; |
| | | 204 | | } |
| | | 205 | | |
| | 286 | 206 | | offset = new Vector3d(x, y, z); |
| | 286 | 207 | | return true; |
| | | 208 | | } |
| | | 209 | | |
| | | 210 | | private static bool TryScaleOffset( |
| | | 211 | | Vector3d value, |
| | | 212 | | Fixed64 scale, |
| | | 213 | | out Vector3d result) |
| | | 214 | | { |
| | 1100 | 215 | | if (scale == Fixed64.One) |
| | | 216 | | { |
| | 1094 | 217 | | result = value; |
| | 1094 | 218 | | return true; |
| | | 219 | | } |
| | | 220 | | |
| | 6 | 221 | | bool representable = Fixed64.TryMultiplyDivide( |
| | 6 | 222 | | value.X, |
| | 6 | 223 | | scale, |
| | 6 | 224 | | Fixed64.One, |
| | 6 | 225 | | out Fixed64 x) |
| | 6 | 226 | | & Fixed64.TryMultiplyDivide( |
| | 6 | 227 | | value.Y, |
| | 6 | 228 | | scale, |
| | 6 | 229 | | Fixed64.One, |
| | 6 | 230 | | out Fixed64 y) |
| | 6 | 231 | | & Fixed64.TryMultiplyDivide( |
| | 6 | 232 | | value.Z, |
| | 6 | 233 | | scale, |
| | 6 | 234 | | Fixed64.One, |
| | 6 | 235 | | out Fixed64 z); |
| | 6 | 236 | | result = representable ? new Vector3d(x, y, z) : default; |
| | 6 | 237 | | return representable; |
| | | 238 | | } |
| | | 239 | | |
| | | 240 | | internal static bool TryGetLocalPointIn( |
| | | 241 | | Vector3d pointOrigin, |
| | | 242 | | FixedQuaternion pointRotation, |
| | | 243 | | Vector3d pointLocalPoint, |
| | | 244 | | Vector3d pointLocalDisplacement, |
| | | 245 | | Vector3d frameOrigin, |
| | | 246 | | FixedQuaternion frameRotation, |
| | | 247 | | out Vector3d localPoint) |
| | | 248 | | { |
| | 4475 | 249 | | WideRationalBasis3d pointBasis = new(pointRotation); |
| | 4475 | 250 | | WideRationalBasis3d frameBasis = new(frameRotation); |
| | 4475 | 251 | | Signed320 denominator = WideArithmetic.MultiplySigned192( |
| | 4475 | 252 | | pointBasis.Denominator, |
| | 4475 | 253 | | frameBasis.Denominator); |
| | 4475 | 254 | | Signed576 denominatorWide = Signed576.ExtendValue(denominator); |
| | 4475 | 255 | | Signed192 originX = WideArithmetic.SubtractSigned192( |
| | 4475 | 256 | | Signed192.Raw(pointOrigin.X), |
| | 4475 | 257 | | Signed192.Raw(frameOrigin.X)); |
| | 4475 | 258 | | Signed192 originY = WideArithmetic.SubtractSigned192( |
| | 4475 | 259 | | Signed192.Raw(pointOrigin.Y), |
| | 4475 | 260 | | Signed192.Raw(frameOrigin.Y)); |
| | 4475 | 261 | | Signed192 originZ = WideArithmetic.SubtractSigned192( |
| | 4475 | 262 | | Signed192.Raw(pointOrigin.Z), |
| | 4475 | 263 | | Signed192.Raw(frameOrigin.Z)); |
| | 4475 | 264 | | Signed320 rotatedX = WideArithmetic.GetDotProduct3D( |
| | 4475 | 265 | | Signed192.Raw(pointLocalPoint.X), |
| | 4475 | 266 | | Signed192.Raw(pointLocalPoint.Y), |
| | 4475 | 267 | | Signed192.Raw(pointLocalPoint.Z), |
| | 4475 | 268 | | pointBasis.Xx, |
| | 4475 | 269 | | pointBasis.Yx, |
| | 4475 | 270 | | pointBasis.Zx); |
| | 4475 | 271 | | rotatedX = WideArithmetic.AddSigned320( |
| | 4475 | 272 | | rotatedX, |
| | 4475 | 273 | | WideArithmetic.GetDotProduct3D( |
| | 4475 | 274 | | Signed192.Raw(pointLocalDisplacement.X), |
| | 4475 | 275 | | Signed192.Raw(pointLocalDisplacement.Y), |
| | 4475 | 276 | | Signed192.Raw(pointLocalDisplacement.Z), |
| | 4475 | 277 | | pointBasis.Xx, |
| | 4475 | 278 | | pointBasis.Yx, |
| | 4475 | 279 | | pointBasis.Zx)); |
| | 4475 | 280 | | Signed320 rotatedY = WideArithmetic.GetDotProduct3D( |
| | 4475 | 281 | | Signed192.Raw(pointLocalPoint.X), |
| | 4475 | 282 | | Signed192.Raw(pointLocalPoint.Y), |
| | 4475 | 283 | | Signed192.Raw(pointLocalPoint.Z), |
| | 4475 | 284 | | pointBasis.Xy, |
| | 4475 | 285 | | pointBasis.Yy, |
| | 4475 | 286 | | pointBasis.Zy); |
| | 4475 | 287 | | rotatedY = WideArithmetic.AddSigned320( |
| | 4475 | 288 | | rotatedY, |
| | 4475 | 289 | | WideArithmetic.GetDotProduct3D( |
| | 4475 | 290 | | Signed192.Raw(pointLocalDisplacement.X), |
| | 4475 | 291 | | Signed192.Raw(pointLocalDisplacement.Y), |
| | 4475 | 292 | | Signed192.Raw(pointLocalDisplacement.Z), |
| | 4475 | 293 | | pointBasis.Xy, |
| | 4475 | 294 | | pointBasis.Yy, |
| | 4475 | 295 | | pointBasis.Zy)); |
| | 4475 | 296 | | Signed320 rotatedZ = WideArithmetic.GetDotProduct3D( |
| | 4475 | 297 | | Signed192.Raw(pointLocalPoint.X), |
| | 4475 | 298 | | Signed192.Raw(pointLocalPoint.Y), |
| | 4475 | 299 | | Signed192.Raw(pointLocalPoint.Z), |
| | 4475 | 300 | | pointBasis.Xz, |
| | 4475 | 301 | | pointBasis.Yz, |
| | 4475 | 302 | | pointBasis.Zz); |
| | 4475 | 303 | | rotatedZ = WideArithmetic.AddSigned320( |
| | 4475 | 304 | | rotatedZ, |
| | 4475 | 305 | | WideArithmetic.GetDotProduct3D( |
| | 4475 | 306 | | Signed192.Raw(pointLocalDisplacement.X), |
| | 4475 | 307 | | Signed192.Raw(pointLocalDisplacement.Y), |
| | 4475 | 308 | | Signed192.Raw(pointLocalDisplacement.Z), |
| | 4475 | 309 | | pointBasis.Xz, |
| | 4475 | 310 | | pointBasis.Yz, |
| | 4475 | 311 | | pointBasis.Zz)); |
| | | 312 | | |
| | 4475 | 313 | | bool representable = TryGetLocalPointCoordinate( |
| | 4475 | 314 | | originX, |
| | 4475 | 315 | | originY, |
| | 4475 | 316 | | originZ, |
| | 4475 | 317 | | rotatedX, |
| | 4475 | 318 | | rotatedY, |
| | 4475 | 319 | | rotatedZ, |
| | 4475 | 320 | | pointBasis.Denominator, |
| | 4475 | 321 | | frameBasis.Xx, |
| | 4475 | 322 | | frameBasis.Xy, |
| | 4475 | 323 | | frameBasis.Xz, |
| | 4475 | 324 | | denominatorWide, |
| | 4475 | 325 | | out Fixed64 x) |
| | 4475 | 326 | | & TryGetLocalPointCoordinate( |
| | 4475 | 327 | | originX, |
| | 4475 | 328 | | originY, |
| | 4475 | 329 | | originZ, |
| | 4475 | 330 | | rotatedX, |
| | 4475 | 331 | | rotatedY, |
| | 4475 | 332 | | rotatedZ, |
| | 4475 | 333 | | pointBasis.Denominator, |
| | 4475 | 334 | | frameBasis.Yx, |
| | 4475 | 335 | | frameBasis.Yy, |
| | 4475 | 336 | | frameBasis.Yz, |
| | 4475 | 337 | | denominatorWide, |
| | 4475 | 338 | | out Fixed64 y) |
| | 4475 | 339 | | & TryGetLocalPointCoordinate( |
| | 4475 | 340 | | originX, |
| | 4475 | 341 | | originY, |
| | 4475 | 342 | | originZ, |
| | 4475 | 343 | | rotatedX, |
| | 4475 | 344 | | rotatedY, |
| | 4475 | 345 | | rotatedZ, |
| | 4475 | 346 | | pointBasis.Denominator, |
| | 4475 | 347 | | frameBasis.Zx, |
| | 4475 | 348 | | frameBasis.Zy, |
| | 4475 | 349 | | frameBasis.Zz, |
| | 4475 | 350 | | denominatorWide, |
| | 4475 | 351 | | out Fixed64 z); |
| | 4475 | 352 | | if (!representable) |
| | | 353 | | { |
| | 15 | 354 | | localPoint = default; |
| | 15 | 355 | | return false; |
| | | 356 | | } |
| | | 357 | | |
| | 4460 | 358 | | localPoint = new Vector3d(x, y, z); |
| | 4460 | 359 | | return true; |
| | | 360 | | } |
| | | 361 | | |
| | | 362 | | internal static bool TryGetProjectedOffset( |
| | | 363 | | Vector3d firstOrigin, |
| | | 364 | | FixedQuaternion firstRotation, |
| | | 365 | | Vector3d firstLocalPoint, |
| | | 366 | | Vector3d firstLocalDisplacement, |
| | | 367 | | Vector3d secondOrigin, |
| | | 368 | | FixedQuaternion secondRotation, |
| | | 369 | | Vector3d secondLocalPoint, |
| | | 370 | | Vector3d secondLocalDisplacement, |
| | | 371 | | Vector3d direction, |
| | | 372 | | out Fixed64 projection) |
| | | 373 | | { |
| | 261 | 374 | | GetProjectedOffsetRatio( |
| | 261 | 375 | | firstOrigin, |
| | 261 | 376 | | firstRotation, |
| | 261 | 377 | | firstLocalPoint, |
| | 261 | 378 | | firstLocalDisplacement, |
| | 261 | 379 | | secondOrigin, |
| | 261 | 380 | | secondRotation, |
| | 261 | 381 | | secondLocalPoint, |
| | 261 | 382 | | secondLocalDisplacement, |
| | 261 | 383 | | direction, |
| | 261 | 384 | | out Signed704 numerator, |
| | 261 | 385 | | out Signed704 denominator); |
| | 261 | 386 | | return Fixed64.TryGetSignedRawRatio( |
| | 261 | 387 | | numerator, |
| | 261 | 388 | | denominator, |
| | 261 | 389 | | out projection); |
| | | 390 | | } |
| | | 391 | | |
| | | 392 | | internal static Fixed64 ProjectNonNegativeOffset( |
| | | 393 | | Vector3d firstOrigin, |
| | | 394 | | FixedQuaternion firstRotation, |
| | | 395 | | Vector3d firstLocalPoint, |
| | | 396 | | Vector3d firstLocalDisplacement, |
| | | 397 | | Vector3d secondOrigin, |
| | | 398 | | FixedQuaternion secondRotation, |
| | | 399 | | Vector3d secondLocalPoint, |
| | | 400 | | Vector3d secondLocalDisplacement, |
| | | 401 | | Vector3d direction) |
| | | 402 | | { |
| | 259 | 403 | | GetProjectedOffsetRatio( |
| | 259 | 404 | | firstOrigin, |
| | 259 | 405 | | firstRotation, |
| | 259 | 406 | | firstLocalPoint, |
| | 259 | 407 | | firstLocalDisplacement, |
| | 259 | 408 | | secondOrigin, |
| | 259 | 409 | | secondRotation, |
| | 259 | 410 | | secondLocalPoint, |
| | 259 | 411 | | secondLocalDisplacement, |
| | 259 | 412 | | direction, |
| | 259 | 413 | | out Signed704 numerator, |
| | 259 | 414 | | out Signed704 denominator); |
| | 259 | 415 | | return Fixed64.GetNonNegativeRawRatioFloor( |
| | 259 | 416 | | numerator, |
| | 259 | 417 | | denominator); |
| | | 418 | | } |
| | | 419 | | |
| | | 420 | | internal static void GetProjectedOffsetRatio( |
| | | 421 | | in FixedPointAnchor first, |
| | | 422 | | in FixedPointAnchor second, |
| | | 423 | | Vector3d direction, |
| | | 424 | | out Signed704 numerator, |
| | | 425 | | out Signed704 denominator) |
| | | 426 | | { |
| | 155 | 427 | | if (!first.ExactLocalTerm.IsZero |
| | 155 | 428 | | || first.LocalTranslation != Vector3d.Zero) |
| | | 429 | | { |
| | 78 | 430 | | GetExactProjectedOffsetRatio( |
| | 78 | 431 | | first.Origin, |
| | 78 | 432 | | first.Rotation, |
| | 78 | 433 | | first.LocalPoint, |
| | 78 | 434 | | first.LocalDisplacement, |
| | 78 | 435 | | first.LocalTranslation, |
| | 78 | 436 | | first.ExactLocalTerm, |
| | 78 | 437 | | second.Origin, |
| | 78 | 438 | | second.Rotation, |
| | 78 | 439 | | second.LocalPoint, |
| | 78 | 440 | | second.LocalDisplacement, |
| | 78 | 441 | | second.LocalTranslation, |
| | 78 | 442 | | second.ExactLocalTerm, |
| | 78 | 443 | | direction, |
| | 78 | 444 | | out numerator, |
| | 78 | 445 | | out denominator); |
| | 78 | 446 | | return; |
| | | 447 | | } |
| | | 448 | | |
| | 77 | 449 | | GetProjectedOffsetRatio( |
| | 77 | 450 | | first.Origin, |
| | 77 | 451 | | first.Rotation, |
| | 77 | 452 | | first.LocalPoint, |
| | 77 | 453 | | first.LocalDisplacement, |
| | 77 | 454 | | second.Origin, |
| | 77 | 455 | | second.Rotation, |
| | 77 | 456 | | second.LocalPoint, |
| | 77 | 457 | | second.LocalDisplacement, |
| | 77 | 458 | | direction, |
| | 77 | 459 | | out numerator, |
| | 77 | 460 | | out denominator); |
| | 77 | 461 | | } |
| | | 462 | | |
| | | 463 | | private static void GetProjectedOffsetRatio( |
| | | 464 | | Vector3d firstOrigin, |
| | | 465 | | FixedQuaternion firstRotation, |
| | | 466 | | Vector3d firstLocalPoint, |
| | | 467 | | Vector3d firstLocalDisplacement, |
| | | 468 | | Vector3d secondOrigin, |
| | | 469 | | FixedQuaternion secondRotation, |
| | | 470 | | Vector3d secondLocalPoint, |
| | | 471 | | Vector3d secondLocalDisplacement, |
| | | 472 | | Vector3d direction, |
| | | 473 | | out Signed704 numerator, |
| | | 474 | | out Signed704 denominator) |
| | | 475 | | { |
| | 597 | 476 | | WideRationalBasis3d firstBasis = new(firstRotation); |
| | 597 | 477 | | WideRationalBasis3d secondBasis = new(secondRotation); |
| | 597 | 478 | | Signed320 coordinateDenominator = |
| | 597 | 479 | | WideArithmetic.MultiplySigned192( |
| | 597 | 480 | | firstBasis.Denominator, |
| | 597 | 481 | | secondBasis.Denominator); |
| | 597 | 482 | | Signed576 coordinateDenominatorWide = |
| | 597 | 483 | | Signed576.ExtendValue(coordinateDenominator); |
| | 597 | 484 | | Signed576 x = GetRelativeOffsetNumerator( |
| | 597 | 485 | | firstOrigin.X, |
| | 597 | 486 | | firstBasis.Xx, |
| | 597 | 487 | | firstBasis.Yx, |
| | 597 | 488 | | firstBasis.Zx, |
| | 597 | 489 | | firstBasis.Denominator, |
| | 597 | 490 | | firstLocalPoint, |
| | 597 | 491 | | firstLocalDisplacement, |
| | 597 | 492 | | secondOrigin.X, |
| | 597 | 493 | | secondBasis.Xx, |
| | 597 | 494 | | secondBasis.Yx, |
| | 597 | 495 | | secondBasis.Zx, |
| | 597 | 496 | | secondBasis.Denominator, |
| | 597 | 497 | | secondLocalPoint, |
| | 597 | 498 | | secondLocalDisplacement, |
| | 597 | 499 | | coordinateDenominator); |
| | 597 | 500 | | Signed576 y = GetRelativeOffsetNumerator( |
| | 597 | 501 | | firstOrigin.Y, |
| | 597 | 502 | | firstBasis.Xy, |
| | 597 | 503 | | firstBasis.Yy, |
| | 597 | 504 | | firstBasis.Zy, |
| | 597 | 505 | | firstBasis.Denominator, |
| | 597 | 506 | | firstLocalPoint, |
| | 597 | 507 | | firstLocalDisplacement, |
| | 597 | 508 | | secondOrigin.Y, |
| | 597 | 509 | | secondBasis.Xy, |
| | 597 | 510 | | secondBasis.Yy, |
| | 597 | 511 | | secondBasis.Zy, |
| | 597 | 512 | | secondBasis.Denominator, |
| | 597 | 513 | | secondLocalPoint, |
| | 597 | 514 | | secondLocalDisplacement, |
| | 597 | 515 | | coordinateDenominator); |
| | 597 | 516 | | Signed576 z = GetRelativeOffsetNumerator( |
| | 597 | 517 | | firstOrigin.Z, |
| | 597 | 518 | | firstBasis.Xz, |
| | 597 | 519 | | firstBasis.Yz, |
| | 597 | 520 | | firstBasis.Zz, |
| | 597 | 521 | | firstBasis.Denominator, |
| | 597 | 522 | | firstLocalPoint, |
| | 597 | 523 | | firstLocalDisplacement, |
| | 597 | 524 | | secondOrigin.Z, |
| | 597 | 525 | | secondBasis.Xz, |
| | 597 | 526 | | secondBasis.Yz, |
| | 597 | 527 | | secondBasis.Zz, |
| | 597 | 528 | | secondBasis.Denominator, |
| | 597 | 529 | | secondLocalPoint, |
| | 597 | 530 | | secondLocalDisplacement, |
| | 597 | 531 | | coordinateDenominator); |
| | 597 | 532 | | numerator = WideArithmetic.AddSigned704( |
| | 597 | 533 | | WideArithmetic.AddSigned704( |
| | 597 | 534 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 597 | 535 | | x, |
| | 597 | 536 | | Signed320.ExtendValue(Signed192.Raw(direction.X))), |
| | 597 | 537 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 597 | 538 | | y, |
| | 597 | 539 | | Signed320.ExtendValue(Signed192.Raw(direction.Y)))), |
| | 597 | 540 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 597 | 541 | | z, |
| | 597 | 542 | | Signed320.ExtendValue(Signed192.Raw(direction.Z)))); |
| | 597 | 543 | | denominator = WideArithmetic.MultiplySigned576ToSigned704( |
| | 597 | 544 | | coordinateDenominatorWide, |
| | 597 | 545 | | Signed320.ExtendValue(Signed192.One)); |
| | 597 | 546 | | } |
| | | 547 | | |
| | | 548 | | private static bool TryGetPointCoordinate( |
| | | 549 | | Fixed64 origin, |
| | | 550 | | Signed192 axisX, |
| | | 551 | | Signed192 axisY, |
| | | 552 | | Signed192 axisZ, |
| | | 553 | | Signed192 denominator, |
| | | 554 | | Signed576 denominatorWide, |
| | | 555 | | Vector3d localPoint, |
| | | 556 | | Vector3d localDisplacement, |
| | | 557 | | out Fixed64 coordinate) |
| | | 558 | | { |
| | 1812 | 559 | | Signed320 numerator = WideArithmetic.AddSigned320( |
| | 1812 | 560 | | WideArithmetic.MultiplySigned192(Signed192.Raw(origin), denominator), |
| | 1812 | 561 | | WideArithmetic.AddSigned320( |
| | 1812 | 562 | | WideArithmetic.GetDotProduct3D( |
| | 1812 | 563 | | Signed192.Raw(localPoint.X), |
| | 1812 | 564 | | Signed192.Raw(localPoint.Y), |
| | 1812 | 565 | | Signed192.Raw(localPoint.Z), |
| | 1812 | 566 | | axisX, |
| | 1812 | 567 | | axisY, |
| | 1812 | 568 | | axisZ), |
| | 1812 | 569 | | WideArithmetic.GetDotProduct3D( |
| | 1812 | 570 | | Signed192.Raw(localDisplacement.X), |
| | 1812 | 571 | | Signed192.Raw(localDisplacement.Y), |
| | 1812 | 572 | | Signed192.Raw(localDisplacement.Z), |
| | 1812 | 573 | | axisX, |
| | 1812 | 574 | | axisY, |
| | 1812 | 575 | | axisZ))); |
| | 1812 | 576 | | return Fixed64.TryGetSignedRawRatio( |
| | 1812 | 577 | | Signed576.ExtendValue(numerator), |
| | 1812 | 578 | | denominatorWide, |
| | 1812 | 579 | | out coordinate); |
| | | 580 | | } |
| | | 581 | | |
| | | 582 | | private static bool TryGetRelativeOffsetCoordinate( |
| | | 583 | | Fixed64 firstOrigin, |
| | | 584 | | Signed192 firstAxisX, |
| | | 585 | | Signed192 firstAxisY, |
| | | 586 | | Signed192 firstAxisZ, |
| | | 587 | | Signed192 firstDenominator, |
| | | 588 | | Vector3d firstLocalPoint, |
| | | 589 | | Vector3d firstLocalDisplacement, |
| | | 590 | | Fixed64 secondOrigin, |
| | | 591 | | Signed192 secondAxisX, |
| | | 592 | | Signed192 secondAxisY, |
| | | 593 | | Signed192 secondAxisZ, |
| | | 594 | | Signed192 secondDenominator, |
| | | 595 | | Vector3d secondLocalPoint, |
| | | 596 | | Vector3d secondLocalDisplacement, |
| | | 597 | | Signed320 denominator, |
| | | 598 | | Signed576 denominatorWide, |
| | | 599 | | Fixed64 scale, |
| | | 600 | | out Fixed64 coordinate) |
| | | 601 | | { |
| | 882 | 602 | | Signed576 numerator = GetRelativeOffsetNumerator( |
| | 882 | 603 | | firstOrigin, |
| | 882 | 604 | | firstAxisX, |
| | 882 | 605 | | firstAxisY, |
| | 882 | 606 | | firstAxisZ, |
| | 882 | 607 | | firstDenominator, |
| | 882 | 608 | | firstLocalPoint, |
| | 882 | 609 | | firstLocalDisplacement, |
| | 882 | 610 | | secondOrigin, |
| | 882 | 611 | | secondAxisX, |
| | 882 | 612 | | secondAxisY, |
| | 882 | 613 | | secondAxisZ, |
| | 882 | 614 | | secondDenominator, |
| | 882 | 615 | | secondLocalPoint, |
| | 882 | 616 | | secondLocalDisplacement, |
| | 882 | 617 | | denominator); |
| | 882 | 618 | | if (scale == Fixed64.One) |
| | | 619 | | { |
| | 105 | 620 | | return Fixed64.TryGetSignedRawRatio( |
| | 105 | 621 | | numerator, |
| | 105 | 622 | | denominatorWide, |
| | 105 | 623 | | out coordinate); |
| | | 624 | | } |
| | 777 | 625 | | Signed320 scaleRaw = Signed320.ExtendValue(Signed192.Raw(scale)); |
| | 777 | 626 | | Signed320 oneRaw = Signed320.ExtendValue(Signed192.One); |
| | 777 | 627 | | return Fixed64.TryGetSignedRawRatio( |
| | 777 | 628 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 777 | 629 | | numerator, |
| | 777 | 630 | | scaleRaw), |
| | 777 | 631 | | WideArithmetic.MultiplySigned576ToSigned704( |
| | 777 | 632 | | denominatorWide, |
| | 777 | 633 | | oneRaw), |
| | 777 | 634 | | out coordinate); |
| | | 635 | | } |
| | | 636 | | |
| | | 637 | | private static Signed576 GetRelativeOffsetNumerator( |
| | | 638 | | Fixed64 firstOrigin, |
| | | 639 | | Signed192 firstAxisX, |
| | | 640 | | Signed192 firstAxisY, |
| | | 641 | | Signed192 firstAxisZ, |
| | | 642 | | Signed192 firstDenominator, |
| | | 643 | | Vector3d firstLocalPoint, |
| | | 644 | | Vector3d firstLocalDisplacement, |
| | | 645 | | Fixed64 secondOrigin, |
| | | 646 | | Signed192 secondAxisX, |
| | | 647 | | Signed192 secondAxisY, |
| | | 648 | | Signed192 secondAxisZ, |
| | | 649 | | Signed192 secondDenominator, |
| | | 650 | | Vector3d secondLocalPoint, |
| | | 651 | | Vector3d secondLocalDisplacement, |
| | | 652 | | Signed320 denominator) |
| | | 653 | | { |
| | 3663 | 654 | | Signed320 firstRotated = WideArithmetic.GetDotProduct3D( |
| | 3663 | 655 | | Signed192.Raw(firstLocalPoint.X), |
| | 3663 | 656 | | Signed192.Raw(firstLocalPoint.Y), |
| | 3663 | 657 | | Signed192.Raw(firstLocalPoint.Z), |
| | 3663 | 658 | | firstAxisX, |
| | 3663 | 659 | | firstAxisY, |
| | 3663 | 660 | | firstAxisZ); |
| | 3663 | 661 | | firstRotated = WideArithmetic.AddSigned320( |
| | 3663 | 662 | | firstRotated, |
| | 3663 | 663 | | WideArithmetic.GetDotProduct3D( |
| | 3663 | 664 | | Signed192.Raw(firstLocalDisplacement.X), |
| | 3663 | 665 | | Signed192.Raw(firstLocalDisplacement.Y), |
| | 3663 | 666 | | Signed192.Raw(firstLocalDisplacement.Z), |
| | 3663 | 667 | | firstAxisX, |
| | 3663 | 668 | | firstAxisY, |
| | 3663 | 669 | | firstAxisZ)); |
| | 3663 | 670 | | Signed320 secondRotated = WideArithmetic.GetDotProduct3D( |
| | 3663 | 671 | | Signed192.Raw(secondLocalPoint.X), |
| | 3663 | 672 | | Signed192.Raw(secondLocalPoint.Y), |
| | 3663 | 673 | | Signed192.Raw(secondLocalPoint.Z), |
| | 3663 | 674 | | secondAxisX, |
| | 3663 | 675 | | secondAxisY, |
| | 3663 | 676 | | secondAxisZ); |
| | 3663 | 677 | | secondRotated = WideArithmetic.AddSigned320( |
| | 3663 | 678 | | secondRotated, |
| | 3663 | 679 | | WideArithmetic.GetDotProduct3D( |
| | 3663 | 680 | | Signed192.Raw(secondLocalDisplacement.X), |
| | 3663 | 681 | | Signed192.Raw(secondLocalDisplacement.Y), |
| | 3663 | 682 | | Signed192.Raw(secondLocalDisplacement.Z), |
| | 3663 | 683 | | secondAxisX, |
| | 3663 | 684 | | secondAxisY, |
| | 3663 | 685 | | secondAxisZ)); |
| | 3663 | 686 | | return WideArithmetic.SubtractSigned576( |
| | 3663 | 687 | | WideArithmetic.AddSigned576( |
| | 3663 | 688 | | WideArithmetic.MultiplySigned320( |
| | 3663 | 689 | | Signed320.ExtendValue( |
| | 3663 | 690 | | WideArithmetic.SubtractSigned192( |
| | 3663 | 691 | | Signed192.Raw(firstOrigin), |
| | 3663 | 692 | | Signed192.Raw(secondOrigin))), |
| | 3663 | 693 | | denominator), |
| | 3663 | 694 | | WideArithmetic.MultiplySigned320( |
| | 3663 | 695 | | firstRotated, |
| | 3663 | 696 | | Signed320.ExtendValue(secondDenominator))), |
| | 3663 | 697 | | WideArithmetic.MultiplySigned320( |
| | 3663 | 698 | | secondRotated, |
| | 3663 | 699 | | Signed320.ExtendValue(firstDenominator))); |
| | | 700 | | } |
| | | 701 | | |
| | | 702 | | private static bool TryGetLocalPointCoordinate( |
| | | 703 | | Signed192 originX, |
| | | 704 | | Signed192 originY, |
| | | 705 | | Signed192 originZ, |
| | | 706 | | Signed320 rotatedX, |
| | | 707 | | Signed320 rotatedY, |
| | | 708 | | Signed320 rotatedZ, |
| | | 709 | | Signed192 pointDenominator, |
| | | 710 | | Signed192 frameAxisX, |
| | | 711 | | Signed192 frameAxisY, |
| | | 712 | | Signed192 frameAxisZ, |
| | | 713 | | Signed576 denominator, |
| | | 714 | | out Fixed64 coordinate) |
| | | 715 | | { |
| | 13425 | 716 | | Signed320 originProjection = WideArithmetic.GetDotProduct3D( |
| | 13425 | 717 | | originX, |
| | 13425 | 718 | | originY, |
| | 13425 | 719 | | originZ, |
| | 13425 | 720 | | frameAxisX, |
| | 13425 | 721 | | frameAxisY, |
| | 13425 | 722 | | frameAxisZ); |
| | 13425 | 723 | | Signed576 rotatedProjection = WideArithmetic.AddSigned576( |
| | 13425 | 724 | | WideArithmetic.AddSigned576( |
| | 13425 | 725 | | WideArithmetic.MultiplySigned320( |
| | 13425 | 726 | | rotatedX, |
| | 13425 | 727 | | Signed320.ExtendValue(frameAxisX)), |
| | 13425 | 728 | | WideArithmetic.MultiplySigned320( |
| | 13425 | 729 | | rotatedY, |
| | 13425 | 730 | | Signed320.ExtendValue(frameAxisY))), |
| | 13425 | 731 | | WideArithmetic.MultiplySigned320( |
| | 13425 | 732 | | rotatedZ, |
| | 13425 | 733 | | Signed320.ExtendValue(frameAxisZ))); |
| | 13425 | 734 | | Signed576 numerator = WideArithmetic.AddSigned576( |
| | 13425 | 735 | | WideArithmetic.MultiplySigned320( |
| | 13425 | 736 | | originProjection, |
| | 13425 | 737 | | Signed320.ExtendValue(pointDenominator)), |
| | 13425 | 738 | | rotatedProjection); |
| | 13425 | 739 | | return Fixed64.TryGetSignedRawRatio( |
| | 13425 | 740 | | numerator, |
| | 13425 | 741 | | denominator, |
| | 13425 | 742 | | out coordinate); |
| | | 743 | | } |
| | | 744 | | } |