| | | 1 | | //======================================================================= |
| | | 2 | | // Vector3d.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 factory methods and arithmetic operations for <see cref="Vector3d"/>. |
| | | 16 | | /// </content> |
| | | 17 | | public partial struct Vector3d |
| | | 18 | | { |
| | | 19 | | #region Static Operations |
| | | 20 | | |
| | | 21 | | /// <summary> |
| | | 22 | | /// Adds two vectors component-wise. |
| | | 23 | | /// </summary> |
| | | 24 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 25 | | public static Vector3d Add(Vector3d v1, Vector3d v2) => v1 + v2; |
| | | 26 | | |
| | | 27 | | /// <summary> |
| | | 28 | | /// Attempts to add two vectors without component saturation. |
| | | 29 | | /// </summary> |
| | | 30 | | /// <param name="left">The left operand.</param> |
| | | 31 | | /// <param name="right">The right operand.</param> |
| | | 32 | | /// <param name="result"> |
| | | 33 | | /// The exact component-wise sum when every component is representable; otherwise, <see langword="default"/>. |
| | | 34 | | /// </param> |
| | | 35 | | /// <returns> |
| | | 36 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 37 | | /// </returns> |
| | | 38 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 39 | | public static bool TryAdd(Vector3d left, Vector3d right, out Vector3d result) |
| | | 40 | | { |
| | 213 | 41 | | if (!Fixed64.TryAdd(left.X, right.X, out Fixed64 x) |
| | 213 | 42 | | || !Fixed64.TryAdd(left.Y, right.Y, out Fixed64 y) |
| | 213 | 43 | | || !Fixed64.TryAdd(left.Z, right.Z, out Fixed64 z)) |
| | | 44 | | { |
| | 13 | 45 | | result = default; |
| | 13 | 46 | | return false; |
| | | 47 | | } |
| | | 48 | | |
| | 200 | 49 | | result = new Vector3d(x, y, z); |
| | 200 | 50 | | return true; |
| | | 51 | | } |
| | | 52 | | |
| | | 53 | | /// <summary> |
| | | 54 | | /// Subtracts two vectors component-wise. |
| | | 55 | | /// </summary> |
| | | 56 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 57 | | public static Vector3d Subtract(Vector3d v1, Vector3d v2) => v1 - v2; |
| | | 58 | | |
| | | 59 | | /// <summary> |
| | | 60 | | /// Attempts to subtract two vectors without component saturation. |
| | | 61 | | /// </summary> |
| | | 62 | | /// <param name="left">The left operand.</param> |
| | | 63 | | /// <param name="right">The right operand.</param> |
| | | 64 | | /// <param name="result"> |
| | | 65 | | /// The exact component-wise difference when every component is representable; otherwise, <see langword="default"/>. |
| | | 66 | | /// </param> |
| | | 67 | | /// <returns> |
| | | 68 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 69 | | /// </returns> |
| | | 70 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 71 | | public static bool TrySubtract(Vector3d left, Vector3d right, out Vector3d result) |
| | | 72 | | { |
| | 1259 | 73 | | if (!Fixed64.TrySubtract(left.X, right.X, out Fixed64 x) |
| | 1259 | 74 | | || !Fixed64.TrySubtract(left.Y, right.Y, out Fixed64 y) |
| | 1259 | 75 | | || !Fixed64.TrySubtract(left.Z, right.Z, out Fixed64 z)) |
| | | 76 | | { |
| | 6 | 77 | | result = default; |
| | 6 | 78 | | return false; |
| | | 79 | | } |
| | | 80 | | |
| | 1253 | 81 | | result = new Vector3d(x, y, z); |
| | 1253 | 82 | | return true; |
| | | 83 | | } |
| | | 84 | | |
| | | 85 | | /// <summary> |
| | | 86 | | /// Attempts to add two vectors and subtract a third component-wise without intermediate saturation. |
| | | 87 | | /// </summary> |
| | | 88 | | /// <param name="firstAddend">The first addend.</param> |
| | | 89 | | /// <param name="secondAddend">The second addend.</param> |
| | | 90 | | /// <param name="subtrahend">The vector to subtract from the exact component-wise sum.</param> |
| | | 91 | | /// <param name="result"> |
| | | 92 | | /// The exact component-wise result when every component is representable; otherwise, <see langword="default"/>. |
| | | 93 | | /// </param> |
| | | 94 | | /// <returns> |
| | | 95 | | /// <see langword="true"/> when every exact component is representable; otherwise, <see langword="false"/>. |
| | | 96 | | /// </returns> |
| | | 97 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 98 | | public static bool TryAddSubtract( |
| | | 99 | | Vector3d firstAddend, |
| | | 100 | | Vector3d secondAddend, |
| | | 101 | | Vector3d subtrahend, |
| | | 102 | | out Vector3d result) |
| | | 103 | | { |
| | 140 | 104 | | if (!Fixed64.TryAddSubtract(firstAddend.X, secondAddend.X, subtrahend.X, out Fixed64 x) |
| | 140 | 105 | | || !Fixed64.TryAddSubtract(firstAddend.Y, secondAddend.Y, subtrahend.Y, out Fixed64 y) |
| | 140 | 106 | | || !Fixed64.TryAddSubtract(firstAddend.Z, secondAddend.Z, subtrahend.Z, out Fixed64 z)) |
| | | 107 | | { |
| | 16 | 108 | | result = default; |
| | 16 | 109 | | return false; |
| | | 110 | | } |
| | | 111 | | |
| | 124 | 112 | | result = new Vector3d(x, y, z); |
| | 124 | 113 | | return true; |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | /// <summary> |
| | | 117 | | /// Attempts to compute <c>(firstLeft + firstRight) - |
| | | 118 | | /// (secondLeft + secondRight)</c> component-wise without intermediate |
| | | 119 | | /// saturation. |
| | | 120 | | /// </summary> |
| | | 121 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 122 | | public static bool TrySubtractSums( |
| | | 123 | | Vector3d firstLeft, |
| | | 124 | | Vector3d firstRight, |
| | | 125 | | Vector3d secondLeft, |
| | | 126 | | Vector3d secondRight, |
| | | 127 | | out Vector3d result) |
| | | 128 | | { |
| | 93 | 129 | | bool representable = Fixed64.TrySubtractSums( |
| | 93 | 130 | | firstLeft.X, |
| | 93 | 131 | | firstRight.X, |
| | 93 | 132 | | secondLeft.X, |
| | 93 | 133 | | secondRight.X, |
| | 93 | 134 | | out Fixed64 x) |
| | 93 | 135 | | & Fixed64.TrySubtractSums( |
| | 93 | 136 | | firstLeft.Y, |
| | 93 | 137 | | firstRight.Y, |
| | 93 | 138 | | secondLeft.Y, |
| | 93 | 139 | | secondRight.Y, |
| | 93 | 140 | | out Fixed64 y) |
| | 93 | 141 | | & Fixed64.TrySubtractSums( |
| | 93 | 142 | | firstLeft.Z, |
| | 93 | 143 | | firstRight.Z, |
| | 93 | 144 | | secondLeft.Z, |
| | 93 | 145 | | secondRight.Z, |
| | 93 | 146 | | out Fixed64 z); |
| | 93 | 147 | | if (!representable) |
| | | 148 | | { |
| | 2 | 149 | | result = default; |
| | 2 | 150 | | return false; |
| | | 151 | | } |
| | | 152 | | |
| | 91 | 153 | | result = new Vector3d(x, y, z); |
| | 91 | 154 | | return true; |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | /// <summary> |
| | | 158 | | /// Attempts to calculate the cross product with one final |
| | | 159 | | /// round-half-to-even conversion per component. |
| | | 160 | | /// </summary> |
| | | 161 | | public static bool TryCross( |
| | | 162 | | Vector3d left, |
| | | 163 | | Vector3d right, |
| | | 164 | | out Vector3d result) |
| | | 165 | | { |
| | 67 | 166 | | bool representable = Fixed64.TrySubtractProducts( |
| | 67 | 167 | | left.Y, |
| | 67 | 168 | | right.Z, |
| | 67 | 169 | | left.Z, |
| | 67 | 170 | | right.Y, |
| | 67 | 171 | | out Fixed64 x) |
| | 67 | 172 | | & Fixed64.TrySubtractProducts( |
| | 67 | 173 | | left.Z, |
| | 67 | 174 | | right.X, |
| | 67 | 175 | | left.X, |
| | 67 | 176 | | right.Z, |
| | 67 | 177 | | out Fixed64 y) |
| | 67 | 178 | | & Fixed64.TrySubtractProducts( |
| | 67 | 179 | | left.X, |
| | 67 | 180 | | right.Y, |
| | 67 | 181 | | left.Y, |
| | 67 | 182 | | right.X, |
| | 67 | 183 | | out Fixed64 z); |
| | 67 | 184 | | if (!representable) |
| | | 185 | | { |
| | 1 | 186 | | result = default; |
| | 1 | 187 | | return false; |
| | | 188 | | } |
| | | 189 | | |
| | 66 | 190 | | result = new Vector3d(x, y, z); |
| | 66 | 191 | | return true; |
| | | 192 | | } |
| | | 193 | | |
| | | 194 | | /// <summary> |
| | | 195 | | /// Attempts to calculate the dot product with one final |
| | | 196 | | /// round-half-to-even conversion. |
| | | 197 | | /// </summary> |
| | | 198 | | public static bool TryDot( |
| | | 199 | | Vector3d left, |
| | | 200 | | Vector3d right, |
| | | 201 | | out Fixed64 result) => |
| | 67 | 202 | | Fixed64.TryAddProducts( |
| | 67 | 203 | | left.X, |
| | 67 | 204 | | right.X, |
| | 67 | 205 | | left.Y, |
| | 67 | 206 | | right.Y, |
| | 67 | 207 | | left.Z, |
| | 67 | 208 | | right.Z, |
| | 67 | 209 | | out result); |
| | | 210 | | |
| | | 211 | | /// <summary> |
| | | 212 | | /// Attempts to combine two scaled vectors with one final |
| | | 213 | | /// round-half-to-even conversion per component. |
| | | 214 | | /// </summary> |
| | | 215 | | public static bool TryLinearCombination( |
| | | 216 | | Vector3d first, |
| | | 217 | | Fixed64 firstScale, |
| | | 218 | | Vector3d second, |
| | | 219 | | Fixed64 secondScale, |
| | | 220 | | out Vector3d result) |
| | | 221 | | { |
| | 2 | 222 | | bool representable = Fixed64.TryAddProducts( |
| | 2 | 223 | | first.X, |
| | 2 | 224 | | firstScale, |
| | 2 | 225 | | second.X, |
| | 2 | 226 | | secondScale, |
| | 2 | 227 | | out Fixed64 x) |
| | 2 | 228 | | & Fixed64.TryAddProducts( |
| | 2 | 229 | | first.Y, |
| | 2 | 230 | | firstScale, |
| | 2 | 231 | | second.Y, |
| | 2 | 232 | | secondScale, |
| | 2 | 233 | | out Fixed64 y) |
| | 2 | 234 | | & Fixed64.TryAddProducts( |
| | 2 | 235 | | first.Z, |
| | 2 | 236 | | firstScale, |
| | 2 | 237 | | second.Z, |
| | 2 | 238 | | secondScale, |
| | 2 | 239 | | out Fixed64 z); |
| | 2 | 240 | | if (!representable) |
| | | 241 | | { |
| | 1 | 242 | | result = default; |
| | 1 | 243 | | return false; |
| | | 244 | | } |
| | | 245 | | |
| | 1 | 246 | | result = new Vector3d(x, y, z); |
| | 1 | 247 | | return true; |
| | | 248 | | } |
| | | 249 | | |
| | | 250 | | /// <summary> |
| | | 251 | | /// Attempts to combine three scaled vectors with one final |
| | | 252 | | /// round-half-to-even conversion per component. |
| | | 253 | | /// </summary> |
| | | 254 | | public static bool TryLinearCombination( |
| | | 255 | | Vector3d first, |
| | | 256 | | Fixed64 firstScale, |
| | | 257 | | Vector3d second, |
| | | 258 | | Fixed64 secondScale, |
| | | 259 | | Vector3d third, |
| | | 260 | | Fixed64 thirdScale, |
| | | 261 | | out Vector3d result) |
| | | 262 | | { |
| | 67 | 263 | | bool representable = Fixed64.TryAddProducts( |
| | 67 | 264 | | first.X, |
| | 67 | 265 | | firstScale, |
| | 67 | 266 | | second.X, |
| | 67 | 267 | | secondScale, |
| | 67 | 268 | | third.X, |
| | 67 | 269 | | thirdScale, |
| | 67 | 270 | | out Fixed64 x) |
| | 67 | 271 | | & Fixed64.TryAddProducts( |
| | 67 | 272 | | first.Y, |
| | 67 | 273 | | firstScale, |
| | 67 | 274 | | second.Y, |
| | 67 | 275 | | secondScale, |
| | 67 | 276 | | third.Y, |
| | 67 | 277 | | thirdScale, |
| | 67 | 278 | | out Fixed64 y) |
| | 67 | 279 | | & Fixed64.TryAddProducts( |
| | 67 | 280 | | first.Z, |
| | 67 | 281 | | firstScale, |
| | 67 | 282 | | second.Z, |
| | 67 | 283 | | secondScale, |
| | 67 | 284 | | third.Z, |
| | 67 | 285 | | thirdScale, |
| | 67 | 286 | | out Fixed64 z); |
| | 67 | 287 | | if (!representable) |
| | | 288 | | { |
| | 1 | 289 | | result = default; |
| | 1 | 290 | | return false; |
| | | 291 | | } |
| | | 292 | | |
| | 66 | 293 | | result = new Vector3d(x, y, z); |
| | 66 | 294 | | return true; |
| | | 295 | | } |
| | | 296 | | |
| | | 297 | | /// <summary> |
| | | 298 | | /// Attempts to combine three scaled vectors, apply a final scale, and |
| | | 299 | | /// round each completed component once. |
| | | 300 | | /// </summary> |
| | | 301 | | public static bool TryScaledLinearCombination( |
| | | 302 | | Vector3d first, |
| | | 303 | | Fixed64 firstScale, |
| | | 304 | | Vector3d second, |
| | | 305 | | Fixed64 secondScale, |
| | | 306 | | Vector3d third, |
| | | 307 | | Fixed64 thirdScale, |
| | | 308 | | Fixed64 resultScale, |
| | | 309 | | out Vector3d result) |
| | | 310 | | { |
| | 4 | 311 | | bool representable = Fixed64.TryAddScaledProducts( |
| | 4 | 312 | | first.X, |
| | 4 | 313 | | firstScale, |
| | 4 | 314 | | second.X, |
| | 4 | 315 | | secondScale, |
| | 4 | 316 | | third.X, |
| | 4 | 317 | | thirdScale, |
| | 4 | 318 | | resultScale, |
| | 4 | 319 | | out Fixed64 x) |
| | 4 | 320 | | & Fixed64.TryAddScaledProducts( |
| | 4 | 321 | | first.Y, |
| | 4 | 322 | | firstScale, |
| | 4 | 323 | | second.Y, |
| | 4 | 324 | | secondScale, |
| | 4 | 325 | | third.Y, |
| | 4 | 326 | | thirdScale, |
| | 4 | 327 | | resultScale, |
| | 4 | 328 | | out Fixed64 y) |
| | 4 | 329 | | & Fixed64.TryAddScaledProducts( |
| | 4 | 330 | | first.Z, |
| | 4 | 331 | | firstScale, |
| | 4 | 332 | | second.Z, |
| | 4 | 333 | | secondScale, |
| | 4 | 334 | | third.Z, |
| | 4 | 335 | | thirdScale, |
| | 4 | 336 | | resultScale, |
| | 4 | 337 | | out Fixed64 z); |
| | 4 | 338 | | if (!representable) |
| | | 339 | | { |
| | 2 | 340 | | result = default; |
| | 2 | 341 | | return false; |
| | | 342 | | } |
| | | 343 | | |
| | 2 | 344 | | result = new Vector3d(x, y, z); |
| | 2 | 345 | | return true; |
| | | 346 | | } |
| | | 347 | | |
| | | 348 | | /// <summary> |
| | | 349 | | /// Compares the exact component-difference projections of two vectors. |
| | | 350 | | /// </summary> |
| | | 351 | | /// <param name="candidate">The candidate point.</param> |
| | | 352 | | /// <param name="current">The point to compare against.</param> |
| | | 353 | | /// <param name="direction">The projection direction; it need not be normalized.</param> |
| | | 354 | | /// <returns> |
| | | 355 | | /// A negative value, zero, or a positive value when the candidate projection is |
| | | 356 | | /// respectively less than, equal to, or greater than the current projection. |
| | | 357 | | /// </returns> |
| | | 358 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 359 | | public static int CompareProjection(Vector3d candidate, Vector3d current, Vector3d direction) => |
| | 5 | 360 | | WideGeometry.CompareDifferenceProjection( |
| | 5 | 361 | | candidate.X, |
| | 5 | 362 | | current.X, |
| | 5 | 363 | | direction.X, |
| | 5 | 364 | | candidate.Y, |
| | 5 | 365 | | current.Y, |
| | 5 | 366 | | direction.Y, |
| | 5 | 367 | | candidate.Z, |
| | 5 | 368 | | current.Z, |
| | 5 | 369 | | direction.Z); |
| | | 370 | | |
| | | 371 | | /// <summary> |
| | | 372 | | /// Projects <paramref name="target"/> minus <paramref name="source"/> onto a direction |
| | | 373 | | /// without intermediate saturation, then returns a conservative nonnegative result. |
| | | 374 | | /// </summary> |
| | | 375 | | /// <param name="target">The target point.</param> |
| | | 376 | | /// <param name="source">The source point.</param> |
| | | 377 | | /// <param name="direction">The projection direction; it need not be normalized.</param> |
| | | 378 | | /// <returns> |
| | | 379 | | /// Zero for a nonpositive projection, the positive projection floored to Q32.32, or |
| | | 380 | | /// <see cref="Fixed64.MaxValue"/> when only the final result is unrepresentable. |
| | | 381 | | /// </returns> |
| | | 382 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 383 | | public static Fixed64 ProjectNonNegativeDifference( |
| | | 384 | | Vector3d target, |
| | | 385 | | Vector3d source, |
| | | 386 | | Vector3d direction) => |
| | 8 | 387 | | Fixed64.ProjectNonNegativeDifference( |
| | 8 | 388 | | target.X, |
| | 8 | 389 | | source.X, |
| | 8 | 390 | | direction.X, |
| | 8 | 391 | | target.Y, |
| | 8 | 392 | | source.Y, |
| | 8 | 393 | | direction.Y, |
| | 8 | 394 | | target.Z, |
| | 8 | 395 | | source.Z, |
| | 8 | 396 | | direction.Z); |
| | | 397 | | |
| | | 398 | | /// <summary> |
| | | 399 | | /// Projects <paramref name="target"/> minus <paramref name="source"/> onto |
| | | 400 | | /// <paramref name="direction"/> and returns the nonnegative parametric |
| | | 401 | | /// coordinate along that direction. |
| | | 402 | | /// </summary> |
| | | 403 | | /// <remarks> |
| | | 404 | | /// Unlike <see cref="ProjectNonNegativeDifference"/>, this method divides |
| | | 405 | | /// the exact dot product by the direction's exact squared length. The |
| | | 406 | | /// direction therefore need not have an exactly unit squared length. |
| | | 407 | | /// Zero and negative projections return zero; an unrepresentable positive |
| | | 408 | | /// parameter saturates to <see cref="Fixed64.MaxValue"/>. |
| | | 409 | | /// </remarks> |
| | | 410 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 411 | | public static Fixed64 ProjectNonNegativeDifferenceParameter( |
| | | 412 | | Vector3d target, |
| | | 413 | | Vector3d source, |
| | | 414 | | Vector3d direction) => |
| | 19 | 415 | | WideGeometry.GetNonNegativeDifferenceProjectionParameter(target, source, direction); |
| | | 416 | | |
| | | 417 | | /// <summary> |
| | | 418 | | /// Multiplies two vectors component-wise. |
| | | 419 | | /// </summary> |
| | | 420 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 2 | 421 | | public static Vector3d Multiply(Vector3d v1, Vector3d v2) => v1 * v2; |
| | | 422 | | |
| | | 423 | | /// <summary> |
| | | 424 | | /// Multiplies each vector component by the specified scalar. |
| | | 425 | | /// </summary> |
| | | 426 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 427 | | public static Vector3d Multiply(Vector3d value, Fixed64 factor) => value * factor; |
| | | 428 | | |
| | | 429 | | /// <summary> |
| | | 430 | | /// Divides each component of the first vector by the corresponding component of the second vector. |
| | | 431 | | /// </summary> |
| | | 432 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 433 | | public static Vector3d Divide(Vector3d v1, Vector3d v2) => v1 / v2; |
| | | 434 | | |
| | | 435 | | /// <summary> |
| | | 436 | | /// Divides each vector component by the specified scalar. |
| | | 437 | | /// </summary> |
| | | 438 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 439 | | public static Vector3d Divide(Vector3d value, Fixed64 divisor) => value / divisor; |
| | | 440 | | |
| | | 441 | | /// <summary> |
| | | 442 | | /// Linearly interpolates between two points. |
| | | 443 | | /// </summary> |
| | | 444 | | /// <param name="a">Start value, returned when t = 0.</param> |
| | | 445 | | /// <param name="b">End value, returned when t = 1.</param> |
| | | 446 | | /// <param name="mag">Value used to interpolate between a and b.</param> |
| | | 447 | | /// <returns> Interpolated value, equals to a + (b - a) * t.</returns> |
| | | 448 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 449 | | public static Vector3d Lerp(Vector3d a, Vector3d b, Fixed64 mag) |
| | | 450 | | { |
| | 489 | 451 | | mag = FixedMath.Clamp01(mag); |
| | 489 | 452 | | return new Vector3d( |
| | 489 | 453 | | FixedMath.Lerp(a.X, b.X, mag), |
| | 489 | 454 | | FixedMath.Lerp(a.Y, b.Y, mag), |
| | 489 | 455 | | FixedMath.Lerp(a.Z, b.Z, mag)); |
| | | 456 | | } |
| | | 457 | | |
| | | 458 | | /// <summary> |
| | | 459 | | /// Linearly interpolates between two vectors without clamping the interpolation factor between 0 and 1. |
| | | 460 | | /// </summary> |
| | | 461 | | /// <param name="a">The start vector.</param> |
| | | 462 | | /// <param name="b">The end vector.</param> |
| | | 463 | | /// <param name="t">The interpolation factor. Values outside the range [0, 1] will cause the interpolation to go bey |
| | | 464 | | /// <returns>The interpolated vector.</returns> |
| | | 465 | | /// <remarks> |
| | | 466 | | /// Unlike traditional Lerp, this function allows interpolation factors greater than 1 or less than 0, |
| | | 467 | | /// which means the resulting vector can extend beyond the endpoints. |
| | | 468 | | /// </remarks> |
| | 4 | 469 | | public static Vector3d UnclampedLerp(Vector3d a, Vector3d b, Fixed64 t) => (b - a) * t + a; |
| | | 470 | | |
| | | 471 | | /// <summary> |
| | | 472 | | /// Moves from a to b at some speed dependent of a delta time with out passing b. |
| | | 473 | | /// </summary> |
| | | 474 | | /// <param name="a"></param> |
| | | 475 | | /// <param name="b"></param> |
| | | 476 | | /// <param name="speed"></param> |
| | | 477 | | /// <param name="dt"></param> |
| | | 478 | | /// <returns></returns> |
| | | 479 | | public static Vector3d SpeedLerp(Vector3d a, Vector3d b, Fixed64 speed, Fixed64 dt) |
| | | 480 | | { |
| | 3 | 481 | | Vector3d v = b - a; |
| | 3 | 482 | | Fixed64 dv = speed * dt; |
| | 3 | 483 | | return dv > v.Magnitude |
| | 3 | 484 | | ? b |
| | 3 | 485 | | : a + v.Normalized * dv; |
| | | 486 | | } |
| | | 487 | | |
| | | 488 | | /// <summary> |
| | | 489 | | /// Spherically interpolates between two vectors, moving along the shortest arc on a unit sphere. |
| | | 490 | | /// </summary> |
| | | 491 | | /// <param name="start">The starting vector.</param> |
| | | 492 | | /// <param name="end">The ending vector.</param> |
| | | 493 | | /// <param name="percent">A value between 0 and 1 that represents the interpolation amount. 0 returns the start vect |
| | | 494 | | /// <returns>The interpolated vector between the two input vectors.</returns> |
| | | 495 | | /// <remarks> |
| | | 496 | | /// Slerp is used to interpolate between two unit vectors on a sphere, providing smooth rotation. |
| | | 497 | | /// It can be more computationally expensive than linear interpolation (Lerp) but results in smoother, arc-like moti |
| | | 498 | | /// </remarks> |
| | | 499 | | public static Vector3d Slerp(Vector3d start, Vector3d end, Fixed64 percent) |
| | | 500 | | { |
| | | 501 | | // Dot product - the cosine of the angle between 2 vectors. |
| | 6 | 502 | | Fixed64 dot = Dot(start, end); |
| | | 503 | | // Clamp it to be in the range of Acos() |
| | | 504 | | // This may be unnecessary, but floating point |
| | | 505 | | // precision can be a fickle mistress. |
| | 6 | 506 | | dot = FixedMath.Clamp(dot, -Fixed64.One, Fixed64.One); |
| | | 507 | | // Acos(dot) returns the angle between start and end, |
| | | 508 | | // And multiplying that by percent returns the angle between |
| | | 509 | | // start and the final result. |
| | 6 | 510 | | Fixed64 theta = FixedMath.Acos(dot) * percent; |
| | 6 | 511 | | Vector3d RelativeVec = end - start * dot; |
| | 6 | 512 | | RelativeVec.NormalizeInPlace(); |
| | | 513 | | // Orthonormal basis |
| | | 514 | | // The final result. |
| | 6 | 515 | | return (start * FixedMath.Cos(theta)) + (RelativeVec * FixedMath.Sin(theta)); |
| | | 516 | | } |
| | | 517 | | |
| | | 518 | | /// <summary> |
| | | 519 | | /// Calculates a position between four points using Catmull-Rom interpolation. |
| | | 520 | | /// </summary> |
| | | 521 | | /// <param name="value1">The first point.</param> |
| | | 522 | | /// <param name="value2">The second point.</param> |
| | | 523 | | /// <param name="value3">The third point.</param> |
| | | 524 | | /// <param name="value4">The fourth point.</param> |
| | | 525 | | /// <param name="amount">The interpolation factor.</param> |
| | | 526 | | /// <returns>The interpolated position.</returns> |
| | | 527 | | public static Vector3d CatmullRom( |
| | | 528 | | Vector3d value1, |
| | | 529 | | Vector3d value2, |
| | | 530 | | Vector3d value3, |
| | | 531 | | Vector3d value4, |
| | | 532 | | Fixed64 amount) |
| | | 533 | | { |
| | 1 | 534 | | return new Vector3d( |
| | 1 | 535 | | FixedMath.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), |
| | 1 | 536 | | FixedMath.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), |
| | 1 | 537 | | FixedMath.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount) |
| | 1 | 538 | | ); |
| | | 539 | | } |
| | | 540 | | |
| | | 541 | | /// <summary> |
| | | 542 | | /// Calculates a position between two points using Hermite spline interpolation, |
| | | 543 | | /// which takes into account the tangents at the endpoints for smoother transitions. |
| | | 544 | | /// </summary> |
| | | 545 | | /// <param name="value1">The first point.</param> |
| | | 546 | | /// <param name="tangent1">The tangent at the first point.</param> |
| | | 547 | | /// <param name="value2">The second point.</param> |
| | | 548 | | /// <param name="tangent2">The tangent at the second point.</param> |
| | | 549 | | /// <param name="amount">The interpolation factor.</param> |
| | | 550 | | /// <returns>The interpolated position.</returns> |
| | | 551 | | public static Vector3d HermiteSpline( |
| | | 552 | | Vector3d value1, |
| | | 553 | | Vector3d tangent1, |
| | | 554 | | Vector3d value2, |
| | | 555 | | Vector3d tangent2, |
| | | 556 | | Fixed64 amount) |
| | | 557 | | { |
| | 1 | 558 | | return new Vector3d( |
| | 1 | 559 | | FixedMath.HermiteSpline(value1.X, tangent1.X, value2.X, tangent2.X, amount), |
| | 1 | 560 | | FixedMath.HermiteSpline(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount), |
| | 1 | 561 | | FixedMath.HermiteSpline(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount)); |
| | | 562 | | } |
| | | 563 | | |
| | | 564 | | /// <summary> |
| | | 565 | | /// Calculates a position between two points using a cubic Hermite interpolation, |
| | | 566 | | /// which is similar to HermiteSpline but assumes zero tangents at the endpoints for a smoother curve. |
| | | 567 | | /// </summary> |
| | | 568 | | /// <param name="value1">The first point.</param> |
| | | 569 | | /// <param name="value2">The second point.</param> |
| | | 570 | | /// <param name="amount">The interpolation factor.</param> |
| | | 571 | | /// <returns>The interpolated position.</returns> |
| | | 572 | | public static Vector3d SmoothStep(Vector3d value1, Vector3d value2, Fixed64 amount) |
| | | 573 | | { |
| | 1 | 574 | | return new Vector3d( |
| | 1 | 575 | | FixedMath.SmoothStep(value1.X, value2.X, amount), |
| | 1 | 576 | | FixedMath.SmoothStep(value1.Y, value2.Y, amount), |
| | 1 | 577 | | FixedMath.SmoothStep(value1.Z, value2.Z, amount) |
| | 1 | 578 | | ); |
| | | 579 | | } |
| | | 580 | | |
| | | 581 | | /// <summary> |
| | | 582 | | /// Normalizes the given vector, returning a unit vector with the same direction. |
| | | 583 | | /// </summary> |
| | | 584 | | /// <param name="value">The vector to normalize.</param> |
| | | 585 | | /// <returns>A normalized (unit) vector with the same direction.</returns> |
| | | 586 | | public static Vector3d GetNormalized(Vector3d value) |
| | | 587 | | { |
| | 246 | 588 | | bool magnitudeIsRepresentable = TryGetMagnitude( |
| | 246 | 589 | | value, |
| | 246 | 590 | | out Fixed64 mag, |
| | 246 | 591 | | out bool isNormalized); |
| | | 592 | | |
| | | 593 | | // If magnitude is zero, return a zero vector to avoid divide-by-zero errors |
| | 246 | 594 | | if (mag == Fixed64.Zero) |
| | 1 | 595 | | return new Vector3d(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero); |
| | | 596 | | |
| | 245 | 597 | | if (isNormalized) |
| | 123 | 598 | | return value; |
| | | 599 | | |
| | 122 | 600 | | if (!magnitudeIsRepresentable || mag == Fixed64.One) |
| | 6 | 601 | | return WideNormalization.GetNormalized(value); |
| | | 602 | | |
| | 116 | 603 | | if (mag <= FixedMath.ScaleSafeMagnitudeThreshold) |
| | 9 | 604 | | return GetScaleNormalized(value); |
| | | 605 | | |
| | 107 | 606 | | var normalized = new Vector3d( |
| | 107 | 607 | | FixedMath.FastDiv(value.X, mag), |
| | 107 | 608 | | FixedMath.FastDiv(value.Y, mag), |
| | 107 | 609 | | FixedMath.FastDiv(value.Z, mag)); |
| | 107 | 610 | | return normalized.IsNormalized() |
| | 107 | 611 | | ? normalized |
| | 107 | 612 | | : WideNormalization.GetNormalized(value); |
| | | 613 | | } |
| | | 614 | | |
| | | 615 | | internal static Vector3d GetScaleNormalized(Vector3d value) |
| | | 616 | | { |
| | 8442 | 617 | | Fixed64 scale = FixedMath.Max(value.X.Abs(), FixedMath.Max(value.Y.Abs(), value.Z.Abs())); |
| | 8442 | 618 | | Vector3d scaled = value / scale; |
| | 8442 | 619 | | Fixed64 scaledMagnitude = FixedMath.GetScaledMagnitude( |
| | 8442 | 620 | | scaled.X, |
| | 8442 | 621 | | scaled.Y, |
| | 8442 | 622 | | scaled.Z, |
| | 8442 | 623 | | Fixed64.Zero); |
| | 8442 | 624 | | return scaled / scaledMagnitude; |
| | | 625 | | } |
| | | 626 | | |
| | | 627 | | /// <summary> |
| | | 628 | | /// Attempts to compose two component-scaled offsets in a shared frame and |
| | | 629 | | /// one rotated inner-frame displacement with one final round-half-to-even |
| | | 630 | | /// conversion per component. |
| | | 631 | | /// </summary> |
| | | 632 | | /// <remarks> |
| | | 633 | | /// Computes |
| | | 634 | | /// <c>outerScale * outerLocalPoint |
| | | 635 | | /// + innerFrameScale * innerFrameOffset |
| | | 636 | | /// + Rotate(innerLocalDisplacement)</c> |
| | | 637 | | /// without narrowing either scaled offset or the rotated displacement |
| | | 638 | | /// independently. |
| | | 639 | | /// </remarks> |
| | | 640 | | public static bool TryComposeScaledLocalPoints( |
| | | 641 | | Vector3d outerLocalPoint, |
| | | 642 | | Vector3d outerScale, |
| | | 643 | | Vector3d innerFrameOffset, |
| | | 644 | | Vector3d innerFrameScale, |
| | | 645 | | Vector3d innerLocalDisplacement, |
| | | 646 | | FixedQuaternion innerRotation, |
| | | 647 | | out Vector3d result) => |
| | 67 | 648 | | WideVector3dTransform.TryComposeScaledLocalPoints( |
| | 67 | 649 | | outerLocalPoint, |
| | 67 | 650 | | outerScale, |
| | 67 | 651 | | innerFrameOffset, |
| | 67 | 652 | | innerFrameScale, |
| | 67 | 653 | | innerLocalDisplacement, |
| | 67 | 654 | | innerRotation, |
| | 67 | 655 | | out result); |
| | | 656 | | |
| | | 657 | | /// <summary> |
| | | 658 | | /// Returns the normalized direction from <paramref name="start"/> toward |
| | | 659 | | /// <paramref name="end"/> across the complete coordinate domain. |
| | | 660 | | /// </summary> |
| | | 661 | | /// <remarks> |
| | | 662 | | /// Equal endpoints return <see cref="Zero"/>. Endpoint differences are |
| | | 663 | | /// evaluated exactly even when a component cannot be represented by |
| | | 664 | | /// <see cref="Fixed64"/>. |
| | | 665 | | /// </remarks> |
| | | 666 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 667 | | public static Vector3d GetDirection(Vector3d start, Vector3d end) => |
| | 4 | 668 | | WideNormalization.GetDirection(start, end); |
| | | 669 | | |
| | | 670 | | /// <summary> |
| | | 671 | | /// Returns the magnitude (length) of this vector. |
| | | 672 | | /// </summary> |
| | | 673 | | /// <param name="vector">The vector whose magnitude is being calculated.</param> |
| | | 674 | | /// <returns>The magnitude of the vector.</returns> |
| | | 675 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 676 | | public static Fixed64 GetMagnitude(Vector3d vector) |
| | | 677 | | { |
| | 14180 | 678 | | _ = TryGetMagnitude(vector, out Fixed64 magnitude); |
| | 14180 | 679 | | return magnitude; |
| | | 680 | | } |
| | | 681 | | |
| | | 682 | | /// <summary> |
| | | 683 | | /// Attempts to return the magnitude of the given vector without saturating the result. |
| | | 684 | | /// </summary> |
| | | 685 | | /// <param name="vector">The vector to measure.</param> |
| | | 686 | | /// <param name="magnitude">The magnitude, or <see cref="Fixed64.MaxValue"/> when it is not representable.</param> |
| | | 687 | | /// <returns><see langword="true"/> when the magnitude fits in <see cref="Fixed64"/>; otherwise, <see langword="fals |
| | | 688 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 689 | | public static bool TryGetMagnitude(Vector3d vector, out Fixed64 magnitude) => |
| | 14558 | 690 | | TryGetMagnitude(vector, out magnitude, out _); |
| | | 691 | | |
| | | 692 | | private static bool TryGetMagnitude( |
| | | 693 | | Vector3d vector, |
| | | 694 | | out Fixed64 magnitude, |
| | | 695 | | out bool isNormalized) |
| | | 696 | | { |
| | 14819 | 697 | | Fixed64 mag = (vector.X * vector.X) + (vector.Y * vector.Y) + (vector.Z * vector.Z); |
| | 14819 | 698 | | isNormalized = mag != Fixed64.Zero |
| | 14819 | 699 | | && FixedMath.Abs(mag - Fixed64.One) <= Fixed64.Epsilon; |
| | | 700 | | |
| | 14819 | 701 | | if (mag == Fixed64.MaxValue) |
| | 45 | 702 | | return FixedMath.TryGetScaledMagnitude( |
| | 45 | 703 | | vector.X, |
| | 45 | 704 | | vector.Y, |
| | 45 | 705 | | vector.Z, |
| | 45 | 706 | | Fixed64.Zero, |
| | 45 | 707 | | out magnitude); |
| | | 708 | | |
| | 14774 | 709 | | if (mag <= FixedMath.ScaleSafeMagnitudeSquaredThreshold) |
| | | 710 | | { |
| | 73 | 711 | | magnitude = FixedMath.GetScaledMagnitude( |
| | 73 | 712 | | vector.X, |
| | 73 | 713 | | vector.Y, |
| | 73 | 714 | | vector.Z, |
| | 73 | 715 | | Fixed64.Zero); |
| | 73 | 716 | | return true; |
| | | 717 | | } |
| | | 718 | | |
| | 14701 | 719 | | if (isNormalized) |
| | | 720 | | { |
| | 14125 | 721 | | magnitude = Fixed64.One; |
| | 14125 | 722 | | return true; |
| | | 723 | | } |
| | | 724 | | |
| | 576 | 725 | | magnitude = FixedMath.Sqrt(mag); |
| | 576 | 726 | | return true; |
| | | 727 | | } |
| | | 728 | | |
| | | 729 | | /// <summary> |
| | | 730 | | /// Attempts to return the distance between two endpoints without saturating |
| | | 731 | | /// any component difference. |
| | | 732 | | /// </summary> |
| | | 733 | | /// <param name="start">The first endpoint.</param> |
| | | 734 | | /// <param name="end">The second endpoint.</param> |
| | | 735 | | /// <param name="distance">The rounded distance, or <see cref="Fixed64.MaxValue"/> when it is not representable.</pa |
| | | 736 | | /// <returns><see langword="true"/> when the distance fits in <see cref="Fixed64"/>; otherwise, <see langword="false |
| | | 737 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 738 | | public static bool TryGetDistance(Vector3d start, Vector3d end, out Fixed64 distance) => |
| | 24 | 739 | | WideGeometry.TryGetDistance(start, end, out distance); |
| | | 740 | | |
| | | 741 | | /// <summary> |
| | | 742 | | /// Compares the exact squared magnitudes of two vectors without fixed-point saturation. |
| | | 743 | | /// </summary> |
| | | 744 | | /// <returns>A negative value when <paramref name="left"/> is shorter, zero when the |
| | | 745 | | /// magnitudes are equal, or a positive value when <paramref name="left"/> is longer.</returns> |
| | | 746 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 747 | | public static int CompareMagnitudeSquared(Vector3d left, Vector3d right) => |
| | 3 | 748 | | Fixed64.CompareMagnitudeSquared( |
| | 3 | 749 | | left.X, |
| | 3 | 750 | | left.Y, |
| | 3 | 751 | | left.Z, |
| | 3 | 752 | | Fixed64.Zero, |
| | 3 | 753 | | right.X, |
| | 3 | 754 | | right.Y, |
| | 3 | 755 | | right.Z, |
| | 3 | 756 | | Fixed64.Zero); |
| | | 757 | | |
| | | 758 | | /// <summary> |
| | | 759 | | /// Compares the exact squared distances between two pairs of points without fixed-point saturation. |
| | | 760 | | /// </summary> |
| | | 761 | | /// <returns>A negative value when the left distance is shorter, zero when the |
| | | 762 | | /// distances are equal, or a positive value when the left distance is longer.</returns> |
| | | 763 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 764 | | public static int CompareDistanceSquared( |
| | | 765 | | Vector3d leftStart, |
| | | 766 | | Vector3d leftEnd, |
| | | 767 | | Vector3d rightStart, |
| | | 768 | | Vector3d rightEnd) => |
| | 72 | 769 | | WideGeometry.CompareSquaredDistance3D( |
| | 72 | 770 | | leftStart.X, leftEnd.X, |
| | 72 | 771 | | leftStart.Y, leftEnd.Y, |
| | 72 | 772 | | leftStart.Z, leftEnd.Z, |
| | 72 | 773 | | rightStart.X, rightEnd.X, |
| | 72 | 774 | | rightStart.Y, rightEnd.Y, |
| | 72 | 775 | | rightStart.Z, rightEnd.Z); |
| | | 776 | | |
| | | 777 | | /// <summary> |
| | | 778 | | /// Returns the exact sign of the scalar triple product without fixed-point saturation. |
| | | 779 | | /// </summary> |
| | | 780 | | /// <returns><c>1</c> for a positive product, <c>-1</c> for a negative product, |
| | | 781 | | /// or <c>0</c> when the vectors are exactly coplanar.</returns> |
| | | 782 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 783 | | public static int ScalarTripleProductSign(Vector3d first, Vector3d second, Vector3d third) => |
| | 3 | 784 | | WideGeometry.GetTripleProductSign( |
| | 3 | 785 | | first.X, first.Y, first.Z, |
| | 3 | 786 | | second.X, second.Y, second.Z, |
| | 3 | 787 | | third.X, third.Y, third.Z); |
| | | 788 | | |
| | | 789 | | /// <summary> |
| | | 790 | | /// Returns a new <see cref="Vector3d"/> where each component is the absolute value of the corresponding input compo |
| | | 791 | | /// </summary> |
| | | 792 | | /// <param name="value">The input vector.</param> |
| | | 793 | | /// <returns>A vector with absolute values for each component.</returns> |
| | | 794 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 795 | | public static Vector3d Abs(Vector3d value) => new(value.X.Abs(), value.Y.Abs(), value.Z.Abs()); |
| | | 796 | | |
| | | 797 | | /// <summary> |
| | | 798 | | /// Returns a new <see cref="Vector3d"/> where each component is the sign of the corresponding input component. |
| | | 799 | | /// </summary> |
| | | 800 | | /// <param name="value">The input vector.</param> |
| | | 801 | | /// <returns>A vector where each component is -1, 0, or 1 based on the sign of the input.</returns> |
| | | 802 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 803 | | public static Vector3d Sign(Vector3d value) => new(value.X.Sign(), value.Y.Sign(), value.Z.Sign()); |
| | | 804 | | |
| | | 805 | | /// <summary> |
| | | 806 | | /// Attempts to calculate the exact non-negative weighted average with one |
| | | 807 | | /// final round-half-to-even conversion per component. |
| | | 808 | | /// </summary> |
| | | 809 | | /// <remarks> |
| | | 810 | | /// Zero-weight values are ignored. The operation returns |
| | | 811 | | /// <see langword="false"/> only when the total weight is zero. |
| | | 812 | | /// </remarks> |
| | | 813 | | public static bool TryGetWeightedAverage( |
| | | 814 | | ReadOnlySpan<Vector3d> values, |
| | | 815 | | ReadOnlySpan<Fixed64> weights, |
| | | 816 | | out Vector3d average) |
| | | 817 | | { |
| | 179 | 818 | | WideWeightedAverage.ValidateInputs(values.Length, weights); |
| | 177 | 819 | | return WideWeightedAverage.TryGet(values, weights, out average); |
| | | 820 | | } |
| | | 821 | | |
| | | 822 | | /// <summary> |
| | | 823 | | /// Clamps each component of the given <see cref="Vector3d"/> within the specified min and max bounds. |
| | | 824 | | /// </summary> |
| | | 825 | | /// <param name="value">The vector to clamp.</param> |
| | | 826 | | /// <param name="min">The minimum bounds.</param> |
| | | 827 | | /// <param name="max">The maximum bounds.</param> |
| | | 828 | | /// <returns>A vector with each component clamped between min and max.</returns> |
| | | 829 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 830 | | public static Vector3d Clamp(Vector3d value, Vector3d min, Vector3d max) => |
| | 5 | 831 | | new(FixedMath.Clamp(value.X, min.X, max.X), |
| | 5 | 832 | | FixedMath.Clamp(value.Y, min.Y, max.Y), |
| | 5 | 833 | | FixedMath.Clamp(value.Z, min.Z, max.Z)); |
| | | 834 | | |
| | | 835 | | /// <summary> |
| | | 836 | | /// Clamps the given Vector3d within the specified magnitude. |
| | | 837 | | /// </summary> |
| | | 838 | | /// <param name="value"></param> |
| | | 839 | | /// <param name="maxMagnitude"></param> |
| | | 840 | | /// <returns></returns> |
| | | 841 | | public static Vector3d ClampMagnitude(Vector3d value, Fixed64 maxMagnitude) |
| | | 842 | | { |
| | 2 | 843 | | if (value.MagnitudeSquared > maxMagnitude * maxMagnitude) |
| | 1 | 844 | | return value.Normalized * maxMagnitude; // Clamp magnitude without changing direction |
| | | 845 | | |
| | 1 | 846 | | return value; |
| | | 847 | | } |
| | | 848 | | |
| | | 849 | | /// <summary> |
| | | 850 | | /// Determines if two vectors are exactly parallel by checking if their cross product is zero. |
| | | 851 | | /// </summary> |
| | | 852 | | /// <param name="v1">The first vector.</param> |
| | | 853 | | /// <param name="v2">The second vector.</param> |
| | | 854 | | /// <returns>True if the vectors are exactly parallel, false otherwise.</returns> |
| | 2 | 855 | | public static bool AreParallel(Vector3d v1, Vector3d v2) => Cross(v1, v2).MagnitudeSquared == Fixed64.Zero; |
| | | 856 | | |
| | | 857 | | /// <summary> |
| | | 858 | | /// Determines if two vectors are approximately parallel based on a cosine similarity threshold. |
| | | 859 | | /// </summary> |
| | | 860 | | /// <param name="v1">The first normalized vector.</param> |
| | | 861 | | /// <param name="v2">The second normalized vector.</param> |
| | | 862 | | /// <param name="cosThreshold">The cosine similarity threshold for near-parallel vectors.</param> |
| | | 863 | | /// <returns>True if the vectors are nearly parallel, false otherwise.</returns> |
| | | 864 | | public static bool AreAlmostParallel(Vector3d v1, Vector3d v2, Fixed64 cosThreshold) |
| | | 865 | | { |
| | | 866 | | // Assuming v1 and v2 are already normalized |
| | 2 | 867 | | Fixed64 dot = Dot(v1, v2); |
| | | 868 | | |
| | | 869 | | // Compare dot product directly to the cosine threshold |
| | 2 | 870 | | return dot >= cosThreshold; |
| | | 871 | | } |
| | | 872 | | |
| | | 873 | | /// <summary> |
| | | 874 | | /// Computes the midpoint between two vectors. |
| | | 875 | | /// </summary> |
| | | 876 | | /// <param name="v1">The first vector.</param> |
| | | 877 | | /// <param name="v2">The second vector.</param> |
| | | 878 | | /// <returns>The midpoint vector.</returns> |
| | | 879 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 880 | | public static Vector3d Midpoint(Vector3d v1, Vector3d v2) => |
| | 57 | 881 | | new(FixedMath.Midpoint(v1.X, v2.X), FixedMath.Midpoint(v1.Y, v2.Y), FixedMath.Midpoint(v1.Z, v2.Z)); |
| | | 882 | | |
| | | 883 | | /// <inheritdoc cref="Distance(Fixed64, Fixed64, Fixed64)" /> |
| | | 884 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 36 | 885 | | public static Fixed64 Distance(Vector3d start, Vector3d end) => start.Distance(end.X, end.Y, end.Z); |
| | | 886 | | |
| | | 887 | | /// <inheritdoc cref="DistanceSquared(Fixed64, Fixed64, Fixed64)" /> |
| | | 888 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 100 | 889 | | public static Fixed64 DistanceSquared(Vector3d start, Vector3d end) => start.DistanceSquared(end.X, end.Y, end.Z); |
| | | 890 | | |
| | | 891 | | /// <summary> |
| | | 892 | | /// Calculates the closest point on a line segment defined by start and end points to a given point in space. |
| | | 893 | | /// </summary> |
| | | 894 | | /// <param name="point">The point to project onto the segment.</param> |
| | | 895 | | /// <param name="start">The start of the line segment.</param> |
| | | 896 | | /// <param name="end">The end of the line segment.</param> |
| | | 897 | | /// <returns>The closest point on the line segment to the given point.</returns> |
| | | 898 | | /// <remarks> |
| | | 899 | | /// Endpoint differences and projection products are evaluated across the |
| | | 900 | | /// complete raw domain before the parameter is clamped and rounded. |
| | | 901 | | /// A direction whose exact Q64.64 squared-length total is at most 2^31 raw |
| | | 902 | | /// units rounds to zero in Q32.32 and returns <paramref name="start"/>. |
| | | 903 | | /// </remarks> |
| | | 904 | | public static Vector3d ClosestPointOnLineSegment(Vector3d point, Vector3d start, Vector3d end) |
| | | 905 | | { |
| | 143 | 906 | | Signed192 denominator = WideGeometry.GetDifferenceDotProduct3D( |
| | 143 | 907 | | end.X, start.X, end.Y, start.Y, end.Z, start.Z, |
| | 143 | 908 | | end.X, start.X, end.Y, start.Y, end.Z, start.Z); |
| | 143 | 909 | | if (WideGeometry.IsSquaredLengthDegenerate(denominator)) |
| | 37 | 910 | | return start; |
| | | 911 | | |
| | 106 | 912 | | Signed192 numerator = WideGeometry.GetDifferenceDotProduct3D( |
| | 106 | 913 | | point.X, start.X, point.Y, start.Y, point.Z, start.Z, |
| | 106 | 914 | | end.X, start.X, end.Y, start.Y, end.Z, start.Z); |
| | 106 | 915 | | if (numerator.Sign <= 0) |
| | 20 | 916 | | return start; |
| | 86 | 917 | | if (WideArithmetic.CompareMagnitude(numerator, denominator) >= 0) |
| | 29 | 918 | | return end; |
| | | 919 | | |
| | 57 | 920 | | _ = Fixed64.TryGetUnitIntervalRatio(numerator, denominator, out Fixed64 parameter); |
| | 57 | 921 | | return new Vector3d( |
| | 57 | 922 | | FixedMath.Lerp(start.X, end.X, parameter), |
| | 57 | 923 | | FixedMath.Lerp(start.Y, end.Y, parameter), |
| | 57 | 924 | | FixedMath.Lerp(start.Z, end.Z, parameter)); |
| | | 925 | | } |
| | | 926 | | |
| | | 927 | | /// <summary> |
| | | 928 | | /// Dot Product of two vectors. |
| | | 929 | | /// </summary> |
| | | 930 | | /// <param name="lhs"></param> |
| | | 931 | | /// <param name="rhs"></param> |
| | | 932 | | /// <returns></returns> |
| | | 933 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 32658 | 934 | | public static Fixed64 Dot(Vector3d lhs, Vector3d rhs) => lhs.Dot(rhs.X, rhs.Y, rhs.Z); |
| | | 935 | | |
| | | 936 | | /// <summary> |
| | | 937 | | /// Cross Product of two vectors. |
| | | 938 | | /// </summary> |
| | | 939 | | /// <param name="lhs"></param> |
| | | 940 | | /// <param name="rhs"></param> |
| | | 941 | | /// <returns></returns> |
| | | 942 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 6208 | 943 | | public static Vector3d Cross(Vector3d lhs, Vector3d rhs) => lhs.Cross(rhs.X, rhs.Y, rhs.Z); |
| | | 944 | | |
| | | 945 | | /// <inheritdoc cref="CrossProduct(Fixed64, Fixed64, Fixed64)"/> |
| | | 946 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 4 | 947 | | public static Fixed64 CrossProduct(Vector3d lhs, Vector3d rhs) => lhs.CrossProduct(rhs.X, rhs.Y, rhs.Z); |
| | | 948 | | |
| | | 949 | | /// <summary> |
| | | 950 | | /// Projects a vector onto another vector. |
| | | 951 | | /// </summary> |
| | | 952 | | /// <param name="vector"></param> |
| | | 953 | | /// <param name="onNormal"></param> |
| | | 954 | | /// <returns></returns> |
| | | 955 | | public static Vector3d Project(Vector3d vector, Vector3d onNormal) |
| | | 956 | | { |
| | 5 | 957 | | Fixed64 sqrMag = Dot(onNormal, onNormal); |
| | 5 | 958 | | if (sqrMag.Abs() < Fixed64.Epsilon) |
| | 1 | 959 | | return Zero; |
| | | 960 | | else |
| | | 961 | | { |
| | 4 | 962 | | Fixed64 dot = Dot(vector, onNormal); |
| | 4 | 963 | | return new Vector3d(onNormal.X * dot / sqrMag, |
| | 4 | 964 | | onNormal.Y * dot / sqrMag, |
| | 4 | 965 | | onNormal.Z * dot / sqrMag); |
| | | 966 | | } |
| | | 967 | | } |
| | | 968 | | |
| | | 969 | | /// <summary> |
| | | 970 | | /// Projects a vector onto a plane defined by a normal orthogonal to the plane. |
| | | 971 | | /// </summary> |
| | | 972 | | /// <param name="vector"></param> |
| | | 973 | | /// <param name="planeNormal"></param> |
| | | 974 | | /// <returns></returns> |
| | | 975 | | public static Vector3d ProjectOnPlane(Vector3d vector, Vector3d planeNormal) |
| | | 976 | | { |
| | 4 | 977 | | Fixed64 sqrMag = Dot(planeNormal, planeNormal); |
| | 4 | 978 | | if (sqrMag.Abs() < Fixed64.Epsilon) |
| | 1 | 979 | | return vector; |
| | | 980 | | else |
| | | 981 | | { |
| | 3 | 982 | | Fixed64 dot = Dot(vector, planeNormal); |
| | 3 | 983 | | return new Vector3d(vector.X - planeNormal.X * dot / sqrMag, |
| | 3 | 984 | | vector.Y - planeNormal.Y * dot / sqrMag, |
| | 3 | 985 | | vector.Z - planeNormal.Z * dot / sqrMag); |
| | | 986 | | } |
| | | 987 | | } |
| | | 988 | | |
| | | 989 | | /// <summary> |
| | | 990 | | /// Returns the normalized direction of a vector projected onto a plane. |
| | | 991 | | /// </summary> |
| | | 992 | | /// <remarks> |
| | | 993 | | /// The rejection is formed exactly with full-domain intermediates before |
| | | 994 | | /// returning its nearest representable normalized direction. A zero normal |
| | | 995 | | /// returns the normalized input vector; a zero projection returns |
| | | 996 | | /// <see cref="Zero"/>. |
| | | 997 | | /// </remarks> |
| | | 998 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 999 | | public static Vector3d GetNormalizedProjectionOnPlane( |
| | | 1000 | | Vector3d vector, |
| | | 1001 | | Vector3d planeNormal) => |
| | 5 | 1002 | | Geometry.WideGeometry.GetNormalizedProjectionOnPlane(vector, planeNormal); |
| | | 1003 | | |
| | | 1004 | | /// <summary> |
| | | 1005 | | /// Projects a point onto a plane defined by a normal and a distance from the origin. |
| | | 1006 | | /// </summary> |
| | | 1007 | | /// <param name="point">The point to project.</param> |
| | | 1008 | | /// <param name="plane">The plane onto which the point is projected.</param> |
| | | 1009 | | /// <returns>The projected point.</returns> |
| | | 1010 | | public static Vector3d ProjectOnPlane(Vector3d point, FixedPlane plane) |
| | | 1011 | | { |
| | 28 | 1012 | | Fixed64 normalLengthSquared = plane.Normal.MagnitudeSquared; |
| | 28 | 1013 | | if (normalLengthSquared == Fixed64.Zero) |
| | 1 | 1014 | | return point; |
| | | 1015 | | |
| | 27 | 1016 | | Fixed64 distance = plane.DotCoordinate(point); |
| | 27 | 1017 | | return point - plane.Normal * FixedMath.FastDiv(distance, normalLengthSquared); |
| | | 1018 | | } |
| | | 1019 | | |
| | | 1020 | | /// <summary> |
| | | 1021 | | /// Computes the angle in degrees between two vectors. |
| | | 1022 | | /// </summary> |
| | | 1023 | | /// <param name="from">The starting vector.</param> |
| | | 1024 | | /// <param name="to">The target vector.</param> |
| | | 1025 | | /// <returns>The angle in degrees between the two vectors.</returns> |
| | | 1026 | | /// <remarks> |
| | | 1027 | | /// This method calculates the angle by using the dot product between the vectors and normalizing the result. |
| | | 1028 | | /// The angle is always the smaller angle between the two vectors on a plane. |
| | | 1029 | | /// </remarks> |
| | | 1030 | | public static Fixed64 Angle(Vector3d from, Vector3d to) |
| | | 1031 | | { |
| | 4 | 1032 | | Fixed64 denominator = FixedMath.Sqrt(from.MagnitudeSquared * to.MagnitudeSquared); |
| | | 1033 | | |
| | 4 | 1034 | | if (denominator.Abs() < Fixed64.Epsilon) |
| | 1 | 1035 | | return Fixed64.Zero; |
| | | 1036 | | |
| | 3 | 1037 | | Fixed64 dot = FixedMath.Clamp(Dot(from, to) / denominator, -Fixed64.One, Fixed64.One); |
| | | 1038 | | |
| | 3 | 1039 | | return FixedMath.RadToDeg(FixedMath.Acos(dot)); |
| | | 1040 | | } |
| | | 1041 | | |
| | | 1042 | | /// <summary> |
| | | 1043 | | /// Calculates the barycentric coordinates of a point with respect to a triangle defined by three vertices. |
| | | 1044 | | /// </summary> |
| | | 1045 | | /// <param name="coordA">The first vertex of the triangle.</param> |
| | | 1046 | | /// <param name="coordB">The second vertex of the triangle.</param> |
| | | 1047 | | /// <param name="coordC">The third vertex of the triangle.</param> |
| | | 1048 | | /// <param name="weightB">The barycentric weight for the second vertex.</param> |
| | | 1049 | | /// <param name="weightC">The barycentric weight for the third vertex.</param> |
| | | 1050 | | /// <returns>The cartesian translation represented by the barycentric coordinates within the triangle.</returns> |
| | | 1051 | | public static Vector3d BarycentricCoordinates( |
| | | 1052 | | Vector3d coordA, |
| | | 1053 | | Vector3d coordB, |
| | | 1054 | | Vector3d coordC, |
| | | 1055 | | Fixed64 weightB, |
| | | 1056 | | Fixed64 weightC) |
| | | 1057 | | { |
| | 115 | 1058 | | return new( |
| | 115 | 1059 | | FixedMath.BarycentricCoordinate(coordA.X, coordB.X, coordC.X, weightB, weightC), |
| | 115 | 1060 | | FixedMath.BarycentricCoordinate(coordA.Y, coordB.Y, coordC.Y, weightB, weightC), |
| | 115 | 1061 | | FixedMath.BarycentricCoordinate(coordA.Z, coordB.Z, coordC.Z, weightB, weightC)); |
| | | 1062 | | } |
| | | 1063 | | |
| | | 1064 | | /// <summary> |
| | | 1065 | | /// Returns a vector whose elements are the maximum of each of the pairs of elements in two specified vectors. |
| | | 1066 | | /// </summary> |
| | | 1067 | | /// <param name="value1">The first vector.</param> |
| | | 1068 | | /// <param name="value2">The second vector.</param> |
| | | 1069 | | /// <returns>The maximized vector.</returns> |
| | | 1070 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1071 | | public static Vector3d Max(Vector3d value1, Vector3d value2) => |
| | 1461 | 1072 | | new(FixedMath.Max(value1.X, value2.X), |
| | 1461 | 1073 | | FixedMath.Max(value1.Y, value2.Y), |
| | 1461 | 1074 | | FixedMath.Max(value1.Z, value2.Z)); |
| | | 1075 | | |
| | | 1076 | | /// <summary> |
| | | 1077 | | /// Returns a vector whose elements are the minimum of each of the pairs of elements in two specified vectors. |
| | | 1078 | | /// </summary> |
| | | 1079 | | /// <param name="value1">The first vector.</param> |
| | | 1080 | | /// <param name="value2">The second vector.</param> |
| | | 1081 | | /// <returns>The minimized vector.</returns> |
| | | 1082 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1083 | | public static Vector3d Min(Vector3d value1, Vector3d value2) => |
| | 1461 | 1084 | | new(FixedMath.Min(value1.X, value2.X), |
| | 1461 | 1085 | | FixedMath.Min(value1.Y, value2.Y), |
| | 1461 | 1086 | | FixedMath.Min(value1.Z, value2.Z)); |
| | | 1087 | | |
| | | 1088 | | /// <summary> |
| | | 1089 | | /// Returns a vector that is the negation of the specified vector, effectively reversing its direction. |
| | | 1090 | | /// </summary> |
| | | 1091 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 1092 | | public static Vector3d Negate(Vector3d value) => -value; |
| | | 1093 | | |
| | | 1094 | | /// <summary> |
| | | 1095 | | /// Rotates the vector around a given position using a specified quaternion rotation. |
| | | 1096 | | /// </summary> |
| | | 1097 | | /// <param name="source">The vector to rotate.</param> |
| | | 1098 | | /// <param name="position">The position around which the vector is rotated.</param> |
| | | 1099 | | /// <param name="rotation">The quaternion representing the rotation.</param> |
| | | 1100 | | /// <returns>The rotated vector.</returns> |
| | | 1101 | | public static Vector3d Rotate(Vector3d source, Vector3d position, FixedQuaternion rotation) |
| | | 1102 | | { |
| | 1 | 1103 | | source -= position; // Translate the vector by the position |
| | 1 | 1104 | | var normalizedRotation = rotation.Normalized; |
| | 1 | 1105 | | return (normalizedRotation * source) + position; |
| | | 1106 | | } |
| | | 1107 | | |
| | | 1108 | | /// <summary> |
| | | 1109 | | /// Applies the inverse of a specified quaternion rotation to the vector around a given position. |
| | | 1110 | | /// </summary> |
| | | 1111 | | /// <param name="source">The vector to rotate.</param> |
| | | 1112 | | /// <param name="position">The position around which the vector is rotated.</param> |
| | | 1113 | | /// <param name="rotation">The quaternion representing the inverse rotation.</param> |
| | | 1114 | | /// <returns>The rotated vector.</returns> |
| | | 1115 | | public static Vector3d InverseRotate(Vector3d source, Vector3d position, FixedQuaternion rotation) |
| | | 1116 | | { |
| | 1 | 1117 | | source -= position; // Translate the vector by the position |
| | 1 | 1118 | | var normalizedRotation = rotation.Normalized; |
| | | 1119 | | // Undo the rotation |
| | 1 | 1120 | | source = normalizedRotation.Inverse() * source; |
| | | 1121 | | // Add the original position back |
| | 1 | 1122 | | return source + position; |
| | | 1123 | | } |
| | | 1124 | | |
| | | 1125 | | /// <summary> |
| | | 1126 | | /// Reflects a vector off the plane defined by a normal. |
| | | 1127 | | /// The result is a vector that points in the direction a perfectly reflected ray would go, |
| | | 1128 | | /// based on the incoming vector and the normal of the plane it reflects off. |
| | | 1129 | | /// </summary> |
| | | 1130 | | /// <param name="vector">The vector to reflect.</param> |
| | | 1131 | | /// <param name="normal">The normal of the plane to reflect off.</param> |
| | | 1132 | | /// <returns>The reflected vector.</returns> |
| | | 1133 | | public static Vector3d Reflect(Vector3d vector, Vector3d normal) |
| | | 1134 | | { |
| | 2 | 1135 | | Fixed64 dot = Dot(vector, normal); |
| | 2 | 1136 | | return vector - 2 * dot * normal; |
| | | 1137 | | } |
| | | 1138 | | |
| | | 1139 | | /// <summary> |
| | | 1140 | | /// Transforms a vector by the given 4x4 matrix, applying rotation, scaling, and translation as defined by the matri |
| | | 1141 | | /// </summary> |
| | | 1142 | | /// <param name="vector">The vector to transform.</param> |
| | | 1143 | | /// <param name="matrix">The transformation matrix.</param> |
| | | 1144 | | /// <returns>The transformed vector.</returns> |
| | | 1145 | | /// <remarks> |
| | | 1146 | | /// Same as <see cref="operator *(Vector3d, Fixed4x4)"/>. |
| | | 1147 | | /// </remarks> |
| | 2 | 1148 | | public static Vector3d Transform(Vector3d vector, Fixed4x4 matrix) => matrix * vector; |
| | | 1149 | | |
| | | 1150 | | #endregion |
| | | 1151 | | } |