| | | 1 | | //======================================================================= |
| | | 2 | | // WideVector2dTransform.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; |
| | | 9 | | |
| | | 10 | | /// <summary> |
| | | 11 | | /// Provides methods for transforming two-dimensional points and |
| | | 12 | | /// vectors using wide arithmetic to avoid overflow and maintain precision. |
| | | 13 | | /// </summary> |
| | | 14 | | internal static class WideVector2dTransform |
| | | 15 | | { |
| | | 16 | | internal static bool TryTransformScaledPoint( |
| | | 17 | | Vector2d origin, |
| | | 18 | | Vector2d localPoint, |
| | | 19 | | Vector2d scale, |
| | | 20 | | Vector2d localDisplacement, |
| | | 21 | | Fixed64 angleInRadians, |
| | | 22 | | out Vector2d result) |
| | | 23 | | { |
| | | 24 | | // Both rotated numerators are zero, so the exact final ratio is the origin. |
| | 239 | 25 | | if (localPoint == Vector2d.Zero && localDisplacement == Vector2d.Zero) |
| | | 26 | | { |
| | 168 | 27 | | result = origin; |
| | 168 | 28 | | return true; |
| | | 29 | | } |
| | | 30 | | |
| | 71 | 31 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 71 | 32 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 71 | 33 | | Signed320 denominatorWide = Product(Fixed64.One, Fixed64.One); |
| | 71 | 34 | | Signed576 denominator = |
| | 71 | 35 | | Signed576.ExtendValue(denominatorWide); |
| | 71 | 36 | | GetScaledRotationNumerators( |
| | 71 | 37 | | localPoint, |
| | 71 | 38 | | scale, |
| | 71 | 39 | | localDisplacement, |
| | 71 | 40 | | cosine, |
| | 71 | 41 | | sine, |
| | 71 | 42 | | out Signed576 rotatedX, |
| | 71 | 43 | | out Signed576 rotatedY); |
| | 71 | 44 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 71 | 45 | | WideArithmetic.AddSigned576( |
| | 71 | 46 | | WideArithmetic.MultiplySigned320( |
| | 71 | 47 | | Signed320.ExtendValue(Signed192.Raw(origin.X)), |
| | 71 | 48 | | denominatorWide), |
| | 71 | 49 | | rotatedX), |
| | 71 | 50 | | denominator, |
| | 71 | 51 | | out Fixed64 x) |
| | 71 | 52 | | & Fixed64.TryGetSignedRawRatio( |
| | 71 | 53 | | WideArithmetic.AddSigned576( |
| | 71 | 54 | | WideArithmetic.MultiplySigned320( |
| | 71 | 55 | | Signed320.ExtendValue(Signed192.Raw(origin.Y)), |
| | 71 | 56 | | denominatorWide), |
| | 71 | 57 | | rotatedY), |
| | 71 | 58 | | denominator, |
| | 71 | 59 | | out Fixed64 y); |
| | 71 | 60 | | if (!representable) |
| | | 61 | | { |
| | 2 | 62 | | result = default; |
| | 2 | 63 | | return false; |
| | | 64 | | } |
| | | 65 | | |
| | 69 | 66 | | result = new Vector2d(x, y); |
| | 69 | 67 | | return true; |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | internal static bool TryInverseTransformScaledPoint( |
| | | 71 | | Vector2d origin, |
| | | 72 | | Vector2d worldPoint, |
| | | 73 | | Vector2d scale, |
| | | 74 | | Fixed64 angleInRadians, |
| | | 75 | | out Vector2d localPoint) |
| | | 76 | | { |
| | 71 | 77 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 71 | 78 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 71 | 79 | | Signed192 offsetX = WideArithmetic.SubtractSigned192( |
| | 71 | 80 | | Signed192.Raw(worldPoint.X), |
| | 71 | 81 | | Signed192.Raw(origin.X)); |
| | 71 | 82 | | Signed192 offsetY = WideArithmetic.SubtractSigned192( |
| | 71 | 83 | | Signed192.Raw(worldPoint.Y), |
| | 71 | 84 | | Signed192.Raw(origin.Y)); |
| | 71 | 85 | | Signed320 projectionX = WideArithmetic.AddSigned320( |
| | 71 | 86 | | WideArithmetic.MultiplySigned192( |
| | 71 | 87 | | offsetX, |
| | 71 | 88 | | Signed192.Raw(cosine)), |
| | 71 | 89 | | WideArithmetic.MultiplySigned192( |
| | 71 | 90 | | offsetY, |
| | 71 | 91 | | Signed192.Raw(sine))); |
| | 71 | 92 | | Signed320 projectionY = WideArithmetic.SubtractSigned320( |
| | 71 | 93 | | WideArithmetic.MultiplySigned192( |
| | 71 | 94 | | offsetY, |
| | 71 | 95 | | Signed192.Raw(cosine)), |
| | 71 | 96 | | WideArithmetic.MultiplySigned192( |
| | 71 | 97 | | offsetX, |
| | 71 | 98 | | Signed192.Raw(sine))); |
| | 71 | 99 | | Signed320 rotationDenominator = WideArithmetic.AddSigned320( |
| | 71 | 100 | | Product(cosine, cosine), |
| | 71 | 101 | | Product(sine, sine)); |
| | 71 | 102 | | bool representable = TryInverseTransformScaledCoordinate( |
| | 71 | 103 | | projectionX, |
| | 71 | 104 | | rotationDenominator, |
| | 71 | 105 | | scale.X, |
| | 71 | 106 | | out Fixed64 x) |
| | 71 | 107 | | & TryInverseTransformScaledCoordinate( |
| | 71 | 108 | | projectionY, |
| | 71 | 109 | | rotationDenominator, |
| | 71 | 110 | | scale.Y, |
| | 71 | 111 | | out Fixed64 y); |
| | 71 | 112 | | if (!representable) |
| | | 113 | | { |
| | 2 | 114 | | localPoint = default; |
| | 2 | 115 | | return false; |
| | | 116 | | } |
| | | 117 | | |
| | 69 | 118 | | localPoint = new Vector2d(x, y); |
| | 69 | 119 | | return true; |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | internal static bool TryComposeScaledLocalPoints( |
| | | 123 | | Vector2d outerLocalPoint, |
| | | 124 | | Vector2d outerScale, |
| | | 125 | | Vector2d innerFrameOffset, |
| | | 126 | | Vector2d innerFrameScale, |
| | | 127 | | Vector2d innerLocalDisplacement, |
| | | 128 | | Fixed64 innerAngleInRadians, |
| | | 129 | | out Vector2d result) |
| | | 130 | | { |
| | 68 | 131 | | Fixed64 cosine = FixedMath.Cos(innerAngleInRadians); |
| | 68 | 132 | | Fixed64 sine = FixedMath.Sin(innerAngleInRadians); |
| | 68 | 133 | | Signed320 one = |
| | 68 | 134 | | Signed320.ExtendValue(Signed192.One); |
| | 68 | 135 | | Signed576 denominator = Signed576.ExtendValue( |
| | 68 | 136 | | Product(Fixed64.One, Fixed64.One)); |
| | 68 | 137 | | GetScaledRotationNumerators( |
| | 68 | 138 | | Vector2d.Zero, |
| | 68 | 139 | | Vector2d.One, |
| | 68 | 140 | | innerLocalDisplacement, |
| | 68 | 141 | | cosine, |
| | 68 | 142 | | sine, |
| | 68 | 143 | | out Signed576 rotatedX, |
| | 68 | 144 | | out Signed576 rotatedY); |
| | 68 | 145 | | Signed576 outerX = WideArithmetic.MultiplySigned320( |
| | 68 | 146 | | Product(outerLocalPoint.X, outerScale.X), |
| | 68 | 147 | | one); |
| | 68 | 148 | | Signed576 outerY = WideArithmetic.MultiplySigned320( |
| | 68 | 149 | | Product(outerLocalPoint.Y, outerScale.Y), |
| | 68 | 150 | | one); |
| | 68 | 151 | | Signed576 innerX = WideArithmetic.MultiplySigned320( |
| | 68 | 152 | | Product(innerFrameOffset.X, innerFrameScale.X), |
| | 68 | 153 | | one); |
| | 68 | 154 | | Signed576 innerY = WideArithmetic.MultiplySigned320( |
| | 68 | 155 | | Product(innerFrameOffset.Y, innerFrameScale.Y), |
| | 68 | 156 | | one); |
| | 68 | 157 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 68 | 158 | | WideArithmetic.AddSigned576( |
| | 68 | 159 | | WideArithmetic.AddSigned576(outerX, innerX), |
| | 68 | 160 | | rotatedX), |
| | 68 | 161 | | denominator, |
| | 68 | 162 | | out Fixed64 x) |
| | 68 | 163 | | & Fixed64.TryGetSignedRawRatio( |
| | 68 | 164 | | WideArithmetic.AddSigned576( |
| | 68 | 165 | | WideArithmetic.AddSigned576(outerY, innerY), |
| | 68 | 166 | | rotatedY), |
| | 68 | 167 | | denominator, |
| | 68 | 168 | | out Fixed64 y); |
| | 68 | 169 | | if (!representable) |
| | | 170 | | { |
| | 1 | 171 | | result = default; |
| | 1 | 172 | | return false; |
| | | 173 | | } |
| | | 174 | | |
| | 67 | 175 | | result = new Vector2d(x, y); |
| | 67 | 176 | | return true; |
| | | 177 | | } |
| | | 178 | | |
| | | 179 | | internal static bool TryTransformPoint( |
| | | 180 | | Vector2d origin, |
| | | 181 | | Vector2d localPoint, |
| | | 182 | | Fixed64 angleInRadians, |
| | | 183 | | out Vector2d result) => |
| | 35 | 184 | | TryTransformCompositePoint( |
| | 35 | 185 | | origin, |
| | 35 | 186 | | localPoint, |
| | 35 | 187 | | Vector2d.Zero, |
| | 35 | 188 | | angleInRadians, |
| | 35 | 189 | | out result); |
| | | 190 | | |
| | | 191 | | internal static bool TryTransformCompositePoint( |
| | | 192 | | Vector2d origin, |
| | | 193 | | Vector2d localPoint, |
| | | 194 | | Vector2d localDisplacement, |
| | | 195 | | Fixed64 angleInRadians, |
| | | 196 | | out Vector2d result) |
| | | 197 | | { |
| | 358 | 198 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 358 | 199 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 358 | 200 | | Signed576 denominator = Signed576.One; |
| | 358 | 201 | | GetRotationNumerators( |
| | 358 | 202 | | localPoint, |
| | 358 | 203 | | localDisplacement, |
| | 358 | 204 | | cosine, |
| | 358 | 205 | | sine, |
| | 358 | 206 | | out Signed320 rotatedX, |
| | 358 | 207 | | out Signed320 rotatedY); |
| | 358 | 208 | | bool representable = TryRoundCoordinate( |
| | 358 | 209 | | WideArithmetic.AddSigned320( |
| | 358 | 210 | | Scale(origin.X), |
| | 358 | 211 | | rotatedX), |
| | 358 | 212 | | denominator, |
| | 358 | 213 | | out Fixed64 x) |
| | 358 | 214 | | & TryRoundCoordinate( |
| | 358 | 215 | | WideArithmetic.AddSigned320( |
| | 358 | 216 | | Scale(origin.Y), |
| | 358 | 217 | | rotatedY), |
| | 358 | 218 | | denominator, |
| | 358 | 219 | | out Fixed64 y); |
| | 358 | 220 | | if (!representable) |
| | | 221 | | { |
| | 17 | 222 | | result = default; |
| | 17 | 223 | | return false; |
| | | 224 | | } |
| | | 225 | | |
| | 341 | 226 | | result = new Vector2d(x, y); |
| | 341 | 227 | | return true; |
| | | 228 | | } |
| | | 229 | | |
| | | 230 | | internal static bool TryTransformPoint( |
| | | 231 | | Vector2d firstOrigin, |
| | | 232 | | Vector2d secondOrigin, |
| | | 233 | | Vector2d localPoint, |
| | | 234 | | Fixed64 angleInRadians, |
| | | 235 | | out Vector2d result) |
| | | 236 | | { |
| | 69 | 237 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 69 | 238 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 69 | 239 | | Signed576 denominator = Signed576.One; |
| | 69 | 240 | | GetRotationNumerators( |
| | 69 | 241 | | localPoint, |
| | 69 | 242 | | cosine, |
| | 69 | 243 | | sine, |
| | 69 | 244 | | out Signed320 rotatedX, |
| | 69 | 245 | | out Signed320 rotatedY); |
| | 69 | 246 | | bool representable = TryRoundCoordinate( |
| | 69 | 247 | | GetTransformNumerator( |
| | 69 | 248 | | firstOrigin.X, |
| | 69 | 249 | | secondOrigin.X, |
| | 69 | 250 | | rotatedX), |
| | 69 | 251 | | denominator, |
| | 69 | 252 | | out Fixed64 x) |
| | 69 | 253 | | & TryRoundCoordinate( |
| | 69 | 254 | | GetTransformNumerator( |
| | 69 | 255 | | firstOrigin.Y, |
| | 69 | 256 | | secondOrigin.Y, |
| | 69 | 257 | | rotatedY), |
| | 69 | 258 | | denominator, |
| | 69 | 259 | | out Fixed64 y); |
| | 69 | 260 | | if (!representable) |
| | | 261 | | { |
| | 1 | 262 | | result = default; |
| | 1 | 263 | | return false; |
| | | 264 | | } |
| | | 265 | | |
| | 68 | 266 | | result = new Vector2d(x, y); |
| | 68 | 267 | | return true; |
| | | 268 | | } |
| | | 269 | | |
| | | 270 | | internal static bool TryGetRelativeOffset( |
| | | 271 | | Vector2d firstOrigin, |
| | | 272 | | Vector2d firstOffset, |
| | | 273 | | Vector2d secondOrigin, |
| | | 274 | | Vector2d secondLocalPoint, |
| | | 275 | | Fixed64 angleInRadians, |
| | | 276 | | out Vector2d result) |
| | | 277 | | { |
| | 35 | 278 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 35 | 279 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 35 | 280 | | Signed576 denominator = Signed576.One; |
| | 35 | 281 | | GetRotationNumerators( |
| | 35 | 282 | | secondLocalPoint, |
| | 35 | 283 | | cosine, |
| | 35 | 284 | | sine, |
| | 35 | 285 | | out Signed320 rotatedX, |
| | 35 | 286 | | out Signed320 rotatedY); |
| | 35 | 287 | | bool representable = TryRoundCoordinate( |
| | 35 | 288 | | GetRelativeNumerator( |
| | 35 | 289 | | firstOrigin.X, |
| | 35 | 290 | | firstOffset.X, |
| | 35 | 291 | | secondOrigin.X, |
| | 35 | 292 | | rotatedX), |
| | 35 | 293 | | denominator, |
| | 35 | 294 | | out Fixed64 x) |
| | 35 | 295 | | & TryRoundCoordinate( |
| | 35 | 296 | | GetRelativeNumerator( |
| | 35 | 297 | | firstOrigin.Y, |
| | 35 | 298 | | firstOffset.Y, |
| | 35 | 299 | | secondOrigin.Y, |
| | 35 | 300 | | rotatedY), |
| | 35 | 301 | | denominator, |
| | 35 | 302 | | out Fixed64 y); |
| | 35 | 303 | | if (!representable) |
| | | 304 | | { |
| | 1 | 305 | | result = default; |
| | 1 | 306 | | return false; |
| | | 307 | | } |
| | | 308 | | |
| | 34 | 309 | | result = new Vector2d(x, y); |
| | 34 | 310 | | return true; |
| | | 311 | | } |
| | | 312 | | |
| | | 313 | | internal static bool TryGetLocalPointIn( |
| | | 314 | | Vector2d pointOrigin, |
| | | 315 | | Vector2d pointLocalPoint, |
| | | 316 | | Vector2d pointLocalDisplacement, |
| | | 317 | | Fixed64 pointAngleInRadians, |
| | | 318 | | Vector2d frameOrigin, |
| | | 319 | | Fixed64 frameAngleInRadians, |
| | | 320 | | out Vector2d localPoint) |
| | | 321 | | { |
| | 73 | 322 | | Fixed64 pointCosine = FixedMath.Cos(pointAngleInRadians); |
| | 73 | 323 | | Fixed64 pointSine = FixedMath.Sin(pointAngleInRadians); |
| | 73 | 324 | | Fixed64 frameCosine = FixedMath.Cos(frameAngleInRadians); |
| | 73 | 325 | | Fixed64 frameSine = FixedMath.Sin(frameAngleInRadians); |
| | 73 | 326 | | GetRotationNumerators( |
| | 73 | 327 | | pointLocalPoint, |
| | 73 | 328 | | pointLocalDisplacement, |
| | 73 | 329 | | pointCosine, |
| | 73 | 330 | | pointSine, |
| | 73 | 331 | | out Signed320 pointX, |
| | 73 | 332 | | out Signed320 pointY); |
| | 73 | 333 | | Signed320 deltaX = WideArithmetic.AddSigned320( |
| | 73 | 334 | | WideArithmetic.SubtractSigned320( |
| | 73 | 335 | | Scale(pointOrigin.X), |
| | 73 | 336 | | Scale(frameOrigin.X)), |
| | 73 | 337 | | pointX); |
| | 73 | 338 | | Signed320 deltaY = WideArithmetic.AddSigned320( |
| | 73 | 339 | | WideArithmetic.SubtractSigned320( |
| | 73 | 340 | | Scale(pointOrigin.Y), |
| | 73 | 341 | | Scale(frameOrigin.Y)), |
| | 73 | 342 | | pointY); |
| | 73 | 343 | | Signed576 xNumerator = WideArithmetic.AddSigned576( |
| | 73 | 344 | | WideArithmetic.MultiplySigned576( |
| | 73 | 345 | | Signed576.ExtendValue(deltaX), |
| | 73 | 346 | | Signed192.Raw(frameCosine)), |
| | 73 | 347 | | WideArithmetic.MultiplySigned576( |
| | 73 | 348 | | Signed576.ExtendValue(deltaY), |
| | 73 | 349 | | Signed192.Raw(frameSine))); |
| | 73 | 350 | | Signed576 yNumerator = WideArithmetic.SubtractSigned576( |
| | 73 | 351 | | WideArithmetic.MultiplySigned576( |
| | 73 | 352 | | Signed576.ExtendValue(deltaY), |
| | 73 | 353 | | Signed192.Raw(frameCosine)), |
| | 73 | 354 | | WideArithmetic.MultiplySigned576( |
| | 73 | 355 | | Signed576.ExtendValue(deltaX), |
| | 73 | 356 | | Signed192.Raw(frameSine))); |
| | 73 | 357 | | Signed576 denominator = Signed576.ExtendValue( |
| | 73 | 358 | | WideArithmetic.AddSigned320( |
| | 73 | 359 | | Product(frameCosine, frameCosine), |
| | 73 | 360 | | Product(frameSine, frameSine))); |
| | 73 | 361 | | bool representable = |
| | 73 | 362 | | Fixed64.TryGetSignedRawRatio( |
| | 73 | 363 | | xNumerator, |
| | 73 | 364 | | denominator, |
| | 73 | 365 | | out Fixed64 x) |
| | 73 | 366 | | & Fixed64.TryGetSignedRawRatio( |
| | 73 | 367 | | yNumerator, |
| | 73 | 368 | | denominator, |
| | 73 | 369 | | out Fixed64 y); |
| | 73 | 370 | | localPoint = representable ? new Vector2d(x, y) : default; |
| | 73 | 371 | | return representable; |
| | | 372 | | } |
| | | 373 | | |
| | | 374 | | internal static bool TryGetRelativeOffset( |
| | | 375 | | Vector2d firstOrigin, |
| | | 376 | | Vector2d firstLocalPoint, |
| | | 377 | | Fixed64 firstAngleInRadians, |
| | | 378 | | Vector2d secondOrigin, |
| | | 379 | | Vector2d secondLocalPoint, |
| | | 380 | | Fixed64 secondAngleInRadians, |
| | | 381 | | out Vector2d result) => |
| | 35 | 382 | | TryGetRelativeOffset( |
| | 35 | 383 | | firstOrigin, |
| | 35 | 384 | | firstLocalPoint, |
| | 35 | 385 | | Vector2d.Zero, |
| | 35 | 386 | | firstAngleInRadians, |
| | 35 | 387 | | secondOrigin, |
| | 35 | 388 | | secondLocalPoint, |
| | 35 | 389 | | Vector2d.Zero, |
| | 35 | 390 | | secondAngleInRadians, |
| | 35 | 391 | | out result); |
| | | 392 | | |
| | | 393 | | internal static bool TryGetRelativeOffset( |
| | | 394 | | Vector2d firstOrigin, |
| | | 395 | | Vector2d firstLocalPoint, |
| | | 396 | | Vector2d firstLocalDisplacement, |
| | | 397 | | Fixed64 firstAngleInRadians, |
| | | 398 | | Vector2d secondOrigin, |
| | | 399 | | Vector2d secondLocalPoint, |
| | | 400 | | Vector2d secondLocalDisplacement, |
| | | 401 | | Fixed64 secondAngleInRadians, |
| | | 402 | | out Vector2d result) |
| | | 403 | | { |
| | 77 | 404 | | Fixed64 firstCosine = FixedMath.Cos(firstAngleInRadians); |
| | 77 | 405 | | Fixed64 firstSine = FixedMath.Sin(firstAngleInRadians); |
| | 77 | 406 | | Fixed64 secondCosine = FixedMath.Cos(secondAngleInRadians); |
| | 77 | 407 | | Fixed64 secondSine = FixedMath.Sin(secondAngleInRadians); |
| | 77 | 408 | | Signed576 denominator = Signed576.One; |
| | 77 | 409 | | GetRotationNumerators( |
| | 77 | 410 | | firstLocalPoint, |
| | 77 | 411 | | firstLocalDisplacement, |
| | 77 | 412 | | firstCosine, |
| | 77 | 413 | | firstSine, |
| | 77 | 414 | | out Signed320 firstRotatedX, |
| | 77 | 415 | | out Signed320 firstRotatedY); |
| | 77 | 416 | | GetRotationNumerators( |
| | 77 | 417 | | secondLocalPoint, |
| | 77 | 418 | | secondLocalDisplacement, |
| | 77 | 419 | | secondCosine, |
| | 77 | 420 | | secondSine, |
| | 77 | 421 | | out Signed320 secondRotatedX, |
| | 77 | 422 | | out Signed320 secondRotatedY); |
| | 77 | 423 | | bool representable = TryRoundCoordinate( |
| | 77 | 424 | | GetRelativeNumerator( |
| | 77 | 425 | | firstOrigin.X, |
| | 77 | 426 | | firstRotatedX, |
| | 77 | 427 | | secondOrigin.X, |
| | 77 | 428 | | secondRotatedX), |
| | 77 | 429 | | denominator, |
| | 77 | 430 | | out Fixed64 x) |
| | 77 | 431 | | & TryRoundCoordinate( |
| | 77 | 432 | | GetRelativeNumerator( |
| | 77 | 433 | | firstOrigin.Y, |
| | 77 | 434 | | firstRotatedY, |
| | 77 | 435 | | secondOrigin.Y, |
| | 77 | 436 | | secondRotatedY), |
| | 77 | 437 | | denominator, |
| | 77 | 438 | | out Fixed64 y); |
| | 77 | 439 | | if (!representable) |
| | | 440 | | { |
| | 2 | 441 | | result = default; |
| | 2 | 442 | | return false; |
| | | 443 | | } |
| | | 444 | | |
| | 75 | 445 | | result = new Vector2d(x, y); |
| | 75 | 446 | | return true; |
| | | 447 | | } |
| | | 448 | | |
| | | 449 | | internal static void GetRotationNumerators( |
| | | 450 | | Vector2d localPoint, |
| | | 451 | | Vector2d localDisplacement, |
| | | 452 | | Fixed64 cosine, |
| | | 453 | | Fixed64 sine, |
| | | 454 | | out Signed320 x, |
| | | 455 | | out Signed320 y) |
| | | 456 | | { |
| | 775 | 457 | | GetRotationNumerators( |
| | 775 | 458 | | localPoint, |
| | 775 | 459 | | cosine, |
| | 775 | 460 | | sine, |
| | 775 | 461 | | out x, |
| | 775 | 462 | | out y); |
| | 775 | 463 | | GetRotationNumerators( |
| | 775 | 464 | | localDisplacement, |
| | 775 | 465 | | cosine, |
| | 775 | 466 | | sine, |
| | 775 | 467 | | out Signed320 displacementX, |
| | 775 | 468 | | out Signed320 displacementY); |
| | 775 | 469 | | x = WideArithmetic.AddSigned320(x, displacementX); |
| | 775 | 470 | | y = WideArithmetic.AddSigned320(y, displacementY); |
| | 775 | 471 | | } |
| | | 472 | | |
| | | 473 | | private static void GetRotationNumerators( |
| | | 474 | | Vector2d localPoint, |
| | | 475 | | Fixed64 cosine, |
| | | 476 | | Fixed64 sine, |
| | | 477 | | out Signed320 x, |
| | | 478 | | out Signed320 y) |
| | | 479 | | { |
| | 1654 | 480 | | x = WideArithmetic.SubtractSigned320( |
| | 1654 | 481 | | Product(localPoint.X, cosine), |
| | 1654 | 482 | | Product(localPoint.Y, sine)); |
| | 1654 | 483 | | y = WideArithmetic.AddSigned320( |
| | 1654 | 484 | | Product(localPoint.X, sine), |
| | 1654 | 485 | | Product(localPoint.Y, cosine)); |
| | 1654 | 486 | | } |
| | | 487 | | |
| | | 488 | | private static void GetScaledRotationNumerators( |
| | | 489 | | Vector2d localPoint, |
| | | 490 | | Vector2d scale, |
| | | 491 | | Vector2d localDisplacement, |
| | | 492 | | Fixed64 cosine, |
| | | 493 | | Fixed64 sine, |
| | | 494 | | out Signed576 x, |
| | | 495 | | out Signed576 y) |
| | | 496 | | { |
| | 139 | 497 | | Signed320 localX = Product(localPoint.X, scale.X); |
| | 139 | 498 | | Signed320 localY = Product(localPoint.Y, scale.Y); |
| | 139 | 499 | | Signed320 one = |
| | 139 | 500 | | Signed320.ExtendValue(Signed192.One); |
| | 139 | 501 | | Signed576 displacementX = WideArithmetic.MultiplySigned320( |
| | 139 | 502 | | WideArithmetic.SubtractSigned320( |
| | 139 | 503 | | Product(localDisplacement.X, cosine), |
| | 139 | 504 | | Product(localDisplacement.Y, sine)), |
| | 139 | 505 | | one); |
| | 139 | 506 | | Signed576 displacementY = WideArithmetic.MultiplySigned320( |
| | 139 | 507 | | WideArithmetic.AddSigned320( |
| | 139 | 508 | | Product(localDisplacement.X, sine), |
| | 139 | 509 | | Product(localDisplacement.Y, cosine)), |
| | 139 | 510 | | one); |
| | 139 | 511 | | x = WideArithmetic.AddSigned576( |
| | 139 | 512 | | WideArithmetic.SubtractSigned576( |
| | 139 | 513 | | WideArithmetic.MultiplySigned320( |
| | 139 | 514 | | localX, |
| | 139 | 515 | | Signed320.ExtendValue(Signed192.Raw(cosine))), |
| | 139 | 516 | | WideArithmetic.MultiplySigned320( |
| | 139 | 517 | | localY, |
| | 139 | 518 | | Signed320.ExtendValue(Signed192.Raw(sine)))), |
| | 139 | 519 | | displacementX); |
| | 139 | 520 | | y = WideArithmetic.AddSigned576( |
| | 139 | 521 | | WideArithmetic.AddSigned576( |
| | 139 | 522 | | WideArithmetic.MultiplySigned320( |
| | 139 | 523 | | localX, |
| | 139 | 524 | | Signed320.ExtendValue(Signed192.Raw(sine))), |
| | 139 | 525 | | WideArithmetic.MultiplySigned320( |
| | 139 | 526 | | localY, |
| | 139 | 527 | | Signed320.ExtendValue(Signed192.Raw(cosine)))), |
| | 139 | 528 | | displacementY); |
| | 139 | 529 | | } |
| | | 530 | | |
| | | 531 | | private static Signed320 GetRelativeNumerator( |
| | | 532 | | Fixed64 firstOrigin, |
| | | 533 | | Fixed64 firstOffset, |
| | | 534 | | Fixed64 secondOrigin, |
| | | 535 | | Signed320 rotatedLocalPoint) => |
| | 70 | 536 | | WideArithmetic.SubtractSigned320( |
| | 70 | 537 | | WideArithmetic.SubtractSigned320( |
| | 70 | 538 | | WideArithmetic.AddSigned320( |
| | 70 | 539 | | Scale(firstOrigin), |
| | 70 | 540 | | Scale(firstOffset)), |
| | 70 | 541 | | Scale(secondOrigin)), |
| | 70 | 542 | | rotatedLocalPoint); |
| | | 543 | | |
| | | 544 | | private static Signed320 GetRelativeNumerator( |
| | | 545 | | Fixed64 firstOrigin, |
| | | 546 | | Signed320 firstRotatedLocalPoint, |
| | | 547 | | Fixed64 secondOrigin, |
| | | 548 | | Signed320 secondRotatedLocalPoint) => |
| | 154 | 549 | | WideArithmetic.SubtractSigned320( |
| | 154 | 550 | | WideArithmetic.AddSigned320( |
| | 154 | 551 | | WideArithmetic.SubtractSigned320( |
| | 154 | 552 | | Scale(firstOrigin), |
| | 154 | 553 | | Scale(secondOrigin)), |
| | 154 | 554 | | firstRotatedLocalPoint), |
| | 154 | 555 | | secondRotatedLocalPoint); |
| | | 556 | | |
| | | 557 | | private static Signed320 GetTransformNumerator( |
| | | 558 | | Fixed64 firstOrigin, |
| | | 559 | | Fixed64 secondOrigin, |
| | | 560 | | Signed320 rotatedLocalPoint) => |
| | 138 | 561 | | WideArithmetic.AddSigned320( |
| | 138 | 562 | | WideArithmetic.AddSigned320( |
| | 138 | 563 | | Scale(firstOrigin), |
| | 138 | 564 | | Scale(secondOrigin)), |
| | 138 | 565 | | rotatedLocalPoint); |
| | | 566 | | |
| | | 567 | | internal static Signed320 Scale(Fixed64 value) |
| | | 568 | | { |
| | 2182 | 569 | | long raw = value.m_rawValue; |
| | 2182 | 570 | | ulong extension = raw < 0L ? ulong.MaxValue : 0UL; |
| | 2182 | 571 | | ulong unsignedRaw = unchecked((ulong)raw); |
| | 2182 | 572 | | return new Signed320( |
| | 2182 | 573 | | extension, |
| | 2182 | 574 | | extension, |
| | 2182 | 575 | | extension, |
| | 2182 | 576 | | (unsignedRaw >> FixedMath.SHIFT_AMOUNT_I) |
| | 2182 | 577 | | | (extension << FixedMath.SHIFT_AMOUNT_I), |
| | 2182 | 578 | | unsignedRaw << FixedMath.SHIFT_AMOUNT_I); |
| | | 579 | | } |
| | | 580 | | |
| | | 581 | | private static Signed320 Product(Fixed64 left, Fixed64 right) => |
| | 8291 | 582 | | WideArithmetic.MultiplySigned192(Signed192.Raw(left), Signed192.Raw(right)); |
| | | 583 | | |
| | | 584 | | private static bool TryInverseTransformScaledCoordinate( |
| | | 585 | | Signed320 projection, |
| | | 586 | | Signed320 rotationDenominator, |
| | | 587 | | Fixed64 scale, |
| | | 588 | | out Fixed64 result) => |
| | 142 | 589 | | Fixed64.TryGetSignedRawRatio( |
| | 142 | 590 | | WideArithmetic.MultiplySigned320( |
| | 142 | 591 | | projection, |
| | 142 | 592 | | Product(Fixed64.One, Fixed64.One)), |
| | 142 | 593 | | WideArithmetic.MultiplySigned320( |
| | 142 | 594 | | rotationDenominator, |
| | 142 | 595 | | Signed320.ExtendValue(Signed192.Raw(scale))), |
| | 142 | 596 | | out result); |
| | | 597 | | |
| | | 598 | | private static bool TryRoundCoordinate( |
| | | 599 | | Signed320 numerator, |
| | | 600 | | Signed576 denominator, |
| | | 601 | | out Fixed64 result) => |
| | 1078 | 602 | | Fixed64.TryGetSignedRawRatio( |
| | 1078 | 603 | | Signed576.ExtendValue(numerator), |
| | 1078 | 604 | | denominator, |
| | 1078 | 605 | | out result); |
| | | 606 | | |
| | | 607 | | } |