| | | 1 | | //======================================================================= |
| | | 2 | | // WidePointAnchor2d.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 methods for working with 2D point anchors that use wide fixed-point arithmetic. |
| | | 12 | | /// </summary> |
| | | 13 | | internal static class WidePointAnchor2d |
| | | 14 | | { |
| | | 15 | | internal static bool TryGetPoint( |
| | | 16 | | Vector2d origin, |
| | | 17 | | Vector2d localPoint, |
| | | 18 | | Vector2d localDisplacement, |
| | | 19 | | FixedPointAnchorTerm2d exactLocalTerm, |
| | | 20 | | Fixed64 angleInRadians, |
| | | 21 | | out Vector2d result) |
| | | 22 | | { |
| | 332 | 23 | | if (exactLocalTerm.IsZero) |
| | | 24 | | { |
| | 323 | 25 | | return WideVector2dTransform.TryTransformCompositePoint( |
| | 323 | 26 | | origin, |
| | 323 | 27 | | localPoint, |
| | 323 | 28 | | localDisplacement, |
| | 323 | 29 | | angleInRadians, |
| | 323 | 30 | | out result); |
| | | 31 | | } |
| | | 32 | | |
| | 9 | 33 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 9 | 34 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 9 | 35 | | Signed320 denominator = GetExactAnchorDenominator(); |
| | 9 | 36 | | GetExactCoordinates( |
| | 9 | 37 | | origin, |
| | 9 | 38 | | localPoint, |
| | 9 | 39 | | localDisplacement, |
| | 9 | 40 | | exactLocalTerm, |
| | 9 | 41 | | cosine, |
| | 9 | 42 | | sine, |
| | 9 | 43 | | out Signed320 xNumerator, |
| | 9 | 44 | | out Signed320 yNumerator); |
| | 9 | 45 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 9 | 46 | | Signed576.ExtendValue(xNumerator), |
| | 9 | 47 | | Signed576.ExtendValue(denominator), |
| | 9 | 48 | | out Fixed64 x) |
| | 9 | 49 | | & Fixed64.TryGetSignedRawRatio( |
| | 9 | 50 | | Signed576.ExtendValue(yNumerator), |
| | 9 | 51 | | Signed576.ExtendValue(denominator), |
| | 9 | 52 | | out Fixed64 y); |
| | 9 | 53 | | result = representable ? new Vector2d(x, y) : default; |
| | 9 | 54 | | return representable; |
| | | 55 | | } |
| | | 56 | | |
| | | 57 | | internal static bool TryGetRelativeOffset( |
| | | 58 | | Vector2d firstOrigin, |
| | | 59 | | Vector2d firstLocalPoint, |
| | | 60 | | Vector2d firstLocalDisplacement, |
| | | 61 | | FixedPointAnchorTerm2d firstExactLocalTerm, |
| | | 62 | | Fixed64 firstAngleInRadians, |
| | | 63 | | Vector2d secondOrigin, |
| | | 64 | | Vector2d secondLocalPoint, |
| | | 65 | | Vector2d secondLocalDisplacement, |
| | | 66 | | FixedPointAnchorTerm2d secondExactLocalTerm, |
| | | 67 | | Fixed64 secondAngleInRadians, |
| | | 68 | | out Vector2d result) |
| | | 69 | | { |
| | 47 | 70 | | if (firstExactLocalTerm.IsZero |
| | 47 | 71 | | && secondExactLocalTerm.IsZero) |
| | | 72 | | { |
| | 42 | 73 | | return WideVector2dTransform.TryGetRelativeOffset( |
| | 42 | 74 | | firstOrigin, |
| | 42 | 75 | | firstLocalPoint, |
| | 42 | 76 | | firstLocalDisplacement, |
| | 42 | 77 | | firstAngleInRadians, |
| | 42 | 78 | | secondOrigin, |
| | 42 | 79 | | secondLocalPoint, |
| | 42 | 80 | | secondLocalDisplacement, |
| | 42 | 81 | | secondAngleInRadians, |
| | 42 | 82 | | out result); |
| | | 83 | | } |
| | | 84 | | |
| | 5 | 85 | | GetExactRelativeOffsetRatio( |
| | 5 | 86 | | firstOrigin, |
| | 5 | 87 | | firstLocalPoint, |
| | 5 | 88 | | firstLocalDisplacement, |
| | 5 | 89 | | firstExactLocalTerm, |
| | 5 | 90 | | firstAngleInRadians, |
| | 5 | 91 | | secondOrigin, |
| | 5 | 92 | | secondLocalPoint, |
| | 5 | 93 | | secondLocalDisplacement, |
| | 5 | 94 | | secondExactLocalTerm, |
| | 5 | 95 | | secondAngleInRadians, |
| | 5 | 96 | | out Signed320 xNumerator, |
| | 5 | 97 | | out Signed320 yNumerator, |
| | 5 | 98 | | out Signed320 coordinateDenominator); |
| | 5 | 99 | | Signed576 denominator = |
| | 5 | 100 | | Signed576.ExtendValue(coordinateDenominator); |
| | 5 | 101 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 5 | 102 | | Signed576.ExtendValue(xNumerator), |
| | 5 | 103 | | denominator, |
| | 5 | 104 | | out Fixed64 x) |
| | 5 | 105 | | & Fixed64.TryGetSignedRawRatio( |
| | 5 | 106 | | Signed576.ExtendValue(yNumerator), |
| | 5 | 107 | | denominator, |
| | 5 | 108 | | out Fixed64 y); |
| | 5 | 109 | | result = representable ? new Vector2d(x, y) : default; |
| | 5 | 110 | | return representable; |
| | | 111 | | } |
| | | 112 | | |
| | | 113 | | internal static bool RepresentsSamePoint( |
| | | 114 | | in FixedPointAnchor2d first, |
| | | 115 | | in FixedPointAnchor2d second) |
| | | 116 | | { |
| | 35 | 117 | | GetExactCoordinates( |
| | 35 | 118 | | first.Origin, |
| | 35 | 119 | | first.LocalPoint, |
| | 35 | 120 | | first.LocalDisplacement, |
| | 35 | 121 | | first.ExactLocalTerm, |
| | 35 | 122 | | FixedMath.Cos(first.Rotation), |
| | 35 | 123 | | FixedMath.Sin(first.Rotation), |
| | 35 | 124 | | out Signed320 firstX, |
| | 35 | 125 | | out Signed320 firstY); |
| | 35 | 126 | | GetExactCoordinates( |
| | 35 | 127 | | second.Origin, |
| | 35 | 128 | | second.LocalPoint, |
| | 35 | 129 | | second.LocalDisplacement, |
| | 35 | 130 | | second.ExactLocalTerm, |
| | 35 | 131 | | FixedMath.Cos(second.Rotation), |
| | 35 | 132 | | FixedMath.Sin(second.Rotation), |
| | 35 | 133 | | out Signed320 secondX, |
| | 35 | 134 | | out Signed320 secondY); |
| | 35 | 135 | | return firstX.Equals(secondX) && firstY.Equals(secondY); |
| | | 136 | | } |
| | | 137 | | |
| | | 138 | | internal static bool TryGetLocalPointIn( |
| | | 139 | | Vector2d pointOrigin, |
| | | 140 | | Vector2d pointLocalPoint, |
| | | 141 | | Vector2d pointLocalDisplacement, |
| | | 142 | | FixedPointAnchorTerm2d exactLocalTerm, |
| | | 143 | | Fixed64 pointAngleInRadians, |
| | | 144 | | Vector2d frameOrigin, |
| | | 145 | | Fixed64 frameAngleInRadians, |
| | | 146 | | out Vector2d localPoint) |
| | | 147 | | { |
| | 76 | 148 | | if (exactLocalTerm.IsZero) |
| | | 149 | | { |
| | 73 | 150 | | return WideVector2dTransform.TryGetLocalPointIn( |
| | 73 | 151 | | pointOrigin, |
| | 73 | 152 | | pointLocalPoint, |
| | 73 | 153 | | pointLocalDisplacement, |
| | 73 | 154 | | pointAngleInRadians, |
| | 73 | 155 | | frameOrigin, |
| | 73 | 156 | | frameAngleInRadians, |
| | 73 | 157 | | out localPoint); |
| | | 158 | | } |
| | | 159 | | |
| | 3 | 160 | | Fixed64 pointCosine = FixedMath.Cos(pointAngleInRadians); |
| | 3 | 161 | | Fixed64 pointSine = FixedMath.Sin(pointAngleInRadians); |
| | 3 | 162 | | Fixed64 frameCosine = FixedMath.Cos(frameAngleInRadians); |
| | 3 | 163 | | Fixed64 frameSine = FixedMath.Sin(frameAngleInRadians); |
| | 3 | 164 | | GetExactCoordinates( |
| | 3 | 165 | | pointOrigin, |
| | 3 | 166 | | pointLocalPoint, |
| | 3 | 167 | | pointLocalDisplacement, |
| | 3 | 168 | | exactLocalTerm, |
| | 3 | 169 | | pointCosine, |
| | 3 | 170 | | pointSine, |
| | 3 | 171 | | out Signed320 pointX, |
| | 3 | 172 | | out Signed320 pointY); |
| | 3 | 173 | | Signed320 denominator = GetExactAnchorDenominator(); |
| | 3 | 174 | | Signed320 deltaX = WideArithmetic.SubtractSigned320( |
| | 3 | 175 | | pointX, |
| | 3 | 176 | | WideArithmetic.MultiplySigned192( |
| | 3 | 177 | | Signed192.Raw(frameOrigin.X), |
| | 3 | 178 | | Signed192.NarrowValue(denominator))); |
| | 3 | 179 | | Signed320 deltaY = WideArithmetic.SubtractSigned320( |
| | 3 | 180 | | pointY, |
| | 3 | 181 | | WideArithmetic.MultiplySigned192( |
| | 3 | 182 | | Signed192.Raw(frameOrigin.Y), |
| | 3 | 183 | | Signed192.NarrowValue(denominator))); |
| | 3 | 184 | | Signed576 xNumerator = WideArithmetic.AddSigned576( |
| | 3 | 185 | | WideArithmetic.MultiplySigned576( |
| | 3 | 186 | | Signed576.ExtendValue(deltaX), |
| | 3 | 187 | | Signed192.Raw(frameCosine)), |
| | 3 | 188 | | WideArithmetic.MultiplySigned576( |
| | 3 | 189 | | Signed576.ExtendValue(deltaY), |
| | 3 | 190 | | Signed192.Raw(frameSine))); |
| | 3 | 191 | | Signed576 yNumerator = WideArithmetic.SubtractSigned576( |
| | 3 | 192 | | WideArithmetic.MultiplySigned576( |
| | 3 | 193 | | Signed576.ExtendValue(deltaY), |
| | 3 | 194 | | Signed192.Raw(frameCosine)), |
| | 3 | 195 | | WideArithmetic.MultiplySigned576( |
| | 3 | 196 | | Signed576.ExtendValue(deltaX), |
| | 3 | 197 | | Signed192.Raw(frameSine))); |
| | 3 | 198 | | Signed576 inverseDenominator = WideArithmetic.MultiplySigned320( |
| | 3 | 199 | | WideArithmetic.AddSigned320( |
| | 3 | 200 | | WideArithmetic.MultiplySigned192( |
| | 3 | 201 | | Signed192.Raw(frameCosine), |
| | 3 | 202 | | Signed192.Raw(frameCosine)), |
| | 3 | 203 | | WideArithmetic.MultiplySigned192( |
| | 3 | 204 | | Signed192.Raw(frameSine), |
| | 3 | 205 | | Signed192.Raw(frameSine))), |
| | 3 | 206 | | Signed320.ExtendValue( |
| | 3 | 207 | | FixedPointAnchorTerm3d.Denominator)); |
| | 3 | 208 | | bool representable = Fixed64.TryGetSignedRawRatio( |
| | 3 | 209 | | xNumerator, |
| | 3 | 210 | | inverseDenominator, |
| | 3 | 211 | | out Fixed64 x) |
| | 3 | 212 | | & Fixed64.TryGetSignedRawRatio( |
| | 3 | 213 | | yNumerator, |
| | 3 | 214 | | inverseDenominator, |
| | 3 | 215 | | out Fixed64 y); |
| | 3 | 216 | | localPoint = representable ? new Vector2d(x, y) : default; |
| | 3 | 217 | | return representable; |
| | | 218 | | } |
| | | 219 | | |
| | | 220 | | internal static int CompareSquaredDistances( |
| | | 221 | | in FixedPointAnchor2d reference, |
| | | 222 | | in FixedPointAnchor2d first, |
| | | 223 | | in FixedPointAnchor2d second) |
| | | 224 | | { |
| | 9 | 225 | | Signed576 firstSquaredDistance = GetSquaredDistanceNumerator( |
| | 9 | 226 | | reference, |
| | 9 | 227 | | first); |
| | 9 | 228 | | Signed576 secondSquaredDistance = GetSquaredDistanceNumerator( |
| | 9 | 229 | | reference, |
| | 9 | 230 | | second); |
| | 9 | 231 | | return WideArithmetic.CompareNonNegative( |
| | 9 | 232 | | firstSquaredDistance, |
| | 9 | 233 | | secondSquaredDistance); |
| | | 234 | | } |
| | | 235 | | |
| | | 236 | | private static Signed576 GetSquaredDistanceNumerator( |
| | | 237 | | in FixedPointAnchor2d reference, |
| | | 238 | | in FixedPointAnchor2d point) |
| | | 239 | | { |
| | 18 | 240 | | GetExactRelativeOffsetRatio( |
| | 18 | 241 | | point.Origin, |
| | 18 | 242 | | point.LocalPoint, |
| | 18 | 243 | | point.LocalDisplacement, |
| | 18 | 244 | | point.ExactLocalTerm, |
| | 18 | 245 | | point.Rotation, |
| | 18 | 246 | | reference.Origin, |
| | 18 | 247 | | reference.LocalPoint, |
| | 18 | 248 | | reference.LocalDisplacement, |
| | 18 | 249 | | reference.ExactLocalTerm, |
| | 18 | 250 | | reference.Rotation, |
| | 18 | 251 | | out Signed320 x, |
| | 18 | 252 | | out Signed320 y, |
| | 18 | 253 | | // Every point-anchor term uses the same fixed denominator, so the |
| | 18 | 254 | | // squared coordinate numerators can be compared directly. |
| | 18 | 255 | | out _); |
| | 18 | 256 | | return WideArithmetic.AddSigned576( |
| | 18 | 257 | | WideArithmetic.MultiplySigned320(x, x), |
| | 18 | 258 | | WideArithmetic.MultiplySigned320(y, y)); |
| | | 259 | | } |
| | | 260 | | |
| | | 261 | | internal static void GetExactRelativeOffsetRatio( |
| | | 262 | | Vector2d firstOrigin, |
| | | 263 | | Vector2d firstLocalPoint, |
| | | 264 | | Vector2d firstLocalDisplacement, |
| | | 265 | | FixedPointAnchorTerm2d firstExactLocalTerm, |
| | | 266 | | Fixed64 firstAngleInRadians, |
| | | 267 | | Vector2d secondOrigin, |
| | | 268 | | Vector2d secondLocalPoint, |
| | | 269 | | Vector2d secondLocalDisplacement, |
| | | 270 | | FixedPointAnchorTerm2d secondExactLocalTerm, |
| | | 271 | | Fixed64 secondAngleInRadians, |
| | | 272 | | out Signed320 x, |
| | | 273 | | out Signed320 y, |
| | | 274 | | out Signed320 denominator) |
| | | 275 | | { |
| | 54 | 276 | | GetExactCoordinates( |
| | 54 | 277 | | firstOrigin, |
| | 54 | 278 | | firstLocalPoint, |
| | 54 | 279 | | firstLocalDisplacement, |
| | 54 | 280 | | firstExactLocalTerm, |
| | 54 | 281 | | FixedMath.Cos(firstAngleInRadians), |
| | 54 | 282 | | FixedMath.Sin(firstAngleInRadians), |
| | 54 | 283 | | out Signed320 firstX, |
| | 54 | 284 | | out Signed320 firstY); |
| | 54 | 285 | | GetExactCoordinates( |
| | 54 | 286 | | secondOrigin, |
| | 54 | 287 | | secondLocalPoint, |
| | 54 | 288 | | secondLocalDisplacement, |
| | 54 | 289 | | secondExactLocalTerm, |
| | 54 | 290 | | FixedMath.Cos(secondAngleInRadians), |
| | 54 | 291 | | FixedMath.Sin(secondAngleInRadians), |
| | 54 | 292 | | out Signed320 secondX, |
| | 54 | 293 | | out Signed320 secondY); |
| | 54 | 294 | | x = WideArithmetic.SubtractSigned320(firstX, secondX); |
| | 54 | 295 | | y = WideArithmetic.SubtractSigned320(firstY, secondY); |
| | 54 | 296 | | denominator = GetExactAnchorDenominator(); |
| | 54 | 297 | | } |
| | | 298 | | |
| | | 299 | | private static void GetExactCoordinates( |
| | | 300 | | Vector2d origin, |
| | | 301 | | Vector2d localPoint, |
| | | 302 | | Vector2d localDisplacement, |
| | | 303 | | FixedPointAnchorTerm2d exactLocalTerm, |
| | | 304 | | Fixed64 cosine, |
| | | 305 | | Fixed64 sine, |
| | | 306 | | out Signed320 x, |
| | | 307 | | out Signed320 y) |
| | | 308 | | { |
| | 190 | 309 | | WideVector2dTransform.GetRotationNumerators( |
| | 190 | 310 | | localPoint, |
| | 190 | 311 | | localDisplacement, |
| | 190 | 312 | | cosine, |
| | 190 | 313 | | sine, |
| | 190 | 314 | | out Signed320 roundedX, |
| | 190 | 315 | | out Signed320 roundedY); |
| | 190 | 316 | | Signed320 scaledRoundedX = Signed320.NarrowValue( |
| | 190 | 317 | | WideArithmetic.MultiplySigned320( |
| | 190 | 318 | | WideArithmetic.AddSigned320( |
| | 190 | 319 | | WideVector2dTransform.Scale(origin.X), |
| | 190 | 320 | | roundedX), |
| | 190 | 321 | | Signed320.ExtendValue( |
| | 190 | 322 | | FixedPointAnchorTerm3d.Denominator))); |
| | 190 | 323 | | Signed320 scaledRoundedY = Signed320.NarrowValue( |
| | 190 | 324 | | WideArithmetic.MultiplySigned320( |
| | 190 | 325 | | WideArithmetic.AddSigned320( |
| | 190 | 326 | | WideVector2dTransform.Scale(origin.Y), |
| | 190 | 327 | | roundedY), |
| | 190 | 328 | | Signed320.ExtendValue( |
| | 190 | 329 | | FixedPointAnchorTerm3d.Denominator))); |
| | 190 | 330 | | Signed320 residualX = WideArithmetic.SubtractSigned320( |
| | 190 | 331 | | WideArithmetic.MultiplySigned192( |
| | 190 | 332 | | Signed192.Signed(exactLocalTerm.X), |
| | 190 | 333 | | Signed192.Raw(cosine)), |
| | 190 | 334 | | WideArithmetic.MultiplySigned192( |
| | 190 | 335 | | Signed192.Signed(exactLocalTerm.Y), |
| | 190 | 336 | | Signed192.Raw(sine))); |
| | 190 | 337 | | Signed320 residualY = WideArithmetic.AddSigned320( |
| | 190 | 338 | | WideArithmetic.MultiplySigned192( |
| | 190 | 339 | | Signed192.Signed(exactLocalTerm.X), |
| | 190 | 340 | | Signed192.Raw(sine)), |
| | 190 | 341 | | WideArithmetic.MultiplySigned192( |
| | 190 | 342 | | Signed192.Signed(exactLocalTerm.Y), |
| | 190 | 343 | | Signed192.Raw(cosine))); |
| | 190 | 344 | | x = WideArithmetic.AddSigned320(scaledRoundedX, residualX); |
| | 190 | 345 | | y = WideArithmetic.AddSigned320(scaledRoundedY, residualY); |
| | 190 | 346 | | } |
| | | 347 | | |
| | | 348 | | private static Signed320 GetExactAnchorDenominator() => |
| | 66 | 349 | | WideArithmetic.MultiplySigned192( |
| | 66 | 350 | | Signed192.One, |
| | 66 | 351 | | FixedPointAnchorTerm3d.Denominator); |
| | | 352 | | } |