| | | 1 | | using MemoryPack; |
| | | 2 | | using System; |
| | | 3 | | using System.Collections.Generic; |
| | | 4 | | using System.Runtime.CompilerServices; |
| | | 5 | | using System.Text.Json.Serialization; |
| | | 6 | | |
| | | 7 | | namespace FixedMathSharp; |
| | | 8 | | |
| | | 9 | | /// <summary> |
| | | 10 | | /// Represents a 3D vector with fixed-point precision, supporting a wide range of vector operations such as rotation, sc |
| | | 11 | | /// </summary> |
| | | 12 | | /// <remarks> |
| | | 13 | | /// The Vector3d struct is designed for high-precision applications in 3D space, including games, simulations, and physi |
| | | 14 | | /// It offers essential operations like addition, subtraction, dot product, cross product, distance calculation, and nor |
| | | 15 | | /// |
| | | 16 | | /// Use Cases: |
| | | 17 | | /// - Modeling 3D positions, directions, and velocities with fixed-point precision. |
| | | 18 | | /// - Performing vector transformations, including rotations using quaternions. |
| | | 19 | | /// - Calculating distances, angles, projections, and interpolation between vectors. |
| | | 20 | | /// - Essential for fixed-point math scenarios where floating-point precision isn't suitable. |
| | | 21 | | /// </remarks> |
| | | 22 | | [Serializable] |
| | | 23 | | [MemoryPackable] |
| | | 24 | | public partial struct Vector3d : IEquatable<Vector3d>, IComparable<Vector3d>, IEqualityComparer<Vector3d> |
| | | 25 | | { |
| | | 26 | | #region Static Readonly Fields |
| | | 27 | | |
| | | 28 | | |
| | | 29 | | /// <summary> |
| | | 30 | | /// The upward direction vector (0, 1, 0). |
| | | 31 | | /// </summary> |
| | | 32 | | public static readonly Vector3d Up = new Vector3d(0, 1, 0); |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// (1, 0, 0) |
| | | 36 | | /// </summary> |
| | | 37 | | public static readonly Vector3d Right = new Vector3d(1, 0, 0); |
| | | 38 | | |
| | | 39 | | /// <summary> |
| | | 40 | | /// (0, -1, 0) |
| | | 41 | | /// </summary> |
| | | 42 | | public static readonly Vector3d Down = new Vector3d(0, -1, 0); |
| | | 43 | | |
| | | 44 | | /// <summary> |
| | | 45 | | /// (-1, 0, 0) |
| | | 46 | | /// </summary> |
| | | 47 | | public static readonly Vector3d Left = new Vector3d(-1, 0, 0); |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// The forward direction vector (0, 0, 1). |
| | | 51 | | /// </summary> |
| | | 52 | | public static readonly Vector3d Forward = new Vector3d(0, 0, 1); |
| | | 53 | | |
| | | 54 | | /// <summary> |
| | | 55 | | /// (0, 0, -1) |
| | | 56 | | /// </summary> |
| | | 57 | | public static readonly Vector3d Backward = new Vector3d(0, 0, -1); |
| | | 58 | | |
| | | 59 | | /// <summary> |
| | | 60 | | /// (1, 1, 1) |
| | | 61 | | /// </summary> |
| | | 62 | | public static readonly Vector3d One = new Vector3d(1, 1, 1); |
| | | 63 | | |
| | | 64 | | /// <summary> |
| | | 65 | | /// (-1, -1, -1) |
| | | 66 | | /// </summary> |
| | | 67 | | public static readonly Vector3d Negative = new Vector3d(-1, -1, -1); |
| | | 68 | | |
| | | 69 | | /// <summary> |
| | | 70 | | /// (0, 0, 0) |
| | | 71 | | /// </summary> |
| | | 72 | | public static readonly Vector3d Zero = new Vector3d(0, 0, 0); |
| | | 73 | | |
| | | 74 | | |
| | | 75 | | #endregion |
| | | 76 | | |
| | | 77 | | #region Fields |
| | | 78 | | |
| | | 79 | | /// <summary> |
| | | 80 | | /// The X component of the vector. |
| | | 81 | | /// </summary> |
| | | 82 | | [JsonInclude] |
| | | 83 | | [MemoryPackOrder(0)] |
| | | 84 | | public Fixed64 x; |
| | | 85 | | |
| | | 86 | | /// <summary> |
| | | 87 | | /// The Y component of the vector. |
| | | 88 | | /// </summary> |
| | | 89 | | [JsonInclude] |
| | | 90 | | [MemoryPackOrder(1)] |
| | | 91 | | public Fixed64 y; |
| | | 92 | | |
| | | 93 | | /// <summary> |
| | | 94 | | /// The Z component of the vector. |
| | | 95 | | /// </summary> |
| | | 96 | | [JsonInclude] |
| | | 97 | | [MemoryPackOrder(2)] |
| | | 98 | | public Fixed64 z; |
| | | 99 | | |
| | | 100 | | #endregion |
| | | 101 | | |
| | | 102 | | #region Constructors |
| | | 103 | | |
| | 1800 | 104 | | public Vector3d(int xInt, int yInt, int zInt) : this((Fixed64)xInt, (Fixed64)yInt, (Fixed64)zInt) { } |
| | | 105 | | |
| | 69 | 106 | | public Vector3d(double xDoub, double yDoub, double zDoub) : this((Fixed64)xDoub, (Fixed64)yDoub, (Fixed64)zDoub) { } |
| | | 107 | | |
| | | 108 | | [JsonConstructor] |
| | | 109 | | public Vector3d(Fixed64 x, Fixed64 y, Fixed64 z) |
| | 1643 | 110 | | { |
| | 1643 | 111 | | this.x = x; |
| | 1643 | 112 | | this.y = y; |
| | 1643 | 113 | | this.z = z; |
| | 1643 | 114 | | } |
| | | 115 | | |
| | | 116 | | #endregion |
| | | 117 | | |
| | | 118 | | #region Properties |
| | | 119 | | |
| | | 120 | | /// <summary> |
| | | 121 | | /// Provides a rotated version of the current vector, where rotation is a 90 degrees rotation around the Y axis in |
| | | 122 | | /// </summary> |
| | | 123 | | /// <remarks> |
| | | 124 | | /// These operations rotate the vector 90 degrees around the Y-axis. |
| | | 125 | | /// Note that the positive direction of rotation is defined by the right-hand rule: |
| | | 126 | | /// If your right hand's thumb points in the positive Y direction, then your fingers curl in the positive direction |
| | | 127 | | /// </remarks> |
| | | 128 | | [JsonIgnore] |
| | | 129 | | [MemoryPackIgnore] |
| | | 130 | | public Vector3d RightHandNormal |
| | | 131 | | { |
| | | 132 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 133 | | get => new Vector3d(z, y, -x); |
| | | 134 | | } |
| | | 135 | | |
| | | 136 | | /// <summary> |
| | | 137 | | /// Provides a rotated version of the current vector, where rotation is a 90 degrees rotation around the Y axis in t |
| | | 138 | | /// </summary> |
| | | 139 | | [JsonIgnore] |
| | | 140 | | [MemoryPackIgnore] |
| | | 141 | | public Vector3d LeftHandNormal |
| | | 142 | | { |
| | | 143 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 144 | | get => new Vector3d(-z, y, x); |
| | | 145 | | } |
| | | 146 | | |
| | | 147 | | /// <inheritdoc cref="GetNormalized(Vector3d)"/> |
| | | 148 | | [JsonIgnore] |
| | | 149 | | [MemoryPackIgnore] |
| | | 150 | | public Vector3d Normal |
| | | 151 | | { |
| | | 152 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 12 | 153 | | get => GetNormalized(this); |
| | | 154 | | } |
| | | 155 | | |
| | | 156 | | /// <summary> |
| | | 157 | | /// Returns the actual length of this vector (RO). |
| | | 158 | | /// </summary> |
| | | 159 | | [JsonIgnore] |
| | | 160 | | [MemoryPackIgnore] |
| | | 161 | | public Fixed64 Magnitude |
| | | 162 | | { |
| | | 163 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 221 | 164 | | get => GetMagnitude(this); |
| | | 165 | | } |
| | | 166 | | |
| | | 167 | | /// <summary> |
| | | 168 | | /// Calculates the forward direction vector based on the yaw (x) and pitch (y) angles. |
| | | 169 | | /// </summary> |
| | | 170 | | /// <remarks> |
| | | 171 | | /// This is commonly used to determine the direction an object is facing in 3D space, |
| | | 172 | | /// where 'x' represents the yaw (horizontal rotation) and 'y' represents the pitch (vertical rotation). |
| | | 173 | | /// </remarks> |
| | | 174 | | [JsonIgnore] |
| | | 175 | | [MemoryPackIgnore] |
| | | 176 | | public Vector3d Direction |
| | | 177 | | { |
| | | 178 | | get |
| | 1 | 179 | | { |
| | 1 | 180 | | Fixed64 temp1 = FixedMath.Cos(x) * FixedMath.Sin(y); |
| | 1 | 181 | | Fixed64 temp2 = FixedMath.Sin(-x); |
| | 1 | 182 | | Fixed64 temp3 = FixedMath.Cos(x) * FixedMath.Cos(y); |
| | 1 | 183 | | return new Vector3d(temp1, temp2, temp3); |
| | 1 | 184 | | } |
| | | 185 | | } |
| | | 186 | | |
| | | 187 | | /// <summary> |
| | | 188 | | /// Are all components of this vector equal to zero? |
| | | 189 | | /// </summary> |
| | | 190 | | /// <returns></returns> |
| | | 191 | | [JsonIgnore] |
| | | 192 | | [MemoryPackIgnore] |
| | | 193 | | public bool IsZero |
| | | 194 | | { |
| | | 195 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 4 | 196 | | get => this.Equals(Zero); |
| | | 197 | | } |
| | | 198 | | |
| | | 199 | | /// <summary> |
| | | 200 | | /// This vector's square magnitude. |
| | | 201 | | /// If you're doing distance checks, use SqrMagnitude and square the distance you're checking against |
| | | 202 | | /// If you need to know the actual distance, use MyMagnitude |
| | | 203 | | /// </summary> |
| | | 204 | | /// <returns>The magnitude.</returns> |
| | | 205 | | [JsonIgnore] |
| | | 206 | | [MemoryPackIgnore] |
| | | 207 | | public Fixed64 SqrMagnitude |
| | | 208 | | { |
| | | 209 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 10 | 210 | | get => (x * x) + (y * y) + (z * z); |
| | | 211 | | } |
| | | 212 | | |
| | | 213 | | /// <summary> |
| | | 214 | | /// Returns a long hash of the vector based on its x, y, and z values. |
| | | 215 | | /// </summary> |
| | | 216 | | [JsonIgnore] |
| | | 217 | | [MemoryPackIgnore] |
| | | 218 | | public long LongStateHash |
| | | 219 | | { |
| | | 220 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 12 | 221 | | get => (x.m_rawValue * 31) + (y.m_rawValue * 7) + (z.m_rawValue * 11); |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | /// <summary> |
| | | 225 | | /// Returns a hash of the vector based on its state. |
| | | 226 | | /// </summary> |
| | | 227 | | [JsonIgnore] |
| | | 228 | | [MemoryPackIgnore] |
| | | 229 | | public int StateHash |
| | | 230 | | { |
| | | 231 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 12 | 232 | | get => (int)(LongStateHash % int.MaxValue); |
| | | 233 | | } |
| | | 234 | | |
| | | 235 | | [JsonIgnore] |
| | | 236 | | [MemoryPackIgnore] |
| | | 237 | | public Fixed64 this[int index] |
| | | 238 | | { |
| | | 239 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 240 | | get |
| | 5 | 241 | | { |
| | 5 | 242 | | return index switch |
| | 5 | 243 | | { |
| | 1 | 244 | | 0 => x, |
| | 1 | 245 | | 1 => y, |
| | 1 | 246 | | 2 => z, |
| | 2 | 247 | | _ => throw new IndexOutOfRangeException("Invalid Vector3d index!"), |
| | 5 | 248 | | }; |
| | 3 | 249 | | } |
| | | 250 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 251 | | set |
| | 5 | 252 | | { |
| | 5 | 253 | | switch (index) |
| | | 254 | | { |
| | | 255 | | case 0: |
| | 1 | 256 | | x = value; |
| | 1 | 257 | | break; |
| | | 258 | | case 1: |
| | 1 | 259 | | y = value; |
| | 1 | 260 | | break; |
| | | 261 | | case 2: |
| | 1 | 262 | | z = value; |
| | 1 | 263 | | break; |
| | | 264 | | default: |
| | 2 | 265 | | throw new IndexOutOfRangeException("Invalid Vector3d index!"); |
| | | 266 | | } |
| | 3 | 267 | | } |
| | | 268 | | } |
| | | 269 | | |
| | | 270 | | #endregion |
| | | 271 | | |
| | | 272 | | #region Methods |
| | | 273 | | |
| | | 274 | | /// <summary> |
| | | 275 | | /// Set x, y and z components of an existing Vector3. |
| | | 276 | | /// </summary> |
| | | 277 | | /// <param name="newX"></param> |
| | | 278 | | /// <param name="newY"></param> |
| | | 279 | | /// <param name="newZ"></param> |
| | | 280 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 281 | | public Vector3d Set(Fixed64 newX, Fixed64 newY, Fixed64 newZ) |
| | 1 | 282 | | { |
| | 1 | 283 | | x = newX; |
| | 1 | 284 | | y = newY; |
| | 1 | 285 | | z = newZ; |
| | 1 | 286 | | return this; |
| | 1 | 287 | | } |
| | | 288 | | |
| | | 289 | | /// <summary> |
| | | 290 | | /// Adds the specified values to the components of the vector in place and returns the modified vector. |
| | | 291 | | /// </summary> |
| | | 292 | | /// <param name="amount">The amount to add to the components.</param> |
| | | 293 | | /// <returns>The modified vector after addition.</returns> |
| | | 294 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 295 | | public Vector3d AddInPlace(Fixed64 amount) |
| | 1 | 296 | | { |
| | 1 | 297 | | x += amount; |
| | 1 | 298 | | y += amount; |
| | 1 | 299 | | z += amount; |
| | 1 | 300 | | return this; |
| | 1 | 301 | | } |
| | | 302 | | |
| | | 303 | | /// <summary> |
| | | 304 | | /// Adds the specified values to the components of the vector in place and returns the modified vector. |
| | | 305 | | /// </summary> |
| | | 306 | | /// <param name="xAmount">The amount to add to the x component.</param> |
| | | 307 | | /// <param name="yAmount">The amount to add to the y component.</param> |
| | | 308 | | /// <param name="zAmount">The amount to add to the z component.</param> |
| | | 309 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 310 | | public Vector3d AddInPlace(Fixed64 xAmount, Fixed64 yAmount, Fixed64 zAmount) |
| | 2 | 311 | | { |
| | 2 | 312 | | x += xAmount; |
| | 2 | 313 | | y += yAmount; |
| | 2 | 314 | | z += zAmount; |
| | 2 | 315 | | return this; |
| | 2 | 316 | | } |
| | | 317 | | |
| | | 318 | | /// <summary> |
| | | 319 | | /// Adds the specified vector components to the corresponding components of the in place vector and returns the modi |
| | | 320 | | /// </summary> |
| | | 321 | | /// <param name="other">The other vector to add the components.</param> |
| | | 322 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 323 | | public Vector3d AddInPlace(Vector3d other) |
| | 1 | 324 | | { |
| | 1 | 325 | | AddInPlace(other.x, other.y, other.z); |
| | 1 | 326 | | return this; |
| | 1 | 327 | | } |
| | | 328 | | |
| | | 329 | | /// <summary> |
| | | 330 | | /// Subtracts the specified value from all components of the vector in place and returns the modified vector. |
| | | 331 | | /// </summary> |
| | | 332 | | /// <param name="amount">The amount to subtract from each component.</param> |
| | | 333 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 334 | | public Vector3d SubtractInPlace(Fixed64 amount) |
| | 1 | 335 | | { |
| | 1 | 336 | | x -= amount; |
| | 1 | 337 | | y -= amount; |
| | 1 | 338 | | z -= amount; |
| | 1 | 339 | | return this; |
| | 1 | 340 | | } |
| | | 341 | | |
| | | 342 | | /// <summary> |
| | | 343 | | /// Subtracts the specified values from the components of the vector in place and returns the modified vector. |
| | | 344 | | /// </summary> |
| | | 345 | | /// <param name="xAmount">The amount to subtract from the x component.</param> |
| | | 346 | | /// <param name="yAmount">The amount to subtract from the y component.</param> |
| | | 347 | | /// <param name="zAmount">The amount to subtract from the z component.</param> |
| | | 348 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 349 | | public Vector3d SubtractInPlace(Fixed64 xAmount, Fixed64 yAmount, Fixed64 zAmount) |
| | 2 | 350 | | { |
| | 2 | 351 | | x -= xAmount; |
| | 2 | 352 | | y -= yAmount; |
| | 2 | 353 | | z -= zAmount; |
| | 2 | 354 | | return this; |
| | 2 | 355 | | } |
| | | 356 | | |
| | | 357 | | /// <summary> |
| | | 358 | | /// Subtracts the specified vector from the components of the vector in place and returns the modified vector. |
| | | 359 | | /// </summary> |
| | | 360 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 361 | | public Vector3d SubtractInPlace(Vector3d other) |
| | 1 | 362 | | { |
| | 1 | 363 | | SubtractInPlace(other.x, other.y, other.z); |
| | 1 | 364 | | return this; |
| | 1 | 365 | | } |
| | | 366 | | |
| | | 367 | | /// <summary> |
| | | 368 | | /// Scales the components of the vector by the specified scalar factor in place and returns the modified vector. |
| | | 369 | | /// </summary> |
| | | 370 | | /// <param name="scaleFactor">The scalar factor to multiply each component by.</param> |
| | | 371 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 372 | | public Vector3d ScaleInPlace(Fixed64 scaleFactor) |
| | 1 | 373 | | { |
| | 1 | 374 | | x *= scaleFactor; |
| | 1 | 375 | | y *= scaleFactor; |
| | 1 | 376 | | z *= scaleFactor; |
| | 1 | 377 | | return this; |
| | 1 | 378 | | } |
| | | 379 | | |
| | | 380 | | /// <summary> |
| | | 381 | | /// Scales each component of the vector by the corresponding component of the given vector in place and returns the |
| | | 382 | | /// </summary> |
| | | 383 | | /// <param name="scale">The vector containing the scale factors for each component.</param> |
| | | 384 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 385 | | public Vector3d ScaleInPlace(Vector3d scale) |
| | 1 | 386 | | { |
| | 1 | 387 | | x *= scale.x; |
| | 1 | 388 | | y *= scale.y; |
| | 1 | 389 | | z *= scale.z; |
| | 1 | 390 | | return this; |
| | 1 | 391 | | } |
| | | 392 | | |
| | | 393 | | /// <summary> |
| | | 394 | | /// Normalizes this vector in place, making its magnitude (length) equal to 1, and returns the modified vector. |
| | | 395 | | /// </summary> |
| | | 396 | | /// <remarks> |
| | | 397 | | /// If the vector is zero-length or already normalized, no operation is performed. |
| | | 398 | | /// This method modifies the current vector in place and supports method chaining. |
| | | 399 | | /// </remarks> |
| | | 400 | | /// <returns>The normalized vector.</returns> |
| | | 401 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 402 | | public Vector3d Normalize() |
| | 64 | 403 | | { |
| | 64 | 404 | | return this = GetNormalized(this); |
| | 64 | 405 | | } |
| | | 406 | | |
| | | 407 | | /// <summary> |
| | | 408 | | /// Normalizes this vector in place and outputs its original magnitude. |
| | | 409 | | /// </summary> |
| | | 410 | | /// <remarks> |
| | | 411 | | /// If the vector is zero-length or already normalized, no operation is performed, but the original magnitude will s |
| | | 412 | | /// </remarks> |
| | | 413 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 414 | | public Vector3d Normalize(out Fixed64 mag) |
| | 3 | 415 | | { |
| | 3 | 416 | | mag = GetMagnitude(this); |
| | | 417 | | |
| | | 418 | | // If magnitude is zero, return a zero vector to avoid divide-by-zero errors |
| | 3 | 419 | | if (mag == Fixed64.Zero) |
| | 1 | 420 | | { |
| | 1 | 421 | | x = Fixed64.Zero; |
| | 1 | 422 | | y = Fixed64.Zero; |
| | 1 | 423 | | z = Fixed64.Zero; |
| | 1 | 424 | | return this; |
| | | 425 | | } |
| | | 426 | | |
| | | 427 | | // If already normalized, return as-is |
| | 2 | 428 | | if (mag == Fixed64.One) |
| | 1 | 429 | | return this; |
| | | 430 | | |
| | 1 | 431 | | x /= mag; |
| | 1 | 432 | | y /= mag; |
| | 1 | 433 | | z /= mag; |
| | | 434 | | |
| | 1 | 435 | | return this; |
| | 3 | 436 | | } |
| | | 437 | | |
| | | 438 | | /// <summary> |
| | | 439 | | /// Checks if this vector has been normalized by checking if the magnitude is close to 1. |
| | | 440 | | /// </summary> |
| | | 441 | | public bool IsNormalized() |
| | 39 | 442 | | { |
| | 39 | 443 | | return FixedMath.Abs(Magnitude - Fixed64.One) <= Fixed64.Epsilon; |
| | 39 | 444 | | } |
| | | 445 | | |
| | | 446 | | /// <summary> |
| | | 447 | | /// Checks whether all components are strictly greater than <see cref="Fixed64.Epsilon"/>. |
| | | 448 | | /// </summary> |
| | | 449 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 450 | | public bool AllComponentsGreaterThanEpsilon() |
| | 2 | 451 | | { |
| | 2 | 452 | | return x > Fixed64.Epsilon && y > Fixed64.Epsilon && z > Fixed64.Epsilon; |
| | 2 | 453 | | } |
| | | 454 | | |
| | | 455 | | /// <summary> |
| | | 456 | | /// Computes the distance between this vector and another vector. |
| | | 457 | | /// </summary> |
| | | 458 | | /// <param name="otherX">The x component of the other vector.</param> |
| | | 459 | | /// <param name="otherY">The y component of the other vector.</param> |
| | | 460 | | /// <param name="otherZ">The z component of the other vector.</param> |
| | | 461 | | /// <returns>The distance between the two vectors.</returns> |
| | | 462 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 463 | | public Fixed64 Distance(Fixed64 otherX, Fixed64 otherY, Fixed64 otherZ) |
| | 16 | 464 | | { |
| | 16 | 465 | | Fixed64 temp1 = x - otherX; |
| | 16 | 466 | | temp1 *= temp1; |
| | 16 | 467 | | Fixed64 temp2 = y - otherY; |
| | 16 | 468 | | temp2 *= temp2; |
| | 16 | 469 | | Fixed64 temp3 = z - otherZ; |
| | 16 | 470 | | temp3 *= temp3; |
| | 16 | 471 | | return FixedMath.Sqrt(temp1 + temp2 + temp3); |
| | 16 | 472 | | } |
| | | 473 | | |
| | | 474 | | /// <summary> |
| | | 475 | | /// Calculates the squared distance between two vectors, avoiding the need for a square root operation. |
| | | 476 | | /// </summary> |
| | | 477 | | /// <returns>The squared distance between the two vectors.</returns> |
| | | 478 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 479 | | public Fixed64 SqrDistance(Fixed64 otherX, Fixed64 otherY, Fixed64 otherZ) |
| | 13 | 480 | | { |
| | 13 | 481 | | Fixed64 temp1 = x - otherX; |
| | 13 | 482 | | temp1 *= temp1; |
| | 13 | 483 | | Fixed64 temp2 = y - otherY; |
| | 13 | 484 | | temp2 *= temp2; |
| | 13 | 485 | | Fixed64 temp3 = z - otherZ; |
| | 13 | 486 | | temp3 *= temp3; |
| | 13 | 487 | | return temp1 + temp2 + temp3; |
| | 13 | 488 | | } |
| | | 489 | | |
| | | 490 | | /// <summary> |
| | | 491 | | /// Computes the dot product of this vector with another vector specified by its components. |
| | | 492 | | /// </summary> |
| | | 493 | | /// <param name="otherX">The x component of the other vector.</param> |
| | | 494 | | /// <param name="otherY">The y component of the other vector.</param> |
| | | 495 | | /// <param name="otherZ">The z component of the other vector.</param> |
| | | 496 | | /// <returns>The dot product of the two vectors.</returns> |
| | | 497 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 498 | | public Fixed64 Dot(Fixed64 otherX, Fixed64 otherY, Fixed64 otherZ) |
| | 52 | 499 | | { |
| | 52 | 500 | | return x * otherX + y * otherY + z * otherZ; |
| | 52 | 501 | | } |
| | | 502 | | |
| | | 503 | | /// <summary> |
| | | 504 | | /// Computes the cross product magnitude of this vector with another vector. |
| | | 505 | | /// </summary> |
| | | 506 | | /// <param name="otherX">The X component of the other vector.</param> |
| | | 507 | | /// <param name="otherY">The Y component of the other vector.</param> |
| | | 508 | | /// <param name="otherZ">The Z component of the other vector.</param> |
| | | 509 | | /// <returns>The cross product magnitude.</returns> |
| | | 510 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 511 | | public Fixed64 CrossProduct(Fixed64 otherX, Fixed64 otherY, Fixed64 otherZ) |
| | 2 | 512 | | { |
| | 2 | 513 | | return (y * otherZ - z * otherY) + (z * otherX - x * otherZ) + (x * otherY - y * otherX); |
| | 2 | 514 | | } |
| | | 515 | | |
| | | 516 | | /// <summary> |
| | | 517 | | /// Returns the cross vector of this vector with another vector. |
| | | 518 | | /// </summary> |
| | | 519 | | /// <returns>A new vector representing the cross product.</returns> |
| | | 520 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 521 | | public Vector3d Cross(Fixed64 otherX, Fixed64 otherY, Fixed64 otherZ) |
| | 13 | 522 | | { |
| | 13 | 523 | | return new Vector3d( |
| | 13 | 524 | | y * otherZ - z * otherY, |
| | 13 | 525 | | z * otherX - x * otherZ, |
| | 13 | 526 | | x * otherY - y * otherX); |
| | 13 | 527 | | } |
| | | 528 | | |
| | | 529 | | #endregion |
| | | 530 | | |
| | | 531 | | #region Vector3d Operations |
| | | 532 | | |
| | | 533 | | /// <summary> |
| | | 534 | | /// Linearly interpolates between two points. |
| | | 535 | | /// </summary> |
| | | 536 | | /// <param name="a">Start value, returned when t = 0.</param> |
| | | 537 | | /// <param name="b">End value, returned when t = 1.</param> |
| | | 538 | | /// <param name="mag">Value used to interpolate between a and b.</param> |
| | | 539 | | /// <returns> Interpolated value, equals to a + (b - a) * t.</returns> |
| | | 540 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 541 | | public static Vector3d Lerp(Vector3d a, Vector3d b, Fixed64 mag) |
| | 3 | 542 | | { |
| | 3 | 543 | | mag = FixedMath.Clamp01(mag); |
| | 3 | 544 | | return new Vector3d(a.x + (b.x - a.x) * mag, a.y + (b.y - a.y) * mag, a.z + (b.z - a.z) * mag); |
| | 3 | 545 | | } |
| | | 546 | | |
| | | 547 | | /// <summary> |
| | | 548 | | /// Linearly interpolates between two vectors without clamping the interpolation factor between 0 and 1. |
| | | 549 | | /// </summary> |
| | | 550 | | /// <param name="a">The start vector.</param> |
| | | 551 | | /// <param name="b">The end vector.</param> |
| | | 552 | | /// <param name="t">The interpolation factor. Values outside the range [0, 1] will cause the interpolation to go bey |
| | | 553 | | /// <returns>The interpolated vector.</returns> |
| | | 554 | | /// <remarks> |
| | | 555 | | /// Unlike traditional Lerp, this function allows interpolation factors greater than 1 or less than 0, |
| | | 556 | | /// which means the resulting vector can extend beyond the endpoints. |
| | | 557 | | /// </remarks> |
| | | 558 | | public static Vector3d UnclampedLerp(Vector3d a, Vector3d b, Fixed64 t) |
| | 2 | 559 | | { |
| | 2 | 560 | | return (b - a) * t + a; |
| | 2 | 561 | | } |
| | | 562 | | |
| | | 563 | | /// <summary> |
| | | 564 | | /// Moves from a to b at some speed dependent of a delta time with out passing b. |
| | | 565 | | /// </summary> |
| | | 566 | | /// <param name="a"></param> |
| | | 567 | | /// <param name="b"></param> |
| | | 568 | | /// <param name="speed"></param> |
| | | 569 | | /// <param name="dt"></param> |
| | | 570 | | /// <returns></returns> |
| | | 571 | | public static Vector3d SpeedLerp(Vector3d a, Vector3d b, Fixed64 speed, Fixed64 dt) |
| | 3 | 572 | | { |
| | 3 | 573 | | Vector3d v = b - a; |
| | 3 | 574 | | Fixed64 dv = speed * dt; |
| | 3 | 575 | | if (dv > v.Magnitude) |
| | 1 | 576 | | return b; |
| | | 577 | | else |
| | 2 | 578 | | return a + v.Normal * dv; |
| | 3 | 579 | | } |
| | | 580 | | |
| | | 581 | | /// <summary> |
| | | 582 | | /// Spherically interpolates between two vectors, moving along the shortest arc on a unit sphere. |
| | | 583 | | /// </summary> |
| | | 584 | | /// <param name="start">The starting vector.</param> |
| | | 585 | | /// <param name="end">The ending vector.</param> |
| | | 586 | | /// <param name="percent">A value between 0 and 1 that represents the interpolation amount. 0 returns the start vect |
| | | 587 | | /// <returns>The interpolated vector between the two input vectors.</returns> |
| | | 588 | | /// <remarks> |
| | | 589 | | /// Slerp is used to interpolate between two unit vectors on a sphere, providing smooth rotation. |
| | | 590 | | /// It can be more computationally expensive than linear interpolation (Lerp) but results in smoother, arc-like moti |
| | | 591 | | /// </remarks> |
| | | 592 | | public static Vector3d Slerp(Vector3d start, Vector3d end, Fixed64 percent) |
| | 3 | 593 | | { |
| | | 594 | | // Dot product - the cosine of the angle between 2 vectors. |
| | 3 | 595 | | Fixed64 dot = Dot(start, end); |
| | | 596 | | // Clamp it to be in the range of Acos() |
| | | 597 | | // This may be unnecessary, but floating point |
| | | 598 | | // precision can be a fickle mistress. |
| | 3 | 599 | | FixedMath.Clamp(dot, -Fixed64.One, Fixed64.One); |
| | | 600 | | // Acos(dot) returns the angle between start and end, |
| | | 601 | | // And multiplying that by percent returns the angle between |
| | | 602 | | // start and the final result. |
| | 3 | 603 | | Fixed64 theta = FixedMath.Acos(dot) * percent; |
| | 3 | 604 | | Vector3d RelativeVec = end - start * dot; |
| | 3 | 605 | | RelativeVec.Normalize(); |
| | | 606 | | // Orthonormal basis |
| | | 607 | | // The final result. |
| | 3 | 608 | | return (start * FixedMath.Cos(theta)) + (RelativeVec * FixedMath.Sin(theta)); |
| | 3 | 609 | | } |
| | | 610 | | |
| | | 611 | | /// <summary> |
| | | 612 | | /// Normalizes the given vector, returning a unit vector with the same direction. |
| | | 613 | | /// </summary> |
| | | 614 | | /// <param name="value">The vector to normalize.</param> |
| | | 615 | | /// <returns>A normalized (unit) vector with the same direction.</returns> |
| | | 616 | | public static Vector3d GetNormalized(Vector3d value) |
| | 76 | 617 | | { |
| | 76 | 618 | | Fixed64 mag = GetMagnitude(value); |
| | | 619 | | |
| | | 620 | | // If magnitude is zero, return a zero vector to avoid divide-by-zero errors |
| | 76 | 621 | | if (mag == Fixed64.Zero) |
| | 0 | 622 | | return new Vector3d(Fixed64.Zero, Fixed64.Zero, Fixed64.Zero); |
| | | 623 | | |
| | | 624 | | // If already normalized, return as-is |
| | 76 | 625 | | if (mag == Fixed64.One) |
| | 47 | 626 | | return value; |
| | | 627 | | |
| | | 628 | | // Normalize it exactly |
| | 29 | 629 | | return new Vector3d( |
| | 29 | 630 | | value.x / mag, |
| | 29 | 631 | | value.y / mag, |
| | 29 | 632 | | value.z / mag |
| | 29 | 633 | | ); |
| | 76 | 634 | | } |
| | | 635 | | |
| | | 636 | | /// <summary> |
| | | 637 | | /// Returns the magnitude (length) of this vector. |
| | | 638 | | /// </summary> |
| | | 639 | | /// <param name="vector">The vector whose magnitude is being calculated.</param> |
| | | 640 | | /// <returns>The magnitude of the vector.</returns> |
| | | 641 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 642 | | public static Fixed64 GetMagnitude(Vector3d vector) |
| | 301 | 643 | | { |
| | 301 | 644 | | Fixed64 mag = (vector.x * vector.x) + (vector.y * vector.y) + (vector.z * vector.z); |
| | | 645 | | |
| | | 646 | | // If rounding error pushed magnitude slightly above 1, clamp it |
| | 301 | 647 | | if (mag > Fixed64.One && mag <= Fixed64.One + Fixed64.Epsilon) |
| | 4 | 648 | | return Fixed64.One; |
| | | 649 | | |
| | 297 | 650 | | return mag != Fixed64.Zero ? FixedMath.Sqrt(mag) : Fixed64.Zero; |
| | 301 | 651 | | } |
| | | 652 | | |
| | | 653 | | /// <summary> |
| | | 654 | | /// Returns a new <see cref="Vector3d"/> where each component is the absolute value of the corresponding input compo |
| | | 655 | | /// </summary> |
| | | 656 | | /// <param name="value">The input vector.</param> |
| | | 657 | | /// <returns>A vector with absolute values for each component.</returns> |
| | | 658 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 659 | | public static Vector3d Abs(Vector3d value) |
| | 52 | 660 | | { |
| | 52 | 661 | | return new Vector3d(value.x.Abs(), value.y.Abs(), value.z.Abs()); |
| | 52 | 662 | | } |
| | | 663 | | |
| | | 664 | | /// <summary> |
| | | 665 | | /// Returns a new <see cref="Vector3d"/> where each component is the sign of the corresponding input component. |
| | | 666 | | /// </summary> |
| | | 667 | | /// <param name="value">The input vector.</param> |
| | | 668 | | /// <returns>A vector where each component is -1, 0, or 1 based on the sign of the input.</returns> |
| | | 669 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 670 | | public static Vector3d Sign(Vector3d value) |
| | 1 | 671 | | { |
| | 1 | 672 | | return new Vector3d(value.x.Sign(), value.y.Sign(), value.z.Sign()); |
| | 1 | 673 | | } |
| | | 674 | | |
| | | 675 | | /// <summary> |
| | | 676 | | /// Clamps each component of the given <see cref="Vector3d"/> within the specified min and max bounds. |
| | | 677 | | /// </summary> |
| | | 678 | | /// <param name="value">The vector to clamp.</param> |
| | | 679 | | /// <param name="min">The minimum bounds.</param> |
| | | 680 | | /// <param name="max">The maximum bounds.</param> |
| | | 681 | | /// <returns>A vector with each component clamped between min and max.</returns> |
| | | 682 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 683 | | public static Vector3d Clamp(Vector3d value, Vector3d min, Vector3d max) |
| | 3 | 684 | | { |
| | 3 | 685 | | return new Vector3d( |
| | 3 | 686 | | FixedMath.Clamp(value.x, min.x, max.x), |
| | 3 | 687 | | FixedMath.Clamp(value.y, min.y, max.y), |
| | 3 | 688 | | FixedMath.Clamp(value.z, min.z, max.z) |
| | 3 | 689 | | ); |
| | 3 | 690 | | } |
| | | 691 | | |
| | | 692 | | /// <summary> |
| | | 693 | | /// Clamps the given Vector3d within the specified magnitude. |
| | | 694 | | /// </summary> |
| | | 695 | | /// <param name="value"></param> |
| | | 696 | | /// <param name="maxMagnitude"></param> |
| | | 697 | | /// <returns></returns> |
| | | 698 | | public static Vector3d ClampMagnitude(Vector3d value, Fixed64 maxMagnitude) |
| | 2 | 699 | | { |
| | 2 | 700 | | if (value.SqrMagnitude > maxMagnitude * maxMagnitude) |
| | 1 | 701 | | return value.Normal * maxMagnitude; // Scale vector to max magnitude |
| | | 702 | | |
| | 1 | 703 | | return value; |
| | 2 | 704 | | } |
| | | 705 | | |
| | | 706 | | /// <summary> |
| | | 707 | | /// Determines if two vectors are exactly parallel by checking if their cross product is zero. |
| | | 708 | | /// </summary> |
| | | 709 | | /// <param name="v1">The first vector.</param> |
| | | 710 | | /// <param name="v2">The second vector.</param> |
| | | 711 | | /// <returns>True if the vectors are exactly parallel, false otherwise.</returns> |
| | | 712 | | public static bool AreParallel(Vector3d v1, Vector3d v2) |
| | 2 | 713 | | { |
| | 2 | 714 | | return Cross(v1, v2).SqrMagnitude == Fixed64.Zero; |
| | 2 | 715 | | } |
| | | 716 | | |
| | | 717 | | /// <summary> |
| | | 718 | | /// Determines if two vectors are approximately parallel based on a cosine similarity threshold. |
| | | 719 | | /// </summary> |
| | | 720 | | /// <param name="v1">The first normalized vector.</param> |
| | | 721 | | /// <param name="v2">The second normalized vector.</param> |
| | | 722 | | /// <param name="cosThreshold">The cosine similarity threshold for near-parallel vectors.</param> |
| | | 723 | | /// <returns>True if the vectors are nearly parallel, false otherwise.</returns> |
| | | 724 | | public static bool AreAlmostParallel(Vector3d v1, Vector3d v2, Fixed64 cosThreshold) |
| | 2 | 725 | | { |
| | | 726 | | // Assuming v1 and v2 are already normalized |
| | 2 | 727 | | Fixed64 dot = Dot(v1, v2); |
| | | 728 | | |
| | | 729 | | // Compare dot product directly to the cosine threshold |
| | 2 | 730 | | return dot >= cosThreshold; |
| | 2 | 731 | | } |
| | | 732 | | |
| | | 733 | | /// <summary> |
| | | 734 | | /// Computes the midpoint between two vectors. |
| | | 735 | | /// </summary> |
| | | 736 | | /// <param name="v1">The first vector.</param> |
| | | 737 | | /// <param name="v2">The second vector.</param> |
| | | 738 | | /// <returns>The midpoint vector.</returns> |
| | | 739 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 740 | | public static Vector3d Midpoint(Vector3d v1, Vector3d v2) |
| | 2 | 741 | | { |
| | 2 | 742 | | return new Vector3d((v1.x + v2.x) * Fixed64.Half, (v1.y + v2.y) * Fixed64.Half, (v1.z + v2.z) * Fixed64.Half); |
| | 2 | 743 | | } |
| | | 744 | | |
| | | 745 | | /// <inheritdoc cref="Distance(Fixed64, Fixed64, Fixed64)" /> |
| | | 746 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 747 | | public static Fixed64 Distance(Vector3d start, Vector3d end) |
| | 16 | 748 | | { |
| | 16 | 749 | | return start.Distance(end.x, end.y, end.z); |
| | 16 | 750 | | } |
| | | 751 | | |
| | | 752 | | /// <inheritdoc cref="SqrDistance(Fixed64, Fixed64, Fixed64)" /> |
| | | 753 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 754 | | public static Fixed64 SqrDistance(Vector3d start, Vector3d end) |
| | 13 | 755 | | { |
| | 13 | 756 | | return start.SqrDistance(end.x, end.y, end.z); |
| | 13 | 757 | | } |
| | | 758 | | |
| | | 759 | | /// <summary> |
| | | 760 | | /// Calculates the closest points on two line segments. |
| | | 761 | | /// </summary> |
| | | 762 | | /// <param name="line1Start">The starting point of the first line segment.</param> |
| | | 763 | | /// <param name="line1End">The ending point of the first line segment.</param> |
| | | 764 | | /// <param name="line2Start">The starting point of the second line segment.</param> |
| | | 765 | | /// <param name="line2End">The ending point of the second line segment.</param> |
| | | 766 | | /// <returns> |
| | | 767 | | /// A tuple containing two points representing the closest points on each line segment. The first item is the closes |
| | | 768 | | /// and the second item is the closest point on the second line. |
| | | 769 | | /// </returns> |
| | | 770 | | /// <remarks> |
| | | 771 | | /// This method considers the line segments, not the infinite lines they represent, ensuring that the returned point |
| | | 772 | | /// </remarks> |
| | | 773 | | public static (Vector3d, Vector3d) ClosestPointsOnTwoLines(Vector3d line1Start, Vector3d line1End, Vector3d line2Sta |
| | 6 | 774 | | { |
| | 6 | 775 | | Vector3d u = line1End - line1Start; |
| | 6 | 776 | | Vector3d v = line2End - line2Start; |
| | 6 | 777 | | Vector3d w = line1Start - line2Start; |
| | | 778 | | |
| | 6 | 779 | | Fixed64 a = Dot(u, u); |
| | 6 | 780 | | Fixed64 b = Dot(u, v); |
| | 6 | 781 | | Fixed64 c = Dot(v, v); |
| | 6 | 782 | | Fixed64 d = Dot(u, w); |
| | 6 | 783 | | Fixed64 e = Dot(v, w); |
| | 6 | 784 | | Fixed64 D = a * c - b * b; |
| | | 785 | | |
| | | 786 | | Fixed64 sc, tc; |
| | | 787 | | |
| | | 788 | | // compute the line parameters of the two closest points |
| | 6 | 789 | | if (D < Fixed64.Epsilon) |
| | 1 | 790 | | { |
| | | 791 | | // the lines are almost parallel |
| | 1 | 792 | | sc = Fixed64.Zero; |
| | 1 | 793 | | tc = (b > c ? d / b : e / c); // use the largest denominator |
| | 1 | 794 | | } |
| | | 795 | | else |
| | 5 | 796 | | { |
| | 5 | 797 | | sc = (b * e - c * d) / D; |
| | 5 | 798 | | tc = (a * e - b * d) / D; |
| | 5 | 799 | | } |
| | | 800 | | |
| | | 801 | | // recompute sc if it is outside [0,1] |
| | 6 | 802 | | if (sc < Fixed64.Zero) |
| | 1 | 803 | | { |
| | 1 | 804 | | sc = Fixed64.Zero; |
| | 1 | 805 | | tc = (e < Fixed64.Zero ? Fixed64.Zero : (e > c ? Fixed64.One : e / c)); |
| | 1 | 806 | | } |
| | 5 | 807 | | else if (sc > Fixed64.One) |
| | 1 | 808 | | { |
| | 1 | 809 | | sc = Fixed64.One; |
| | 1 | 810 | | tc = (e + b < Fixed64.Zero ? Fixed64.Zero : (e + b > c ? Fixed64.One : (e + b) / c)); |
| | 1 | 811 | | } |
| | | 812 | | |
| | | 813 | | // recompute tc if it is outside [0,1] |
| | 6 | 814 | | if (tc < Fixed64.Zero) |
| | 1 | 815 | | { |
| | 1 | 816 | | tc = Fixed64.Zero; |
| | 1 | 817 | | sc = (-d < Fixed64.Zero ? Fixed64.Zero : (-d > a ? Fixed64.One : -d / a)); |
| | 1 | 818 | | } |
| | 5 | 819 | | else if (tc > Fixed64.One) |
| | 1 | 820 | | { |
| | 1 | 821 | | tc = Fixed64.One; |
| | 1 | 822 | | sc = ((-d + b) < Fixed64.Zero ? Fixed64.Zero : ((-d + b) > a ? Fixed64.One : (-d + b) / a)); |
| | 1 | 823 | | } |
| | | 824 | | |
| | | 825 | | // get the difference of the two closest points |
| | 6 | 826 | | Vector3d pointOnLine1 = line1Start + sc * u; |
| | 6 | 827 | | Vector3d pointOnLine2 = line2Start + tc * v; |
| | | 828 | | |
| | 6 | 829 | | return (pointOnLine1, pointOnLine2); |
| | 6 | 830 | | } |
| | | 831 | | |
| | | 832 | | /// <summary> |
| | | 833 | | /// Finds the closest point on a line segment between points A and B to a given point P. |
| | | 834 | | /// </summary> |
| | | 835 | | /// <param name="a">The start of the line segment.</param> |
| | | 836 | | /// <param name="b">The end of the line segment.</param> |
| | | 837 | | /// <param name="p">The point to project onto the segment.</param> |
| | | 838 | | /// <returns>The closest point on the line segment to P.</returns> |
| | | 839 | | public static Vector3d ClosestPointOnLineSegment(Vector3d a, Vector3d b, Vector3d p) |
| | 2 | 840 | | { |
| | 2 | 841 | | Vector3d ab = b - a; |
| | 2 | 842 | | Fixed64 t = Dot(p - a, ab) / Dot(ab, ab); |
| | 2 | 843 | | t = FixedMath.Max(Fixed64.Zero, FixedMath.Min(Fixed64.One, t)); |
| | 2 | 844 | | return a + ab * t; |
| | 2 | 845 | | } |
| | | 846 | | |
| | | 847 | | /// <summary> |
| | | 848 | | /// Dot Product of two vectors. |
| | | 849 | | /// </summary> |
| | | 850 | | /// <param name="lhs"></param> |
| | | 851 | | /// <param name="rhs"></param> |
| | | 852 | | /// <returns></returns> |
| | | 853 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 854 | | public static Fixed64 Dot(Vector3d lhs, Vector3d rhs) |
| | 52 | 855 | | { |
| | 52 | 856 | | return lhs.Dot(rhs.x, rhs.y, rhs.z); |
| | 52 | 857 | | } |
| | | 858 | | |
| | | 859 | | /// <summary> |
| | | 860 | | /// Multiplies two vectors component-wise. |
| | | 861 | | /// </summary> |
| | | 862 | | /// <param name="a"></param> |
| | | 863 | | /// <param name="b"></param> |
| | | 864 | | /// <returns></returns> |
| | | 865 | | public static Vector3d Scale(Vector3d a, Vector3d b) |
| | 1 | 866 | | { |
| | 1 | 867 | | return new Vector3d(a.x * b.x, a.y * b.y, a.z * b.z); |
| | 1 | 868 | | } |
| | | 869 | | |
| | | 870 | | /// <summary> |
| | | 871 | | /// Cross Product of two vectors. |
| | | 872 | | /// </summary> |
| | | 873 | | /// <param name="lhs"></param> |
| | | 874 | | /// <param name="rhs"></param> |
| | | 875 | | /// <returns></returns> |
| | | 876 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 877 | | public static Vector3d Cross(Vector3d lhs, Vector3d rhs) |
| | 13 | 878 | | { |
| | 13 | 879 | | return lhs.Cross(rhs.x, rhs.y, rhs.z); |
| | 13 | 880 | | } |
| | | 881 | | |
| | | 882 | | /// <inheritdoc cref="CrossProduct(Fixed64, Fixed64, Fixed64)"/> |
| | | 883 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 884 | | public static Fixed64 CrossProduct(Vector3d lhs, Vector3d rhs) |
| | 1 | 885 | | { |
| | 1 | 886 | | return lhs.CrossProduct(rhs.x, rhs.y, rhs.z); |
| | 1 | 887 | | } |
| | | 888 | | |
| | | 889 | | /// <summary> |
| | | 890 | | /// Projects a vector onto another vector. |
| | | 891 | | /// </summary> |
| | | 892 | | /// <param name="vector"></param> |
| | | 893 | | /// <param name="onNormal"></param> |
| | | 894 | | /// <returns></returns> |
| | | 895 | | public static Vector3d Project(Vector3d vector, Vector3d onNormal) |
| | 3 | 896 | | { |
| | 3 | 897 | | Fixed64 sqrMag = Dot(onNormal, onNormal); |
| | 3 | 898 | | if (sqrMag < Fixed64.Epsilon) |
| | 1 | 899 | | return Zero; |
| | | 900 | | else |
| | 2 | 901 | | { |
| | 2 | 902 | | Fixed64 dot = Dot(vector, onNormal); |
| | 2 | 903 | | return new Vector3d(onNormal.x * dot / sqrMag, |
| | 2 | 904 | | onNormal.y * dot / sqrMag, |
| | 2 | 905 | | onNormal.z * dot / sqrMag); |
| | | 906 | | } |
| | 3 | 907 | | } |
| | | 908 | | |
| | | 909 | | /// <summary> |
| | | 910 | | /// Projects a vector onto a plane defined by a normal orthogonal to the plane. |
| | | 911 | | /// </summary> |
| | | 912 | | /// <param name="vector"></param> |
| | | 913 | | /// <param name="planeNormal"></param> |
| | | 914 | | /// <returns></returns> |
| | | 915 | | public static Vector3d ProjectOnPlane(Vector3d vector, Vector3d planeNormal) |
| | 2 | 916 | | { |
| | 2 | 917 | | Fixed64 sqrMag = Dot(planeNormal, planeNormal); |
| | 2 | 918 | | if (sqrMag < Fixed64.Epsilon) |
| | 1 | 919 | | return vector; |
| | | 920 | | else |
| | 1 | 921 | | { |
| | 1 | 922 | | Fixed64 dot = Dot(vector, planeNormal); |
| | 1 | 923 | | return new Vector3d(vector.x - planeNormal.x * dot / sqrMag, |
| | 1 | 924 | | vector.y - planeNormal.y * dot / sqrMag, |
| | 1 | 925 | | vector.z - planeNormal.z * dot / sqrMag); |
| | | 926 | | } |
| | 2 | 927 | | } |
| | | 928 | | |
| | | 929 | | /// <summary> |
| | | 930 | | /// Computes the angle in degrees between two vectors. |
| | | 931 | | /// </summary> |
| | | 932 | | /// <param name="from">The starting vector.</param> |
| | | 933 | | /// <param name="to">The target vector.</param> |
| | | 934 | | /// <returns>The angle in degrees between the two vectors.</returns> |
| | | 935 | | /// <remarks> |
| | | 936 | | /// This method calculates the angle by using the dot product between the vectors and normalizing the result. |
| | | 937 | | /// The angle is always the smaller angle between the two vectors on a plane. |
| | | 938 | | /// </remarks> |
| | | 939 | | public static Fixed64 Angle(Vector3d from, Vector3d to) |
| | 2 | 940 | | { |
| | 2 | 941 | | Fixed64 denominator = FixedMath.Sqrt(from.SqrMagnitude * to.SqrMagnitude); |
| | | 942 | | |
| | 2 | 943 | | if (denominator.Abs() < Fixed64.Epsilon) |
| | 1 | 944 | | return Fixed64.Zero; |
| | | 945 | | |
| | 1 | 946 | | Fixed64 dot = FixedMath.Clamp(Dot(from, to) / denominator, -Fixed64.One, Fixed64.One); |
| | | 947 | | |
| | 1 | 948 | | return FixedMath.RadToDeg(FixedMath.Acos(dot)); |
| | 2 | 949 | | } |
| | | 950 | | |
| | | 951 | | /// <summary> |
| | | 952 | | /// Returns a vector whose elements are the maximum of each of the pairs of elements in two specified vectors. |
| | | 953 | | /// </summary> |
| | | 954 | | /// <param name="value1">The first vector.</param> |
| | | 955 | | /// <param name="value2">The second vector.</param> |
| | | 956 | | /// <returns>The maximized vector.</returns> |
| | | 957 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 958 | | public static Vector3d Max(Vector3d value1, Vector3d value2) |
| | 3 | 959 | | { |
| | 3 | 960 | | return new Vector3d((value1.x > value2.x) ? value1.x : value2.x, (value1.y > value2.y) ? value1.y : value2.y, (v |
| | 3 | 961 | | } |
| | | 962 | | |
| | | 963 | | /// <summary> |
| | | 964 | | /// Returns a vector whose elements are the minimum of each of the pairs of elements in two specified vectors. |
| | | 965 | | /// </summary> |
| | | 966 | | /// <param name="value1">The first vector.</param> |
| | | 967 | | /// <param name="value2">The second vector.</param> |
| | | 968 | | /// <returns>The minimized vector.</returns> |
| | | 969 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 970 | | public static Vector3d Min(Vector3d value1, Vector3d value2) |
| | 3 | 971 | | { |
| | 3 | 972 | | return new Vector3d((value1.x < value2.x) ? value1.x : value2.x, (value1.y < value2.y) ? value1.y : value2.y, (v |
| | 3 | 973 | | } |
| | | 974 | | |
| | | 975 | | /// <summary> |
| | | 976 | | /// Rotates the vector around a given position using a specified quaternion rotation. |
| | | 977 | | /// </summary> |
| | | 978 | | /// <param name="source">The vector to rotate.</param> |
| | | 979 | | /// <param name="position">The position around which the vector is rotated.</param> |
| | | 980 | | /// <param name="rotation">The quaternion representing the rotation.</param> |
| | | 981 | | /// <returns>The rotated vector.</returns> |
| | | 982 | | public static Vector3d Rotate(Vector3d source, Vector3d position, FixedQuaternion rotation) |
| | 1 | 983 | | { |
| | 1 | 984 | | source -= position; // Translate the vector by the position |
| | 1 | 985 | | var normalizedRotation = rotation.Normal; |
| | 1 | 986 | | return (normalizedRotation * source) + position; |
| | 1 | 987 | | } |
| | | 988 | | |
| | | 989 | | /// <summary> |
| | | 990 | | /// Applies the inverse of a specified quaternion rotation to the vector around a given position. |
| | | 991 | | /// </summary> |
| | | 992 | | /// <param name="source">The vector to rotate.</param> |
| | | 993 | | /// <param name="position">The position around which the vector is rotated.</param> |
| | | 994 | | /// <param name="rotation">The quaternion representing the inverse rotation.</param> |
| | | 995 | | /// <returns>The rotated vector.</returns> |
| | | 996 | | public static Vector3d InverseRotate(Vector3d source, Vector3d position, FixedQuaternion rotation) |
| | 1 | 997 | | { |
| | 1 | 998 | | source -= position; // Translate the vector by the position |
| | 1 | 999 | | var normalizedRotation = rotation.Normal; |
| | | 1000 | | // Undo the rotation |
| | 1 | 1001 | | source = normalizedRotation.Inverse() * source; |
| | | 1002 | | // Add the original position back |
| | 1 | 1003 | | return source + position; |
| | 1 | 1004 | | } |
| | | 1005 | | |
| | | 1006 | | #endregion |
| | | 1007 | | |
| | | 1008 | | #region Operators |
| | | 1009 | | |
| | | 1010 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1011 | | public static Vector3d operator +(Vector3d v1, Vector3d v2) |
| | 96 | 1012 | | { |
| | 96 | 1013 | | return new Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); |
| | 96 | 1014 | | } |
| | | 1015 | | |
| | | 1016 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1017 | | public static Vector3d operator +(Vector3d v1, Fixed64 mag) |
| | 2 | 1018 | | { |
| | 2 | 1019 | | return new Vector3d(v1.x + mag, v1.y + mag, v1.z + mag); |
| | 2 | 1020 | | } |
| | | 1021 | | |
| | | 1022 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1023 | | public static Vector3d operator +(Fixed64 mag, Vector3d v1) |
| | 1 | 1024 | | { |
| | 1 | 1025 | | return v1 + mag; |
| | 1 | 1026 | | } |
| | | 1027 | | |
| | | 1028 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1029 | | public static Vector3d operator +(Vector3d v1, (int x, int y, int z) v2) |
| | 2 | 1030 | | { |
| | 2 | 1031 | | return new Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); |
| | 2 | 1032 | | } |
| | | 1033 | | |
| | | 1034 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1035 | | public static Vector3d operator +((int x, int y, int z) v2, Vector3d v1) |
| | 1 | 1036 | | { |
| | 1 | 1037 | | return v1 + v2; |
| | 1 | 1038 | | } |
| | | 1039 | | |
| | | 1040 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1041 | | public static Vector3d operator +(Vector3d v1, (float x, float y, float z) v2) |
| | 2 | 1042 | | { |
| | 2 | 1043 | | return new Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); |
| | 2 | 1044 | | } |
| | | 1045 | | |
| | | 1046 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1047 | | public static Vector3d operator +((float x, float y, float z) v1, Vector3d v2) |
| | 1 | 1048 | | { |
| | 1 | 1049 | | return v2 + v1; |
| | 1 | 1050 | | } |
| | | 1051 | | |
| | | 1052 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1053 | | public static Vector3d operator -(Vector3d v1, Vector3d v2) |
| | 108 | 1054 | | { |
| | 108 | 1055 | | return new Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); |
| | 108 | 1056 | | } |
| | | 1057 | | |
| | | 1058 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1059 | | public static Vector3d operator -(Vector3d v1, Fixed64 mag) |
| | 0 | 1060 | | { |
| | 0 | 1061 | | return new Vector3d(v1.x - mag, v1.y - mag, v1.z - mag); |
| | 0 | 1062 | | } |
| | | 1063 | | |
| | | 1064 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1065 | | public static Vector3d operator -(Fixed64 mag, Vector3d v1) |
| | 0 | 1066 | | { |
| | 0 | 1067 | | return new Vector3d(mag - v1.x, mag - v1.y, mag - v1.z); |
| | 0 | 1068 | | } |
| | | 1069 | | |
| | | 1070 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1071 | | public static Vector3d operator -(Vector3d v1, (int x, int y, int z) v2) |
| | 1 | 1072 | | { |
| | 1 | 1073 | | return new Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); |
| | 1 | 1074 | | } |
| | | 1075 | | |
| | | 1076 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1077 | | public static Vector3d operator -((int x, int y, int z) v1, Vector3d v2) |
| | 1 | 1078 | | { |
| | 1 | 1079 | | return new Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); |
| | 1 | 1080 | | } |
| | | 1081 | | |
| | | 1082 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1083 | | public static Vector3d operator -(Vector3d v1, (float x, float y, float z) v2) |
| | 1 | 1084 | | { |
| | 1 | 1085 | | return new Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); |
| | 1 | 1086 | | } |
| | | 1087 | | |
| | | 1088 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1089 | | public static Vector3d operator -((float x, float y, float z) v1, Vector3d v2) |
| | 1 | 1090 | | { |
| | 1 | 1091 | | return new Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); |
| | 1 | 1092 | | } |
| | | 1093 | | |
| | | 1094 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1095 | | public static Vector3d operator -(Vector3d v1) |
| | 5 | 1096 | | { |
| | 5 | 1097 | | return new Vector3d(v1.x * -Fixed64.One, v1.y * -Fixed64.One, v1.z * -Fixed64.One); |
| | 5 | 1098 | | } |
| | | 1099 | | |
| | | 1100 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1101 | | public static Vector3d operator *(Vector3d v1, Fixed64 mag) |
| | 95 | 1102 | | { |
| | 95 | 1103 | | return new Vector3d(v1.x * mag, v1.y * mag, v1.z * mag); |
| | 95 | 1104 | | } |
| | | 1105 | | |
| | | 1106 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1107 | | public static Vector3d operator *(Fixed64 mag, Vector3d v1) |
| | 13 | 1108 | | { |
| | 13 | 1109 | | return new Vector3d(v1.x * mag, v1.y * mag, v1.z * mag); |
| | 13 | 1110 | | } |
| | | 1111 | | |
| | | 1112 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1113 | | public static Vector3d operator *(Vector3d v1, int mag) |
| | 0 | 1114 | | { |
| | 0 | 1115 | | return new Vector3d(v1.x * mag, v1.y * mag, v1.z * mag); |
| | 0 | 1116 | | } |
| | | 1117 | | |
| | | 1118 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1119 | | public static Vector3d operator *(int mag, Vector3d v1) |
| | 1 | 1120 | | { |
| | 1 | 1121 | | return new Vector3d(v1.x * mag, v1.y * mag, v1.z * mag); |
| | 1 | 1122 | | } |
| | | 1123 | | |
| | | 1124 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1125 | | public static Vector3d operator *(Fixed3x3 matrix, Vector3d vector) |
| | 2 | 1126 | | { |
| | 2 | 1127 | | return new Vector3d( |
| | 2 | 1128 | | matrix.m00 * vector.x + matrix.m01 * vector.y + matrix.m02 * vector.z, |
| | 2 | 1129 | | matrix.m10 * vector.x + matrix.m11 * vector.y + matrix.m12 * vector.z, |
| | 2 | 1130 | | matrix.m20 * vector.x + matrix.m21 * vector.y + matrix.m22 * vector.z |
| | 2 | 1131 | | ); |
| | 2 | 1132 | | } |
| | | 1133 | | |
| | | 1134 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1135 | | public static Vector3d operator *(Vector3d vector, Fixed3x3 matrix) |
| | 1 | 1136 | | { |
| | 1 | 1137 | | return matrix * vector; |
| | 1 | 1138 | | } |
| | | 1139 | | |
| | | 1140 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1141 | | public static Vector3d operator *(Fixed4x4 matrix, Vector3d point) |
| | 6 | 1142 | | { |
| | 6 | 1143 | | if (matrix.IsAffine) |
| | 2 | 1144 | | { |
| | 2 | 1145 | | return new Vector3d( |
| | 2 | 1146 | | matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03 + matrix.m30, |
| | 2 | 1147 | | matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m13 + matrix.m31, |
| | 2 | 1148 | | matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m23 + matrix.m32 |
| | 2 | 1149 | | ); |
| | | 1150 | | } |
| | | 1151 | | |
| | | 1152 | | // Full 4×4 transformation |
| | 4 | 1153 | | Fixed64 w = matrix.m03 * point.x + matrix.m13 * point.y + matrix.m23 * point.z + matrix.m33; |
| | 6 | 1154 | | if (w == Fixed64.Zero) w = Fixed64.One; // Prevent divide-by-zero |
| | | 1155 | | |
| | 4 | 1156 | | return new Vector3d( |
| | 4 | 1157 | | (matrix.m00 * point.x + matrix.m01 * point.y + matrix.m02 * point.z + matrix.m03 + matrix.m30) / w, |
| | 4 | 1158 | | (matrix.m10 * point.x + matrix.m11 * point.y + matrix.m12 * point.z + matrix.m13 + matrix.m31) / w, |
| | 4 | 1159 | | (matrix.m20 * point.x + matrix.m21 * point.y + matrix.m22 * point.z + matrix.m23 + matrix.m32) / w |
| | 4 | 1160 | | ); |
| | 6 | 1161 | | } |
| | | 1162 | | |
| | | 1163 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1164 | | public static Vector3d operator *(Vector3d vector, Fixed4x4 matrix) |
| | 3 | 1165 | | { |
| | 3 | 1166 | | return matrix * vector; |
| | 3 | 1167 | | } |
| | | 1168 | | |
| | | 1169 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1170 | | public static Vector3d operator *(Vector3d v1, Vector3d v2) |
| | 1 | 1171 | | { |
| | 1 | 1172 | | return new Vector3d(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z); |
| | 1 | 1173 | | } |
| | | 1174 | | |
| | | 1175 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1176 | | public static Vector3d operator /(Vector3d v1, Fixed64 div) |
| | 12 | 1177 | | { |
| | 12 | 1178 | | return div == Fixed64.Zero ? Zero : new Vector3d(v1.x / div, v1.y / div, v1.z / div); |
| | 12 | 1179 | | } |
| | | 1180 | | |
| | | 1181 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1182 | | public static Vector3d operator /(Vector3d v1, Vector3d v2) |
| | 5 | 1183 | | { |
| | 5 | 1184 | | return new Vector3d( |
| | 5 | 1185 | | v2.x == Fixed64.Zero ? Fixed64.Zero : v1.x / v2.x, |
| | 5 | 1186 | | v2.y == Fixed64.Zero ? Fixed64.Zero : v1.y / v2.y, |
| | 5 | 1187 | | v2.z == Fixed64.Zero ? Fixed64.Zero : v1.z / v2.z); |
| | 5 | 1188 | | } |
| | | 1189 | | |
| | | 1190 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1191 | | public static Vector3d operator /(Vector3d v1, int div) |
| | 1 | 1192 | | { |
| | 1 | 1193 | | return div == 0 ? Zero : new Vector3d(v1.x / div, v1.y / div, v1.z / div); |
| | 1 | 1194 | | } |
| | | 1195 | | |
| | | 1196 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1197 | | public static Vector3d operator /(Fixed64 div, Vector3d v1) |
| | 1 | 1198 | | { |
| | 1 | 1199 | | return new Vector3d(div / v1.x, div / v1.y, div / v1.z); |
| | 1 | 1200 | | } |
| | | 1201 | | |
| | | 1202 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1203 | | public static Vector3d operator *(Vector3d point, FixedQuaternion rotation) |
| | 1 | 1204 | | { |
| | 1 | 1205 | | return rotation * point; |
| | 1 | 1206 | | } |
| | | 1207 | | |
| | | 1208 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1209 | | public static Vector3d operator *(FixedQuaternion rotation, Vector3d point) |
| | 4 | 1210 | | { |
| | 4 | 1211 | | Fixed64 num1 = rotation.x * 2; |
| | 4 | 1212 | | Fixed64 num2 = rotation.y * 2; |
| | 4 | 1213 | | Fixed64 num3 = rotation.z * 2; |
| | 4 | 1214 | | Fixed64 num4 = rotation.x * num1; |
| | 4 | 1215 | | Fixed64 num5 = rotation.y * num2; |
| | 4 | 1216 | | Fixed64 num6 = rotation.z * num3; |
| | 4 | 1217 | | Fixed64 num7 = rotation.x * num2; |
| | 4 | 1218 | | Fixed64 num8 = rotation.x * num3; |
| | 4 | 1219 | | Fixed64 num9 = rotation.y * num3; |
| | 4 | 1220 | | Fixed64 num10 = rotation.w * num1; |
| | 4 | 1221 | | Fixed64 num11 = rotation.w * num2; |
| | 4 | 1222 | | Fixed64 num12 = rotation.w * num3; |
| | 4 | 1223 | | Vector3d vector3 = new Vector3d( |
| | 4 | 1224 | | (Fixed64.One - (num5 + num6)) * point.x + (num7 - num12) * point.y + (num8 + num11) * point.z, |
| | 4 | 1225 | | (num7 + num12) * point.x + (Fixed64.One - (num4 + num6)) * point.y + (num9 - num10) * point.z, |
| | 4 | 1226 | | (num8 - num11) * point.x + (num9 + num10) * point.y + (Fixed64.One - (num4 + num5)) * point.z |
| | 4 | 1227 | | ); |
| | 4 | 1228 | | return vector3; |
| | 4 | 1229 | | } |
| | | 1230 | | |
| | | 1231 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1232 | | public static bool operator ==(Vector3d left, Vector3d right) |
| | 1 | 1233 | | { |
| | 1 | 1234 | | return left.Equals(right); |
| | 1 | 1235 | | } |
| | | 1236 | | |
| | | 1237 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1238 | | public static bool operator !=(Vector3d left, Vector3d right) |
| | 1 | 1239 | | { |
| | 1 | 1240 | | return !left.Equals(right); |
| | 1 | 1241 | | } |
| | | 1242 | | |
| | | 1243 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1244 | | public static bool operator >(Vector3d left, Vector3d right) |
| | 1 | 1245 | | { |
| | 1 | 1246 | | return left.x > right.x |
| | 1 | 1247 | | && left.y > right.y |
| | 1 | 1248 | | && left.z > right.z; |
| | 1 | 1249 | | } |
| | | 1250 | | |
| | | 1251 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1252 | | public static bool operator <(Vector3d left, Vector3d right) |
| | 1 | 1253 | | { |
| | 1 | 1254 | | return left.x < right.x |
| | 1 | 1255 | | && left.y < right.y |
| | 1 | 1256 | | && left.z < right.z; |
| | 1 | 1257 | | } |
| | | 1258 | | |
| | | 1259 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1260 | | public static bool operator >=(Vector3d left, Vector3d right) |
| | 1 | 1261 | | { |
| | 1 | 1262 | | return left.x >= right.x |
| | 1 | 1263 | | && left.y >= right.y |
| | 1 | 1264 | | && left.z >= right.z; |
| | 1 | 1265 | | } |
| | | 1266 | | |
| | | 1267 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1268 | | public static bool operator <=(Vector3d left, Vector3d right) |
| | 1 | 1269 | | { |
| | 1 | 1270 | | return left.x <= right.x |
| | 1 | 1271 | | && left.y <= right.y |
| | 1 | 1272 | | && left.z <= right.z; |
| | 1 | 1273 | | } |
| | | 1274 | | |
| | | 1275 | | #endregion |
| | | 1276 | | |
| | | 1277 | | #region Conversion |
| | | 1278 | | |
| | | 1279 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1280 | | public override string ToString() |
| | 107 | 1281 | | { |
| | 107 | 1282 | | return string.Format("({0}, {1}, {2})", x.ToFormattedDouble(), y.ToFormattedDouble(), z.ToFormattedDouble()); |
| | 107 | 1283 | | } |
| | | 1284 | | |
| | | 1285 | | /// <summary> |
| | | 1286 | | /// Converts this <see cref="Vector3d"/> to a <see cref="Vector2d"/>, |
| | | 1287 | | /// dropping the Y component (height) of this vector in the resulting vector. |
| | | 1288 | | /// </summary> |
| | | 1289 | | /// <returns> |
| | | 1290 | | /// A new <see cref="Vector2d"/> where (X, Z) from this <see cref="Vector3d"/> |
| | | 1291 | | /// become (X, Y) in the resulting vector. |
| | | 1292 | | /// </returns> |
| | | 1293 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1294 | | public Vector2d ToVector2d() |
| | 1 | 1295 | | { |
| | 1 | 1296 | | return new Vector2d(x, z); |
| | 1 | 1297 | | } |
| | | 1298 | | |
| | | 1299 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1300 | | public readonly void Deconstruct(out float x, out float y, out float z) |
| | 1 | 1301 | | { |
| | 1 | 1302 | | x = this.x.ToPreciseFloat(); |
| | 1 | 1303 | | y = this.y.ToPreciseFloat(); |
| | 1 | 1304 | | z = this.z.ToPreciseFloat(); |
| | 1 | 1305 | | } |
| | | 1306 | | |
| | | 1307 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1308 | | public readonly void Deconstruct(out int x, out int y, out int z) |
| | 1 | 1309 | | { |
| | 1 | 1310 | | x = this.x.RoundToInt(); |
| | 1 | 1311 | | y = this.y.RoundToInt(); |
| | 1 | 1312 | | z = this.z.RoundToInt(); |
| | 1 | 1313 | | } |
| | | 1314 | | |
| | | 1315 | | /// <summary> |
| | | 1316 | | /// Converts each component of the vector from radians to degrees. |
| | | 1317 | | /// </summary> |
| | | 1318 | | /// <param name="radians">The vector with components in radians.</param> |
| | | 1319 | | /// <returns>A new vector with components converted to degrees.</returns> |
| | | 1320 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1321 | | public static Vector3d ToDegrees(Vector3d radians) |
| | 1 | 1322 | | { |
| | 1 | 1323 | | return new Vector3d( |
| | 1 | 1324 | | FixedMath.RadToDeg(radians.x), |
| | 1 | 1325 | | FixedMath.RadToDeg(radians.y), |
| | 1 | 1326 | | FixedMath.RadToDeg(radians.z)); |
| | 1 | 1327 | | } |
| | | 1328 | | |
| | | 1329 | | /// <summary> |
| | | 1330 | | /// Converts each component of the vector from degrees to radians. |
| | | 1331 | | /// </summary> |
| | | 1332 | | /// <param name="degrees">The vector with components in degrees.</param> |
| | | 1333 | | /// <returns>A new vector with components converted to radians.</returns> |
| | | 1334 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1335 | | public static Vector3d ToRadians(Vector3d degrees) |
| | 1 | 1336 | | { |
| | 1 | 1337 | | return new Vector3d( |
| | 1 | 1338 | | FixedMath.DegToRad(degrees.x), |
| | 1 | 1339 | | FixedMath.DegToRad(degrees.y), |
| | 1 | 1340 | | FixedMath.DegToRad(degrees.z)); |
| | 1 | 1341 | | } |
| | | 1342 | | |
| | | 1343 | | #endregion |
| | | 1344 | | |
| | | 1345 | | #region Equality and HashCode Overrides |
| | | 1346 | | |
| | | 1347 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1348 | | public override bool Equals(object? obj) |
| | 5 | 1349 | | { |
| | 5 | 1350 | | return obj is Vector3d other && Equals(other); |
| | 5 | 1351 | | } |
| | | 1352 | | |
| | | 1353 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1354 | | public bool Equals(Vector3d other) |
| | 208 | 1355 | | { |
| | 208 | 1356 | | return other.x == x && other.y == y && other.z == z; |
| | 208 | 1357 | | } |
| | | 1358 | | |
| | | 1359 | | public bool Equals(Vector3d x, Vector3d y) |
| | 2 | 1360 | | { |
| | 2 | 1361 | | return x.Equals(y); |
| | 2 | 1362 | | } |
| | | 1363 | | |
| | | 1364 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1365 | | public override int GetHashCode() |
| | 12 | 1366 | | { |
| | 12 | 1367 | | return StateHash; |
| | 12 | 1368 | | } |
| | | 1369 | | |
| | | 1370 | | public int GetHashCode(Vector3d obj) |
| | 1 | 1371 | | { |
| | 1 | 1372 | | return obj.GetHashCode(); |
| | 1 | 1373 | | } |
| | | 1374 | | |
| | | 1375 | | public int CompareTo(Vector3d other) |
| | 1 | 1376 | | { |
| | 1 | 1377 | | return SqrMagnitude.CompareTo(other.SqrMagnitude); |
| | 1 | 1378 | | } |
| | | 1379 | | |
| | | 1380 | | #endregion |
| | | 1381 | | } |