| | | 1 | | //======================================================================= |
| | | 2 | | // Vector2d.Statics.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | namespace FixedMathSharp; |
| | | 13 | | |
| | | 14 | | /// <content> |
| | | 15 | | /// Static utility methods for <see cref="Vector2d"/>, including arithmetic helpers |
| | | 16 | | /// and safe (non-saturating) operation variants. |
| | | 17 | | /// </content> |
| | | 18 | | public partial struct Vector2d |
| | | 19 | | { |
| | | 20 | | /// <summary> |
| | | 21 | | /// Adds two vectors component-wise. |
| | | 22 | | /// </summary> |
| | | 23 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 24 | | public static Vector2d Add(Vector2d v1, Vector2d v2) => v1 + v2; |
| | | 25 | | |
| | | 26 | | /// <summary> |
| | | 27 | | /// Attempts to add two vectors without component saturation. |
| | | 28 | | /// </summary> |
| | | 29 | | /// <param name="left">The left operand.</param> |
| | | 30 | | /// <param name="right">The right operand.</param> |
| | | 31 | | /// <param name="result"> |
| | | 32 | | /// The exact component-wise sum when every component is representable; otherwise, <see langword="default"/>. |
| | | 33 | | /// </param> |
| | | 34 | | /// <returns> |
| | | 35 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 36 | | /// </returns> |
| | | 37 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 38 | | public static bool TryAdd(Vector2d left, Vector2d right, out Vector2d result) |
| | | 39 | | { |
| | 59 | 40 | | if (!Fixed64.TryAdd(left.X, right.X, out Fixed64 x) |
| | 59 | 41 | | || !Fixed64.TryAdd(left.Y, right.Y, out Fixed64 y)) |
| | | 42 | | { |
| | 6 | 43 | | result = default; |
| | 6 | 44 | | return false; |
| | | 45 | | } |
| | | 46 | | |
| | 53 | 47 | | result = new Vector2d(x, y); |
| | 53 | 48 | | return true; |
| | | 49 | | } |
| | | 50 | | |
| | | 51 | | /// <summary> |
| | | 52 | | /// Subtracts two vectors component-wise. |
| | | 53 | | /// </summary> |
| | | 54 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 55 | | public static Vector2d Subtract(Vector2d v1, Vector2d v2) => v1 - v2; |
| | | 56 | | |
| | | 57 | | /// <summary> |
| | | 58 | | /// Attempts to subtract two vectors without component saturation. |
| | | 59 | | /// </summary> |
| | | 60 | | /// <param name="left">The left operand.</param> |
| | | 61 | | /// <param name="right">The right operand.</param> |
| | | 62 | | /// <param name="result"> |
| | | 63 | | /// The exact component-wise difference when every component is representable; otherwise, <see langword="default"/>. |
| | | 64 | | /// </param> |
| | | 65 | | /// <returns> |
| | | 66 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 67 | | /// </returns> |
| | | 68 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 69 | | public static bool TrySubtract(Vector2d left, Vector2d right, out Vector2d result) |
| | | 70 | | { |
| | 25 | 71 | | if (!Fixed64.TrySubtract(left.X, right.X, out Fixed64 x) |
| | 25 | 72 | | || !Fixed64.TrySubtract(left.Y, right.Y, out Fixed64 y)) |
| | | 73 | | { |
| | 3 | 74 | | result = default; |
| | 3 | 75 | | return false; |
| | | 76 | | } |
| | | 77 | | |
| | 22 | 78 | | result = new Vector2d(x, y); |
| | 22 | 79 | | return true; |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | /// <summary> |
| | | 83 | | /// Attempts to add two vectors and subtract a third component-wise without intermediate saturation. |
| | | 84 | | /// </summary> |
| | | 85 | | /// <param name="firstAddend">The first addend.</param> |
| | | 86 | | /// <param name="secondAddend">The second addend.</param> |
| | | 87 | | /// <param name="subtrahend">The vector to subtract from the exact component-wise sum.</param> |
| | | 88 | | /// <param name="result"> |
| | | 89 | | /// The exact component-wise result when every component is representable; otherwise, <see langword="default"/>. |
| | | 90 | | /// </param> |
| | | 91 | | /// <returns> |
| | | 92 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 93 | | /// </returns> |
| | | 94 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 95 | | public static bool TryAddSubtract( |
| | | 96 | | Vector2d firstAddend, |
| | | 97 | | Vector2d secondAddend, |
| | | 98 | | Vector2d subtrahend, |
| | | 99 | | out Vector2d result) |
| | | 100 | | { |
| | 3 | 101 | | if (!Fixed64.TryAddSubtract(firstAddend.X, secondAddend.X, subtrahend.X, out Fixed64 x) |
| | 3 | 102 | | || !Fixed64.TryAddSubtract(firstAddend.Y, secondAddend.Y, subtrahend.Y, out Fixed64 y)) |
| | | 103 | | { |
| | 2 | 104 | | result = default; |
| | 2 | 105 | | return false; |
| | | 106 | | } |
| | | 107 | | |
| | 1 | 108 | | result = new Vector2d(x, y); |
| | 1 | 109 | | return true; |
| | | 110 | | } |
| | | 111 | | |
| | | 112 | | /// <summary> |
| | | 113 | | /// Attempts to compute <c>(firstLeft + firstRight) - |
| | | 114 | | /// (secondLeft + secondRight)</c> component-wise without intermediate |
| | | 115 | | /// saturation. |
| | | 116 | | /// </summary> |
| | | 117 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 118 | | public static bool TrySubtractSums( |
| | | 119 | | Vector2d firstLeft, |
| | | 120 | | Vector2d firstRight, |
| | | 121 | | Vector2d secondLeft, |
| | | 122 | | Vector2d secondRight, |
| | | 123 | | out Vector2d result) |
| | | 124 | | { |
| | 71 | 125 | | bool representable = Fixed64.TrySubtractSums( |
| | 71 | 126 | | firstLeft.X, |
| | 71 | 127 | | firstRight.X, |
| | 71 | 128 | | secondLeft.X, |
| | 71 | 129 | | secondRight.X, |
| | 71 | 130 | | out Fixed64 x) |
| | 71 | 131 | | & Fixed64.TrySubtractSums( |
| | 71 | 132 | | firstLeft.Y, |
| | 71 | 133 | | firstRight.Y, |
| | 71 | 134 | | secondLeft.Y, |
| | 71 | 135 | | secondRight.Y, |
| | 71 | 136 | | out Fixed64 y); |
| | 71 | 137 | | if (!representable) |
| | | 138 | | { |
| | 2 | 139 | | result = default; |
| | 2 | 140 | | return false; |
| | | 141 | | } |
| | | 142 | | |
| | 69 | 143 | | result = new Vector2d(x, y); |
| | 69 | 144 | | return true; |
| | | 145 | | } |
| | | 146 | | |
| | | 147 | | /// <summary> |
| | | 148 | | /// Compares the exact component-difference projections of two vectors. |
| | | 149 | | /// </summary> |
| | | 150 | | /// <param name="candidate">The candidate point.</param> |
| | | 151 | | /// <param name="current">The point to compare against.</param> |
| | | 152 | | /// <param name="direction">The projection direction; it need not be normalized.</param> |
| | | 153 | | /// <returns> |
| | | 154 | | /// A negative value, zero, or a positive value when the candidate projection is |
| | | 155 | | /// respectively less than, equal to, or greater than the current projection. |
| | | 156 | | /// </returns> |
| | | 157 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 158 | | public static int CompareProjection(Vector2d candidate, Vector2d current, Vector2d direction) => |
| | 6 | 159 | | WideGeometry.CompareDifferenceProjection( |
| | 6 | 160 | | candidate.X, |
| | 6 | 161 | | current.X, |
| | 6 | 162 | | direction.X, |
| | 6 | 163 | | candidate.Y, |
| | 6 | 164 | | current.Y, |
| | 6 | 165 | | direction.Y, |
| | 6 | 166 | | Fixed64.Zero, |
| | 6 | 167 | | Fixed64.Zero, |
| | 6 | 168 | | Fixed64.Zero); |
| | | 169 | | |
| | | 170 | | /// <summary> |
| | | 171 | | /// Multiplies two vectors component-wise. |
| | | 172 | | /// </summary> |
| | | 173 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 2 | 174 | | public static Vector2d Multiply(Vector2d v1, Vector2d v2) => v1 * v2; |
| | | 175 | | |
| | | 176 | | /// <summary> |
| | | 177 | | /// Multiplies each vector component by the specified scalar. |
| | | 178 | | /// </summary> |
| | | 179 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 180 | | public static Vector2d Multiply(Vector2d value, Fixed64 factor) => value * factor; |
| | | 181 | | |
| | | 182 | | /// <summary> |
| | | 183 | | /// Divides each component of the first vector by the corresponding component of the second vector. |
| | | 184 | | /// </summary> |
| | | 185 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 186 | | public static Vector2d Divide(Vector2d v1, Vector2d v2) => new(v1.X / v2.X, v1.Y / v2.Y); |
| | | 187 | | |
| | | 188 | | /// <summary> |
| | | 189 | | /// Divides each vector component by the specified scalar. |
| | | 190 | | /// </summary> |
| | | 191 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 192 | | public static Vector2d Divide(Vector2d value, Fixed64 divisor) => value / divisor; |
| | | 193 | | |
| | | 194 | | /// <summary> |
| | | 195 | | /// Clamps each component of the given vector within the specified min and max bounds. |
| | | 196 | | /// </summary> |
| | | 197 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 198 | | public static Vector2d Clamp(Vector2d value, Vector2d min, Vector2d max) => |
| | 4 | 199 | | new(FixedMath.Clamp(value.X, min.X, max.X), |
| | 4 | 200 | | FixedMath.Clamp(value.Y, min.Y, max.Y)); |
| | | 201 | | |
| | | 202 | | /// <summary> |
| | | 203 | | /// Normalizes the given vector, returning a unit vector with the same direction. |
| | | 204 | | /// </summary> |
| | | 205 | | /// <param name="value">The vector to normalize.</param> |
| | | 206 | | /// <returns>A normalized (unit) vector with the same direction.</returns> |
| | | 207 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 208 | | public static Vector2d GetNormalized(Vector2d value) |
| | | 209 | | { |
| | 86 | 210 | | bool magnitudeIsRepresentable = TryGetMagnitude( |
| | 86 | 211 | | value, |
| | 86 | 212 | | out Fixed64 mag, |
| | 86 | 213 | | out bool isNormalized); |
| | | 214 | | |
| | 86 | 215 | | if (mag == Fixed64.Zero) |
| | 1 | 216 | | return new Vector2d(Fixed64.Zero, Fixed64.Zero); |
| | | 217 | | |
| | 85 | 218 | | if (isNormalized) |
| | 26 | 219 | | return value; |
| | | 220 | | |
| | 59 | 221 | | if (!magnitudeIsRepresentable || mag == Fixed64.One) |
| | 3 | 222 | | return WideNormalization.GetNormalized(value); |
| | | 223 | | |
| | 56 | 224 | | if (mag <= FixedMath.ScaleSafeMagnitudeThreshold) |
| | 10 | 225 | | return GetScaleNormalized(value); |
| | | 226 | | |
| | 46 | 227 | | var normalized = new Vector2d( |
| | 46 | 228 | | FixedMath.FastDiv(value.X, mag), |
| | 46 | 229 | | FixedMath.FastDiv(value.Y, mag)); |
| | 46 | 230 | | return normalized.IsNormalized() |
| | 46 | 231 | | ? normalized |
| | 46 | 232 | | : WideNormalization.GetNormalized(value); |
| | | 233 | | } |
| | | 234 | | |
| | | 235 | | internal static Vector2d GetScaleNormalized(Vector2d value) |
| | | 236 | | { |
| | 1148 | 237 | | Fixed64 scale = FixedMath.Max(value.X.Abs(), value.Y.Abs()); |
| | 1148 | 238 | | Vector2d scaled = value / scale; |
| | 1148 | 239 | | Fixed64 scaledMagnitude = FixedMath.GetScaledMagnitude( |
| | 1148 | 240 | | scaled.X, |
| | 1148 | 241 | | scaled.Y, |
| | 1148 | 242 | | Fixed64.Zero, |
| | 1148 | 243 | | Fixed64.Zero); |
| | 1148 | 244 | | return scaled / scaledMagnitude; |
| | | 245 | | } |
| | | 246 | | |
| | | 247 | | /// <summary> |
| | | 248 | | /// Attempts to transform a component-scaled local point by a rotation and |
| | | 249 | | /// world origin with one final round-half-to-even conversion per |
| | | 250 | | /// component. |
| | | 251 | | /// </summary> |
| | | 252 | | public static bool TryTransformScaledPoint( |
| | | 253 | | Vector2d origin, |
| | | 254 | | Vector2d localPoint, |
| | | 255 | | Vector2d scale, |
| | | 256 | | Fixed64 angleInRadians, |
| | | 257 | | out Vector2d result) => |
| | 3 | 258 | | TryTransformScaledPoint( |
| | 3 | 259 | | origin, |
| | 3 | 260 | | localPoint, |
| | 3 | 261 | | scale, |
| | 3 | 262 | | Vector2d.Zero, |
| | 3 | 263 | | angleInRadians, |
| | 3 | 264 | | out result); |
| | | 265 | | |
| | | 266 | | /// <summary> |
| | | 267 | | /// Attempts to transform a component-scaled local point plus an unscaled |
| | | 268 | | /// local displacement by a rotation and world origin with one final |
| | | 269 | | /// round-half-to-even conversion per component. |
| | | 270 | | /// </summary> |
| | | 271 | | /// <remarks> |
| | | 272 | | /// Computes |
| | | 273 | | /// <c>origin + Rotate(scale * localPoint + localDisplacement)</c> without |
| | | 274 | | /// narrowing the scaled point, local sum, or rotated offset independently. |
| | | 275 | | /// </remarks> |
| | | 276 | | public static bool TryTransformScaledPoint( |
| | | 277 | | Vector2d origin, |
| | | 278 | | Vector2d localPoint, |
| | | 279 | | Vector2d scale, |
| | | 280 | | Vector2d localDisplacement, |
| | | 281 | | Fixed64 angleInRadians, |
| | | 282 | | out Vector2d result) => |
| | 69 | 283 | | WideVector2dTransform.TryTransformScaledPoint( |
| | 69 | 284 | | origin, |
| | 69 | 285 | | localPoint, |
| | 69 | 286 | | scale, |
| | 69 | 287 | | localDisplacement, |
| | 69 | 288 | | angleInRadians, |
| | 69 | 289 | | out result); |
| | | 290 | | |
| | | 291 | | /// <summary> |
| | | 292 | | /// Attempts to transform a world point into a component-scaled local frame |
| | | 293 | | /// with one final round-half-to-even conversion per component. |
| | | 294 | | /// </summary> |
| | | 295 | | /// <remarks> |
| | | 296 | | /// Computes <c>InverseRotate(worldPoint - origin) / scale</c> without |
| | | 297 | | /// narrowing the world offset, rotated projection, or scale division |
| | | 298 | | /// independently. A zero scale component or unrepresentable final |
| | | 299 | | /// coordinate returns false and a zero result. |
| | | 300 | | /// </remarks> |
| | | 301 | | public static bool TryInverseTransformScaledPoint( |
| | | 302 | | Vector2d origin, |
| | | 303 | | Vector2d worldPoint, |
| | | 304 | | Vector2d scale, |
| | | 305 | | Fixed64 angleInRadians, |
| | | 306 | | out Vector2d localPoint) => |
| | 71 | 307 | | WideVector2dTransform.TryInverseTransformScaledPoint( |
| | 71 | 308 | | origin, |
| | 71 | 309 | | worldPoint, |
| | 71 | 310 | | scale, |
| | 71 | 311 | | angleInRadians, |
| | 71 | 312 | | out localPoint); |
| | | 313 | | |
| | | 314 | | /// <summary> |
| | | 315 | | /// Attempts to compose two component-scaled offsets in a shared frame and |
| | | 316 | | /// one rotated inner-frame displacement with one final round-half-to-even |
| | | 317 | | /// conversion per component. |
| | | 318 | | /// </summary> |
| | | 319 | | /// <remarks> |
| | | 320 | | /// Computes |
| | | 321 | | /// <c>outerScale * outerLocalPoint |
| | | 322 | | /// + innerFrameScale * innerFrameOffset |
| | | 323 | | /// + Rotate(innerLocalDisplacement)</c> |
| | | 324 | | /// without narrowing either scaled offset or the rotated displacement |
| | | 325 | | /// independently. |
| | | 326 | | /// </remarks> |
| | | 327 | | public static bool TryComposeScaledLocalPoints( |
| | | 328 | | Vector2d outerLocalPoint, |
| | | 329 | | Vector2d outerScale, |
| | | 330 | | Vector2d innerFrameOffset, |
| | | 331 | | Vector2d innerFrameScale, |
| | | 332 | | Vector2d innerLocalDisplacement, |
| | | 333 | | Fixed64 innerAngleInRadians, |
| | | 334 | | out Vector2d result) => |
| | 68 | 335 | | WideVector2dTransform.TryComposeScaledLocalPoints( |
| | 68 | 336 | | outerLocalPoint, |
| | 68 | 337 | | outerScale, |
| | 68 | 338 | | innerFrameOffset, |
| | 68 | 339 | | innerFrameScale, |
| | 68 | 340 | | innerLocalDisplacement, |
| | 68 | 341 | | innerAngleInRadians, |
| | 68 | 342 | | out result); |
| | | 343 | | |
| | | 344 | | /// <summary> |
| | | 345 | | /// Returns the normalized direction from <paramref name="start"/> toward |
| | | 346 | | /// <paramref name="end"/> across the complete coordinate domain. |
| | | 347 | | /// </summary> |
| | | 348 | | /// <remarks> |
| | | 349 | | /// Equal endpoints return <see cref="Zero"/>. Endpoint differences are |
| | | 350 | | /// evaluated exactly even when a component cannot be represented by |
| | | 351 | | /// <see cref="Fixed64"/>. |
| | | 352 | | /// </remarks> |
| | | 353 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 354 | | public static Vector2d GetDirection(Vector2d start, Vector2d end) => |
| | 4 | 355 | | WideNormalization.GetDirection(start, end); |
| | | 356 | | |
| | | 357 | | /// <summary> |
| | | 358 | | /// Returns the magnitude (length) of the given vector. |
| | | 359 | | /// </summary> |
| | | 360 | | /// <param name="vector">The vector to compute the magnitude of.</param> |
| | | 361 | | /// <returns>The magnitude (length) of the vector.</returns> |
| | | 362 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 363 | | public static Fixed64 GetMagnitude(Vector2d vector) |
| | | 364 | | { |
| | 48 | 365 | | _ = TryGetMagnitude(vector, out Fixed64 magnitude); |
| | 48 | 366 | | return magnitude; |
| | | 367 | | } |
| | | 368 | | |
| | | 369 | | /// <summary> |
| | | 370 | | /// Attempts to return the magnitude of the given vector without saturating the result. |
| | | 371 | | /// </summary> |
| | | 372 | | /// <param name="vector">The vector to measure.</param> |
| | | 373 | | /// <param name="magnitude">The magnitude, or <see cref="Fixed64.MaxValue"/> when it is not representable.</param> |
| | | 374 | | /// <returns><see langword="true"/> when the magnitude fits in <see cref="Fixed64"/>; otherwise, <see langword="fals |
| | | 375 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 376 | | public static bool TryGetMagnitude(Vector2d vector, out Fixed64 magnitude) => |
| | 59 | 377 | | TryGetMagnitude(vector, out magnitude, out _); |
| | | 378 | | |
| | | 379 | | private static bool TryGetMagnitude( |
| | | 380 | | Vector2d vector, |
| | | 381 | | out Fixed64 magnitude, |
| | | 382 | | out bool isNormalized) |
| | | 383 | | { |
| | 161 | 384 | | Fixed64 mag = (vector.X * vector.X) + (vector.Y * vector.Y); |
| | 161 | 385 | | isNormalized = mag != Fixed64.Zero |
| | 161 | 386 | | && FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon; |
| | | 387 | | |
| | 161 | 388 | | if (mag == Fixed64.MaxValue) |
| | 13 | 389 | | return FixedMath.TryGetScaledMagnitude( |
| | 13 | 390 | | vector.X, |
| | 13 | 391 | | vector.Y, |
| | 13 | 392 | | Fixed64.Zero, |
| | 13 | 393 | | Fixed64.Zero, |
| | 13 | 394 | | out magnitude); |
| | | 395 | | |
| | 148 | 396 | | if (mag <= FixedMath.ScaleSafeMagnitudeSquaredThreshold) |
| | | 397 | | { |
| | 23 | 398 | | magnitude = FixedMath.GetScaledMagnitude( |
| | 23 | 399 | | vector.X, |
| | 23 | 400 | | vector.Y, |
| | 23 | 401 | | Fixed64.Zero, |
| | 23 | 402 | | Fixed64.Zero); |
| | 23 | 403 | | return true; |
| | | 404 | | } |
| | | 405 | | |
| | 125 | 406 | | if (isNormalized) |
| | | 407 | | { |
| | 35 | 408 | | magnitude = Fixed64.One; |
| | 35 | 409 | | return true; |
| | | 410 | | } |
| | | 411 | | |
| | 90 | 412 | | magnitude = FixedMath.Sqrt(mag); |
| | 90 | 413 | | return true; |
| | | 414 | | } |
| | | 415 | | |
| | | 416 | | /// <summary> |
| | | 417 | | /// Attempts to return the distance between two endpoints without saturating |
| | | 418 | | /// either component difference. |
| | | 419 | | /// </summary> |
| | | 420 | | /// <param name="start">The first endpoint.</param> |
| | | 421 | | /// <param name="end">The second endpoint.</param> |
| | | 422 | | /// <param name="distance">The rounded distance, or <see cref="Fixed64.MaxValue"/> when it is not representable.</pa |
| | | 423 | | /// <returns><see langword="true"/> when the distance fits in <see cref="Fixed64"/>; otherwise, <see langword="false |
| | | 424 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 425 | | public static bool TryGetDistance(Vector2d start, Vector2d end, out Fixed64 distance) => |
| | 4 | 426 | | WideGeometry.TryGetDistance(start, end, out distance); |
| | | 427 | | |
| | | 428 | | /// <summary> |
| | | 429 | | /// Compares the exact squared magnitudes of two vectors without fixed-point saturation. |
| | | 430 | | /// </summary> |
| | | 431 | | /// <returns>A negative value when <paramref name="left"/> is shorter, zero when the |
| | | 432 | | /// magnitudes are equal, or a positive value when <paramref name="left"/> is longer.</returns> |
| | | 433 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 434 | | public static int CompareMagnitudeSquared(Vector2d left, Vector2d right) => |
| | 5 | 435 | | Fixed64.CompareMagnitudeSquared( |
| | 5 | 436 | | left.X, |
| | 5 | 437 | | left.Y, |
| | 5 | 438 | | Fixed64.Zero, |
| | 5 | 439 | | Fixed64.Zero, |
| | 5 | 440 | | right.X, |
| | 5 | 441 | | right.Y, |
| | 5 | 442 | | Fixed64.Zero, |
| | 5 | 443 | | Fixed64.Zero); |
| | | 444 | | |
| | | 445 | | /// <summary> |
| | | 446 | | /// Returns a new <see cref="Vector2d"/> where each component is the absolute value of the corresponding input compo |
| | | 447 | | /// </summary> |
| | | 448 | | /// <param name="value">The input vector.</param> |
| | | 449 | | /// <returns>A vector with absolute values for each component.</returns> |
| | | 450 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 451 | | public static Vector2d Abs(Vector2d value) => new(value.X.Abs(), value.Y.Abs()); |
| | | 452 | | |
| | | 453 | | /// <summary> |
| | | 454 | | /// Returns a new <see cref="Vector2d"/> where each component is the sign of the corresponding input component. |
| | | 455 | | /// </summary> |
| | | 456 | | /// <param name="value">The input vector.</param> |
| | | 457 | | /// <returns>A vector where each component is -1, 0, or 1 based on the sign of the input.</returns> |
| | | 458 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 459 | | public static Vector2d Sign(Vector2d value) => new(value.X.Sign(), value.Y.Sign()); |
| | | 460 | | |
| | | 461 | | /// <summary> |
| | | 462 | | /// Attempts to calculate the exact non-negative weighted average with one |
| | | 463 | | /// final round-half-to-even conversion per component. |
| | | 464 | | /// </summary> |
| | | 465 | | /// <remarks> |
| | | 466 | | /// Zero-weight values are ignored. The operation returns |
| | | 467 | | /// <see langword="false"/> only when the total weight is zero. |
| | | 468 | | /// </remarks> |
| | | 469 | | public static bool TryGetWeightedAverage( |
| | | 470 | | ReadOnlySpan<Vector2d> values, |
| | | 471 | | ReadOnlySpan<Fixed64> weights, |
| | | 472 | | out Vector2d average) |
| | | 473 | | { |
| | 5 | 474 | | WideWeightedAverage.ValidateInputs(values.Length, weights); |
| | 3 | 475 | | return WideWeightedAverage.TryGet(values, weights, out average); |
| | | 476 | | } |
| | | 477 | | |
| | | 478 | | /// <summary> |
| | | 479 | | /// Calculates the arithmetic mean with one final round-half-to-even |
| | | 480 | | /// conversion per component. |
| | | 481 | | /// </summary> |
| | | 482 | | /// <param name="values">The non-empty sequence of vectors to average.</param> |
| | | 483 | | /// <returns>The arithmetic mean of <paramref name="values"/>.</returns> |
| | | 484 | | /// <exception cref="ArgumentException"> |
| | | 485 | | /// <paramref name="values"/> is empty. |
| | | 486 | | /// </exception> |
| | | 487 | | public static Vector2d GetAverage(ReadOnlySpan<Vector2d> values) |
| | | 488 | | { |
| | 103 | 489 | | if (values.IsEmpty) |
| | | 490 | | { |
| | 1 | 491 | | throw new ArgumentException( |
| | 1 | 492 | | "At least one value is required.", |
| | 1 | 493 | | nameof(values)); |
| | | 494 | | } |
| | | 495 | | |
| | 102 | 496 | | return WideWeightedAverage.GetAverage(values); |
| | | 497 | | } |
| | | 498 | | |
| | | 499 | | /// <summary> |
| | | 500 | | /// Creates a vector from a given angle in radians. |
| | | 501 | | /// </summary> |
| | 1 | 502 | | public static Vector2d CreateRotation(Fixed64 angle) => new(FixedMath.Cos(angle), FixedMath.Sin(angle)); |
| | | 503 | | |
| | | 504 | | /// <summary> |
| | | 505 | | /// Linearly interpolates between two vectors. |
| | | 506 | | /// </summary> |
| | | 507 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 508 | | public static Vector2d Lerp(Vector2d a, Vector2d b, Fixed64 amount) |
| | | 509 | | { |
| | 3 | 510 | | amount = FixedMath.Clamp01(amount); |
| | 3 | 511 | | return new Vector2d( |
| | 3 | 512 | | FixedMath.Lerp(a.X, b.X, amount), |
| | 3 | 513 | | FixedMath.Lerp(a.Y, b.Y, amount)); |
| | | 514 | | } |
| | | 515 | | |
| | | 516 | | /// <summary> |
| | | 517 | | /// Calculates a position between three points using barycentric weights for the second and third vertices. |
| | | 518 | | /// </summary> |
| | | 519 | | /// <param name="coordA">The first vertex of the triangle.</param> |
| | | 520 | | /// <param name="coordB">The second vertex of the triangle.</param> |
| | | 521 | | /// <param name="coordC">The third vertex of the triangle.</param> |
| | | 522 | | /// <param name="weightB">The barycentric weight for the second vertex.</param> |
| | | 523 | | /// <param name="weightC">The barycentric weight for the third vertex.</param> |
| | | 524 | | /// <returns>The cartesian translation represented by the barycentric coordinates within the triangle.</returns> |
| | | 525 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 526 | | public static Vector2d BarycentricCoordinates( |
| | | 527 | | Vector2d coordA, |
| | | 528 | | Vector2d coordB, |
| | | 529 | | Vector2d coordC, |
| | | 530 | | Fixed64 weightB, |
| | | 531 | | Fixed64 weightC) |
| | | 532 | | { |
| | 10 | 533 | | return new( |
| | 10 | 534 | | FixedMath.BarycentricCoordinate(coordA.X, coordB.X, coordC.X, weightB, weightC), |
| | 10 | 535 | | FixedMath.BarycentricCoordinate(coordA.Y, coordB.Y, coordC.Y, weightB, weightC)); |
| | | 536 | | } |
| | | 537 | | |
| | | 538 | | /// <summary> |
| | | 539 | | /// Computes the distance between two vectors using the Euclidean distance formula. |
| | | 540 | | /// </summary> |
| | | 541 | | /// <param name="start">The starting vector.</param> |
| | | 542 | | /// <param name="end">The ending vector.</param> |
| | | 543 | | /// <returns>The Euclidean distance between the two vectors.</returns> |
| | | 544 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 4 | 545 | | public static Fixed64 Distance(Vector2d start, Vector2d end) => start.Distance(end); |
| | | 546 | | |
| | | 547 | | /// <summary> |
| | | 548 | | /// Calculates the squared distance between two vectors, avoiding the need for a square root operation. |
| | | 549 | | /// </summary> |
| | | 550 | | /// <returns>The squared distance between the two vectors.</returns> |
| | | 551 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 47 | 552 | | public static Fixed64 DistanceSquared(Vector2d start, Vector2d end) => start.DistanceSquared(end); |
| | | 553 | | |
| | | 554 | | /// <summary> |
| | | 555 | | /// Calculates the closest point on a finite line segment to a given point. |
| | | 556 | | /// </summary> |
| | | 557 | | /// <param name="point">The point to project onto the segment.</param> |
| | | 558 | | /// <param name="start">The start of the line segment.</param> |
| | | 559 | | /// <param name="end">The end of the line segment.</param> |
| | | 560 | | /// <returns>The closest point on the segment to the given point.</returns> |
| | | 561 | | /// <remarks> |
| | | 562 | | /// Zero-length segments deterministically return <paramref name="start"/>. |
| | | 563 | | /// </remarks> |
| | | 564 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 565 | | public static Vector2d ClosestPointOnLineSegment(Vector2d point, Vector2d start, Vector2d end) |
| | | 566 | | { |
| | 110 | 567 | | Fixed64 t = GetClosestPointOnLineSegmentParameter(point, start, end); |
| | 110 | 568 | | return new Vector2d( |
| | 110 | 569 | | FixedMath.Lerp(start.X, end.X, t), |
| | 110 | 570 | | FixedMath.Lerp(start.Y, end.Y, t)); |
| | | 571 | | } |
| | | 572 | | |
| | | 573 | | internal static Fixed64 GetClosestPointOnLineSegmentParameter( |
| | | 574 | | Vector2d point, |
| | | 575 | | Vector2d start, |
| | | 576 | | Vector2d end) |
| | | 577 | | { |
| | 114 | 578 | | Signed192 denominator = WideGeometry.GetDifferenceDotProduct2D( |
| | 114 | 579 | | end.X, start.X, end.Y, start.Y, |
| | 114 | 580 | | end.X, start.X, end.Y, start.Y); |
| | 114 | 581 | | if (denominator.IsZero) |
| | 25 | 582 | | return Fixed64.Zero; |
| | | 583 | | |
| | 89 | 584 | | Signed192 numerator = WideGeometry.GetDifferenceDotProduct2D( |
| | 89 | 585 | | point.X, start.X, point.Y, start.Y, |
| | 89 | 586 | | end.X, start.X, end.Y, start.Y); |
| | 89 | 587 | | if (numerator.Sign <= 0) |
| | 21 | 588 | | return Fixed64.Zero; |
| | 68 | 589 | | if (WideArithmetic.CompareMagnitude(numerator, denominator) >= 0) |
| | 31 | 590 | | return Fixed64.One; |
| | | 591 | | |
| | 37 | 592 | | _ = Fixed64.TryGetUnitIntervalRatio(numerator, denominator, out Fixed64 parameter); |
| | 37 | 593 | | return parameter; |
| | | 594 | | } |
| | | 595 | | |
| | | 596 | | /// <summary> |
| | | 597 | | /// Compares the exact squared distances between two pairs of points without fixed-point saturation. |
| | | 598 | | /// </summary> |
| | | 599 | | /// <returns>A negative value when the left distance is shorter, zero when the |
| | | 600 | | /// distances are equal, or a positive value when the left distance is longer.</returns> |
| | | 601 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 602 | | public static int CompareDistanceSquared( |
| | | 603 | | Vector2d leftStart, |
| | | 604 | | Vector2d leftEnd, |
| | | 605 | | Vector2d rightStart, |
| | | 606 | | Vector2d rightEnd) |
| | | 607 | | { |
| | 45 | 608 | | Signed192 leftDistance = WideGeometry.GetDifferenceDotProduct2D( |
| | 45 | 609 | | leftStart.X, leftEnd.X, leftStart.Y, leftEnd.Y, |
| | 45 | 610 | | leftStart.X, leftEnd.X, leftStart.Y, leftEnd.Y); |
| | 45 | 611 | | Signed192 rightDistance = WideGeometry.GetDifferenceDotProduct2D( |
| | 45 | 612 | | rightStart.X, rightEnd.X, rightStart.Y, rightEnd.Y, |
| | 45 | 613 | | rightStart.X, rightEnd.X, rightStart.Y, rightEnd.Y); |
| | | 614 | | |
| | 45 | 615 | | return WideArithmetic.CompareMagnitude(leftDistance, rightDistance); |
| | | 616 | | } |
| | | 617 | | |
| | | 618 | | /// <summary> |
| | | 619 | | /// Returns the exact orientation sign of the ordered points without fixed-point saturation. |
| | | 620 | | /// </summary> |
| | | 621 | | /// <returns><c>1</c> for a counter-clockwise turn, <c>-1</c> for a clockwise |
| | | 622 | | /// turn, or <c>0</c> when the points are exactly collinear.</returns> |
| | | 623 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 624 | | public static int OrientationSign(Vector2d origin, Vector2d first, Vector2d second) => |
| | 3 | 625 | | WideGeometry.GetDifferenceCrossProduct2D( |
| | 3 | 626 | | first.X, origin.X, first.Y, origin.Y, |
| | 3 | 627 | | second.X, origin.X, second.Y, origin.Y).Sign; |
| | | 628 | | |
| | | 629 | | /// <summary> |
| | | 630 | | /// Calculates the forward direction vector in 2D based on a yaw (angle). |
| | | 631 | | /// </summary> |
| | | 632 | | /// <remarks> |
| | | 633 | | /// This is a polar-angle helper: angle zero points along <see cref="Vector2d.Right"/>. |
| | | 634 | | /// It intentionally differs from the named <see cref="Vector2d.Forward"/> plane constant, |
| | | 635 | | /// which is <c>+Y</c>. |
| | | 636 | | /// </remarks> |
| | | 637 | | /// <param name="angle">The angle in radians representing the rotation in 2D space.</param> |
| | | 638 | | /// <returns>A unit vector representing the forward direction.</returns> |
| | | 639 | | public static Vector2d ForwardDirection(Fixed64 angle) |
| | | 640 | | { |
| | 3 | 641 | | Fixed64 x = FixedMath.Cos(angle); // Forward in the x-direction |
| | 3 | 642 | | Fixed64 y = FixedMath.Sin(angle); // Forward in the y-direction |
| | 3 | 643 | | return new Vector2d(x, y); |
| | | 644 | | } |
| | | 645 | | |
| | | 646 | | /// <summary> |
| | | 647 | | /// Dot Product of two vectors. |
| | | 648 | | /// </summary> |
| | | 649 | | /// <param name="lhs"></param> |
| | | 650 | | /// <param name="rhs"></param> |
| | | 651 | | /// <returns></returns> |
| | | 652 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 217 | 653 | | public static Fixed64 Dot(Vector2d lhs, Vector2d rhs) => lhs.Dot(rhs.X, rhs.Y); |
| | | 654 | | |
| | | 655 | | /// <summary> |
| | | 656 | | /// Cross Product of two vectors. |
| | | 657 | | /// </summary> |
| | | 658 | | /// <param name="lhs"></param> |
| | | 659 | | /// <param name="rhs"></param> |
| | | 660 | | /// <returns></returns> |
| | | 661 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 662 | | public static Fixed64 CrossProduct(Vector2d lhs, Vector2d rhs) => lhs.CrossProduct(rhs); |
| | | 663 | | |
| | | 664 | | /// <summary> |
| | | 665 | | /// Rotates this vector by the specified angle (in radians). |
| | | 666 | | /// </summary> |
| | | 667 | | /// <param name="vec">The vector to rotate.</param> |
| | | 668 | | /// <param name="angleInRadians">The angle in radians.</param> |
| | | 669 | | /// <returns>The rotated vector.</returns> |
| | | 670 | | public static Vector2d Rotate(Vector2d vec, Fixed64 angleInRadians) |
| | | 671 | | { |
| | 16 | 672 | | Fixed64 cos = FixedMath.Cos(angleInRadians); |
| | 16 | 673 | | Fixed64 sin = FixedMath.Sin(angleInRadians); |
| | 16 | 674 | | return new Vector2d( |
| | 16 | 675 | | vec.X * cos - vec.Y * sin, |
| | 16 | 676 | | vec.X * sin + vec.Y * cos |
| | 16 | 677 | | ); |
| | | 678 | | } |
| | | 679 | | |
| | | 680 | | /// <summary> |
| | | 681 | | /// Attempts to rotate a vector by the specified angle with one final |
| | | 682 | | /// round-half-to-even conversion per component. |
| | | 683 | | /// </summary> |
| | | 684 | | /// <returns> |
| | | 685 | | /// <see langword="true"/> when both final components are representable; |
| | | 686 | | /// otherwise, <see langword="false"/> and <paramref name="result"/> is |
| | | 687 | | /// <see langword="default"/>. |
| | | 688 | | /// </returns> |
| | | 689 | | public static bool TryRotate( |
| | | 690 | | Vector2d vector, |
| | | 691 | | Fixed64 angleInRadians, |
| | | 692 | | out Vector2d result) |
| | | 693 | | { |
| | 178 | 694 | | Fixed64 cosine = FixedMath.Cos(angleInRadians); |
| | 178 | 695 | | Fixed64 sine = FixedMath.Sin(angleInRadians); |
| | 178 | 696 | | bool representable = Fixed64.TrySubtractProducts( |
| | 178 | 697 | | vector.X, |
| | 178 | 698 | | cosine, |
| | 178 | 699 | | vector.Y, |
| | 178 | 700 | | sine, |
| | 178 | 701 | | out Fixed64 x) |
| | 178 | 702 | | & Fixed64.TryAddProducts( |
| | 178 | 703 | | vector.X, |
| | 178 | 704 | | sine, |
| | 178 | 705 | | vector.Y, |
| | 178 | 706 | | cosine, |
| | 178 | 707 | | out Fixed64 y); |
| | 178 | 708 | | if (!representable) |
| | | 709 | | { |
| | 2 | 710 | | result = default; |
| | 2 | 711 | | return false; |
| | | 712 | | } |
| | | 713 | | |
| | 176 | 714 | | result = new Vector2d(x, y); |
| | 176 | 715 | | return true; |
| | | 716 | | } |
| | | 717 | | |
| | | 718 | | /// <summary> |
| | | 719 | | /// Attempts to transform a local point by a rotation and origin with one |
| | | 720 | | /// final round-half-to-even conversion per component. |
| | | 721 | | /// </summary> |
| | | 722 | | /// <returns> |
| | | 723 | | /// <see langword="true"/> when both final world components are |
| | | 724 | | /// representable; otherwise, <see langword="false"/> and |
| | | 725 | | /// <paramref name="result"/> is <see langword="default"/>. |
| | | 726 | | /// </returns> |
| | | 727 | | public static bool TryTransformPoint( |
| | | 728 | | Vector2d origin, |
| | | 729 | | Vector2d localPoint, |
| | | 730 | | Fixed64 angleInRadians, |
| | | 731 | | out Vector2d result) |
| | | 732 | | { |
| | 70 | 733 | | if (angleInRadians == Fixed64.Zero) |
| | 35 | 734 | | return TryAdd(origin, localPoint, out result); |
| | | 735 | | |
| | 35 | 736 | | return WideVector2dTransform.TryTransformPoint( |
| | 35 | 737 | | origin, |
| | 35 | 738 | | localPoint, |
| | 35 | 739 | | angleInRadians, |
| | 35 | 740 | | out result); |
| | | 741 | | } |
| | | 742 | | |
| | | 743 | | /// <summary> |
| | | 744 | | /// Attempts to transform a local point by a rotation and add two origins |
| | | 745 | | /// with one final round-half-to-even conversion per component. |
| | | 746 | | /// </summary> |
| | | 747 | | /// <returns> |
| | | 748 | | /// <see langword="true"/> when both final world components are |
| | | 749 | | /// representable; otherwise, <see langword="false"/> and |
| | | 750 | | /// <paramref name="result"/> is <see langword="default"/>. |
| | | 751 | | /// </returns> |
| | | 752 | | public static bool TryTransformPoint( |
| | | 753 | | Vector2d firstOrigin, |
| | | 754 | | Vector2d secondOrigin, |
| | | 755 | | Vector2d localPoint, |
| | | 756 | | Fixed64 angleInRadians, |
| | | 757 | | out Vector2d result) => |
| | 69 | 758 | | WideVector2dTransform.TryTransformPoint( |
| | 69 | 759 | | firstOrigin, |
| | 69 | 760 | | secondOrigin, |
| | 69 | 761 | | localPoint, |
| | 69 | 762 | | angleInRadians, |
| | 69 | 763 | | out result); |
| | | 764 | | |
| | | 765 | | /// <summary> |
| | | 766 | | /// Attempts to obtain the exact relative offset |
| | | 767 | | /// <c>firstOrigin + firstOffset - secondOrigin - Rotate(secondLocalPoint)</c>. |
| | | 768 | | /// </summary> |
| | | 769 | | /// <remarks> |
| | | 770 | | /// No rotated point or intermediate sum is narrowed independently. |
| | | 771 | | /// </remarks> |
| | | 772 | | /// <returns> |
| | | 773 | | /// <see langword="true"/> when both final components are representable; |
| | | 774 | | /// otherwise, <see langword="false"/> and <paramref name="result"/> is |
| | | 775 | | /// <see langword="default"/>. |
| | | 776 | | /// </returns> |
| | | 777 | | public static bool TryGetRelativeOffset( |
| | | 778 | | Vector2d firstOrigin, |
| | | 779 | | Vector2d firstOffset, |
| | | 780 | | Vector2d secondOrigin, |
| | | 781 | | Vector2d secondLocalPoint, |
| | | 782 | | Fixed64 angleInRadians, |
| | | 783 | | out Vector2d result) |
| | | 784 | | { |
| | 70 | 785 | | if (angleInRadians == Fixed64.Zero) |
| | | 786 | | { |
| | 35 | 787 | | return TrySubtractSums( |
| | 35 | 788 | | firstOrigin, |
| | 35 | 789 | | firstOffset, |
| | 35 | 790 | | secondOrigin, |
| | 35 | 791 | | secondLocalPoint, |
| | 35 | 792 | | out result); |
| | | 793 | | } |
| | | 794 | | |
| | 35 | 795 | | return WideVector2dTransform.TryGetRelativeOffset( |
| | 35 | 796 | | firstOrigin, |
| | 35 | 797 | | firstOffset, |
| | 35 | 798 | | secondOrigin, |
| | 35 | 799 | | secondLocalPoint, |
| | 35 | 800 | | angleInRadians, |
| | 35 | 801 | | out result); |
| | | 802 | | } |
| | | 803 | | |
| | | 804 | | /// <summary> |
| | | 805 | | /// Attempts to obtain the exact relative offset between two transformed |
| | | 806 | | /// local points. |
| | | 807 | | /// </summary> |
| | | 808 | | /// <remarks> |
| | | 809 | | /// The computed relation is |
| | | 810 | | /// <c>firstOrigin + Rotate(firstLocalPoint, firstAngleInRadians) |
| | | 811 | | /// - secondOrigin - Rotate(secondLocalPoint, secondAngleInRadians)</c>. |
| | | 812 | | /// Neither rotated point nor any intermediate sum is narrowed |
| | | 813 | | /// independently. |
| | | 814 | | /// </remarks> |
| | | 815 | | /// <returns> |
| | | 816 | | /// <see langword="true"/> when both final components are representable; |
| | | 817 | | /// otherwise, <see langword="false"/> and <paramref name="result"/> is |
| | | 818 | | /// <see langword="default"/>. |
| | | 819 | | /// </returns> |
| | | 820 | | public static bool TryGetRelativeOffset( |
| | | 821 | | Vector2d firstOrigin, |
| | | 822 | | Vector2d firstLocalPoint, |
| | | 823 | | Fixed64 firstAngleInRadians, |
| | | 824 | | Vector2d secondOrigin, |
| | | 825 | | Vector2d secondLocalPoint, |
| | | 826 | | Fixed64 secondAngleInRadians, |
| | | 827 | | out Vector2d result) |
| | | 828 | | { |
| | 69 | 829 | | if (firstAngleInRadians == Fixed64.Zero |
| | 69 | 830 | | && secondAngleInRadians == Fixed64.Zero) |
| | | 831 | | { |
| | 34 | 832 | | return TrySubtractSums( |
| | 34 | 833 | | firstOrigin, |
| | 34 | 834 | | firstLocalPoint, |
| | 34 | 835 | | secondOrigin, |
| | 34 | 836 | | secondLocalPoint, |
| | 34 | 837 | | out result); |
| | | 838 | | } |
| | | 839 | | |
| | 35 | 840 | | return WideVector2dTransform.TryGetRelativeOffset( |
| | 35 | 841 | | firstOrigin, |
| | 35 | 842 | | firstLocalPoint, |
| | 35 | 843 | | firstAngleInRadians, |
| | 35 | 844 | | secondOrigin, |
| | 35 | 845 | | secondLocalPoint, |
| | 35 | 846 | | secondAngleInRadians, |
| | 35 | 847 | | out result); |
| | | 848 | | } |
| | | 849 | | } |