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