From 9896af3c3a50f0991bb90e9ffe79b9f46d82c59a Mon Sep 17 00:00:00 2001 From: HurricanKai Date: Sat, 13 Jun 2020 23:05:15 +0200 Subject: [PATCH] Vector3 (#2) * Add Vector3 See dotnet/runtime#24168 * Make Vector3 a struct * Fix IFormattable nullability * Add XML Docs to Vector3 * Apply suggestions from code review Co-authored-by: Tanner Gooding --- .../System.Private.CoreLib.Shared.projitems | 17 +- .../src/System/Numerics/Vector3`1.cs | 966 ++++++++++++++++++ 2 files changed, 972 insertions(+), 11 deletions(-) create mode 100644 src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3`1.cs diff --git a/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems b/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems index 5d3e8a89d5f96..20473f9c35902 100644 --- a/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems +++ b/src/libraries/System.Private.CoreLib/src/System.Private.CoreLib.Shared.projitems @@ -16,16 +16,10 @@ - - - - - - + + + + @@ -447,6 +441,7 @@ + @@ -1900,4 +1895,4 @@ - + \ No newline at end of file diff --git a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3`1.cs b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3`1.cs new file mode 100644 index 0000000000000..8202eb145640b --- /dev/null +++ b/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3`1.cs @@ -0,0 +1,966 @@ +// Licensed to the .NET Foundation under one or more agreements. +// The .NET Foundation licenses this file to you under the MIT license. +// See the LICENSE file in the project root for more information. + +using System; +using System.Globalization; +using System.Runtime.CompilerServices; +using System.Security.Cryptography; + +namespace System.Numerics +{ + public struct Vector3 : IEquatable>, IFormattable + where T : struct + { + /// + /// The X component of the vector. + /// + public T X { get; } + + /// + /// The Y component of the vector. + /// + public T Y { get; } + + /// + /// The Z component of the vector. + /// + public T Z { get; } + + /// + /// Constructs a vector whose elements are all the single specified value + /// + /// The element to fill the vector with. + public Vector3(T value) : this(value, value, value) + { } + + /// + /// Constructs a vector with the given individual elements. + /// + /// The X component. + /// The Y component. + /// The Z component. + public Vector3(T x, T y, T z) + { + X = x; + Y = y; + Z = z; + } + + /// + /// Constructs a Vector3 from the given Vector2 and a third value. + /// + /// The Vector to extract X and Y components from. + /// The Z component. + public Vector3(Vector2 value, T z) : this(value.X, value.Y, z) + { } + + /// + /// Constructs a Vector3 from the given array. + /// + /// The Array to extract the X, Y and Z components from. + public Vector3(T[] value) : this(value, 0) + { } + + /// + /// Constructs a Vector3 from the given array and an offset. + /// + /// The Array to extract the X, Y and Z components from. + /// The Offset to begin extracting the components from. + public Vector3(T[] value, int offset) : this (new ReadOnlySpan(value, offset, 3)) + { } + + /// + /// Constructs a Vector3 from the given ReadOnlySpan. + /// + /// The ReadOnlySpan to extract the X, Y and Z components from. + public Vector3(ReadOnlySpan value) : this(value[0], value[1], value[2]) + { } + + /// + /// Returns the vector (1,1,1) + /// + public static Vector3 One + { + get + { + ThrowForUnsupportedVectorBaseType(); + + T one; + if (typeof(T) == typeof(float)) + { + one = (T)(object)1.0f; + } + else if (typeof(T) == typeof(double)) + { + one = (T)(object)1.0; + } + return new Vector3(one, one, one); + } + } + + /// + /// Returns the vector (1, 0, 0) + /// + public static Vector3 UnitX + { + get + { + ThrowForUnsupportedVectorBaseType(); + + T one; + if (typeof(T) == typeof(float)) + { + one = (T)(object)1.0f; + } + else if (typeof(T) == typeof(double)) + { + one = (T)(object)1.0; + } + return new Vector3(one, default, default); + } + } + + /// + /// Returns the vector (0, 1, 0) + /// + public static Vector3 UnitY + { + get + { + ThrowForUnsupportedVectorBaseType(); + + T one; + if (typeof(T) == typeof(float)) + { + one = (T)(object)1.0f; + } + else if (typeof(T) == typeof(double)) + { + one = (T)(object)1.0; + } + return new Vector3(default, one, default); + } + } + + /// + /// Returns the vector (0, 0, 1) + /// + public static Vector3 UnitZ + { + get + { + ThrowForUnsupportedVectorBaseType(); + + T one; + if (typeof(T) == typeof(float)) + { + one = (T)(object)1.0f; + } + else if (typeof(T) == typeof(double)) + { + one = (T)(object)1.0; + } + return new Vector3(default, default, one); + } + } + + /// + /// Returns the vector (0, 0, 0) + /// + public static Vector3 Zero => default; + + /// + /// Returns a boolean indicating whether the two given vectors are equal. + /// + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are equal; False otherwise + public static bool operator ==(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return (float)(object)left.X == (float)(object)right.X + && (float)(object)left.Y == (float)(object)right.Y + && (float)(object)left.Z == (float)(object)right.Z; + } + + if (typeof(T) == typeof(double)) + { + return (double)(object)left.X == (double)(object)right.X + && (double)(object)left.Y == (double)(object)right.Y + && (double)(object)left.Z == (double)(object)right.Z; + } + + return default; + } + + /// + /// Returns a boolean indicating whether the two given vectors are not equal. + /// + /// The first vector to compare. + /// The second vector to compare. + /// True if the vectors are not equal; False if they are equal + public static bool operator !=(Vector3 left, Vector3 right) => !(left == right); + + public static Vector3 operator +(Vector3 value) => Plus(value); + + /// + /// Negates a given vector. + /// + /// The source vector. + /// The negated vector. + public static Vector3 operator -(Vector3 value) => Negate(value); + + /// + /// Adds two vectors together. + /// + /// The first source vector. + /// The second source vector. + /// The summed vector. + public static Vector3 operator +(Vector3 left, Vector3 right) => Add(right, left); + + /// + /// Subtracts the second vector from the first. + /// + /// The first source vector. + /// The second source vector. + /// The difference vector. + public static Vector3 operator -(Vector3 left, Vector3 right) => Subtract(left, right); + + /// + /// Multiplies two vectors together. + /// + /// The first source vector. + /// The second source vector. + /// The product vector. + public static Vector3 operator *(Vector3 left, Vector3 right) => Multiply(left, right); + + /// + /// Divides the first vector by the second. + /// + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division + public static Vector3 operator /(Vector3 left, Vector3 right) => Divide(left, right); + + /// + /// Multiplies a vector by the given scalar. + /// + /// The source vector. + /// The scalar value. + /// The scaled vector. + public static Vector3 operator *(Vector3 left, T right) => Multiply(left, right); + + /// + /// Divides the vector by the given scalar. + /// + /// The source vector. + /// The scalar value. + /// The result of the division. + public static Vector3 operator /(Vector3 left, T right) => Divide(left, right); + + /// + /// Multiplies a vector by the given scalar. + /// + /// The scalar value. + /// The source vector. + /// The scaled vector. + public static Vector3 operator *(T left, Vector3 right) => Multiply(left, right); + + public static Vector3 Plus(Vector3 value) => value; + + /// + /// Negates a given vector. + /// + /// The source vector. + /// The negated vector. + public static Vector3 Negate(Vector3 value) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3((T)(object)(-(float)(object)value.X), (T)(object)(-(float)(object)value.Y), (T)(object)(-(float)(object)value.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3((T)(object)(-(double)(object)explicitValue.X), (T)(object)(-(double)(object)explicitValue.Y), (T)(object)(-(double)(object)explicitValue.Z)); + } + + return default; + } + + /// + /// Adds two vectors together. + /// + /// The first source vector. + /// The second source vector. + /// The summed vector. + public static Vector3 Add(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)left.X + (float)(object)right.X), + (T)(object)((float)(object)left.Y + (float)(object)right.Y), + (T)(object)((float)(object)left.Z + (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)left.X + (double)(object)right.X), + (T)(object)((double)(object)left.Y + (double)(object)right.Y), + (T)(object)((double)(object)left.Z + (double)(object)right.Z)); + } + + return default; + } + + /// + /// Subtracts the second vector from the first. + /// + /// The first source vector. + /// The second source vector. + /// The difference vector. + public static Vector3 Subtract(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)left.X - (float)(object)right.X), + (T)(object)((float)(object)left.Y - (float)(object)right.Y), + (T)(object)((float)(object)left.Z - (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)left.X - (double)(object)right.X), + (T)(object)((double)(object)left.Y - (double)(object)right.Y), + (T)(object)((double)(object)left.Z - (double)(object)right.Z)); + } + + return default; + } + + /// + /// Multiplies two vectors together. + /// + /// The first source vector. + /// The second source vector. + /// The product vector. + public static Vector3 Multiply(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)left.X * (float)(object)right.X), + (T)(object)((float)(object)left.Y * (float)(object)right.Y), + (T)(object)((float)(object)left.Z * (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)left.X * (double)(object)right.X), + (T)(object)((double)(object)left.Y * (double)(object)right.Y), + (T)(object)((double)(object)left.Z * (double)(object)right.Z)); + } + + return default; + } + + /// + /// Divides the first vector by the second. + /// + /// The first source vector. + /// The second source vector. + /// The vector resulting from the division + public static Vector3 Divide(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)left.X / (float)(object)right.X), + (T)(object)((float)(object)left.Y / (float)(object)right.Y), + (T)(object)((float)(object)left.Z / (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)left.X / (double)(object)right.X), + (T)(object)((double)(object)left.Y / (double)(object)right.Y), + (T)(object)((double)(object)left.Z / (double)(object)right.Z)); + } + + return default; + } + + /// + /// Multiplies a vector by the given scalar. + /// + /// The source vector. + /// The scalar value. + /// The scaled vector. + public static Vector3 Multiply(Vector3 left, T right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + var r = (float)(object)right; + return new Vector3( + (T)(object)((float)(object)left.X * r), + (T)(object)((float)(object)left.Y * r), + (T)(object)((float)(object)left.Z * r)); + } + + if (typeof(T) == typeof(double)) + { + var r = (double)(object)right; + return new Vector3( + (T)(object)((double)(object)left.X * r), + (T)(object)((double)(object)left.Y * r), + (T)(object)((double)(object)left.Z * r)); + } + + return default; + } + + /// + /// Divides the vector by the given scalar. + /// + /// The source vector. + /// The scalar value. + /// The result of the division. + public static Vector3 Divide(Vector3 left, T right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + var r = (float)(object)right; + return new Vector3( + (T)(object)((float)(object)left.X / r), + (T)(object)((float)(object)left.Y / r), + (T)(object)((float)(object)left.Z / r)); + } + + if (typeof(T) == typeof(double)) + { + var r = (double)(object)right; + return new Vector3( + (T)(object)((double)(object)left.X / r), + (T)(object)((double)(object)left.Y / r), + (T)(object)((double)(object)left.Z / r)); + } + + return default; + } + + /// + /// Multiplies a vector by the given scalar. + /// + /// The scalar value. + /// The source vector. + /// The scaled vector. + public static Vector3 Multiply(T left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + var l = (float)(object)left; + return new Vector3( + (T)(object)(l * (float)(object)left.X), + (T)(object)(l * (float)(object)left.Y), + (T)(object)(l * (float)(object)left.Z)); + } + + if (typeof(T) == typeof(double)) + { + var l = (double)(object)left; + return new Vector3( + (T)(object)(l * (double)(object)left.X), + (T)(object)(l * (double)(object)left.Y), + (T)(object)(l * (double)(object)left.Z)); + } + + return default; + } + + /// + /// Gets the absolute of a vector. + /// + /// The source vector. + /// The absolute of the vector. + public static Vector3 Abs(Vector3 value) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)(MathF.Abs((float)(object)left.X)), + (T)(object)(MathF.Abs((float)(object)left.Y)), + (T)(object)(MathF.Abs((float)(object)left.Z))); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)(Math.Abs((double)(object)left.X)), + (T)(object)(Math.Abs((double)(object)left.Y)), + (T)(object)(Math.Abs((double)(object)left.Z))); + } + + return default; + } + + /// + /// Restricts a vector between a min and max value. + /// + /// The source vector. + /// The minimum value. + /// The maximum value. + /// The restricted vector. + public static Vector3 Clamp(Vector3 value, Vector3 min, Vector3 max) + { + // We must follow HLSL behavior in the case user specified min value is bigger than max value. + return Vector3.Min(Vector3.Max(value, min), max); + } + + /// + /// Returns the Euclidean distance between the two given points. + /// + /// The first point. + /// The second point. + /// The distance. + public static T Distance(Vector3 left, Vector3 right) => (left - right).Length(); + + /// + /// Returns the Euclidean distance squared between the two given points. + /// + /// The first point. + /// The second point. + /// The distance squared. + public static T DistanceSquared(Vector3 left, Vector3 right) => (left - right).LengthSquared(); + + /// + /// Computes the cross product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The cross product. + public static Vector3 Cross(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + var a1 = (float)(object)left.X; + var a2 = (float)(object)left.Y; + var a3 = (float)(object)left.Z; + var b1 = (float)(object)right.X; + var b2 = (float)(object)right.Y; + var b3 = (float)(object)right.Z; + return new Vector3( + (T)(object)(a2 * a3 - a3 * b2), + (T)(object)(a3 * b1 - a1 * b3), + (T)(object)(a1 * b2 - a2 * b1) + ); + } + + if (typeof(T) == typeof(double)) + { + var a1 = (double)(object)left.X; + var a2 = (double)(object)left.Y; + var a3 = (double)(object)left.Z; + var b1 = (double)(object)right.X; + var b2 = (double)(object)right.Y; + var b3 = (double)(object)right.Z; + return new Vector3( + (T)(object)(a2 * a3 - a3 * b2), + (T)(object)(a3 * b1 - a1 * b3), + (T)(object)(a1 * b2 - a2 * b1) + ); + } + + return default; + } + + /// + /// Returns the dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product. + public static T Dot(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3((T)(object)( + (float)(object)left.X * (float)(object)right.X + + (float)(object)left.Y * (float)(object)right.Y + + (float)(object)left.Z * (float)(object)right.Z + )); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3((T)(object)( + (double)(object)left.X * (double)(object)right.X + + (double)(object)left.Y * (double)(object)right.Y + + (double)(object)left.Z * (double)(object)right.Z + )); + } + + return default; + } + + /// + /// Linearly interpolates between two vectors based on the given weighting. + /// + /// The first source vector. + /// The second source vector. + /// Value between 0 and 1 indicating the weight of the second source vector. + /// The interpolated vector. + public static Vector3 Lerp(Vector3 min, Vector max, T amount) => (min * (One - amount)) + (max * amount); + + /// + /// Returns a vector whose elements are the minimum of each of the pairs of elements in the two source vectors. + /// + /// The first source vector. + /// The second source vector. + /// The minimized vector. + public static Vector3 Min(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)MathF.Min((float)(object)left.X, (float)(object)right.X), + (T)(object)MathF.Min((float)(object)left.Y, (float)(object)right.Y), + (T)(object)MathF.Min((float)(object)left.Z, (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)Math.Min((double)(object)left.X, (double)(object)right.X), + (T)(object)Math.Min((double)(object)left.Y, (double)(object)right.Y), + (T)(object)Math.Min((double)(object)left.Z, (double)(object)right.Z)); + } + + return default; + } + + /// + /// Returns a vector whose elements are the maximum of each of the pairs of elements in the two source vectors. + /// + /// The first source vector. + /// The second source vector. + /// The maximized vector. + public static Vector3 Max(Vector3 left, Vector3 right) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)MathF.Max((float)(object)left.X, (float)(object)right.X), + (T)(object)MathF.Max((float)(object)left.Y, (float)(object)right.Y), + (T)(object)MathF.Max((float)(object)left.Z, (float)(object)right.Z)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)Math.Max((double)(object)left.X, (double)(object)right.X), + (T)(object)Math.Max((double)(object)left.Y, (double)(object)right.Y), + (T)(object)Math.Max((double)(object)left.Z, (double)(object)right.Z)); + } + + return default; + } + + /// + /// Returns a vector with the same direction as the given vector, but with a length of 1. + /// + /// The vector to normalize. + /// The normalized vector. + public static Vector3 Normalize(Vector3 value) => value / value.Length(); + + /// + /// Returns the reflection of a vector off a surface that has the specified normal. + /// + /// The source vector. + /// The normal of the surface being reflected off. + /// The reflected vector. + public static Vector3 Reflect(Vector3 incident, Vector3 normal) + => incident - (Dot(incident, normal) * 2) * normal; + + /// + /// Returns a vector whose elements are the square root of each of the source vector's elements. + /// + /// The source vector. + /// The square root vector. + public static Vector3 SquareRoot(Vector3 value) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)(MathF.Sqrt((float)(object)left.X)), + (T)(object)(MathF.Sqrt((float)(object)left.Y)), + (T)(object)(MathF.Sqrt((float)(object)left.Z))); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)(Math.Sqrt((double)(object)left.X)), + (T)(object)(Math.Sqrt((double)(object)left.Y)), + (T)(object)(Math.Sqrt((double)(object)left.Z))); + } + + return default; + } + + /// + /// Transforms a vector by the given matrix. + /// + /// The source vector. + /// The transformation matrix. + /// The transformed vector. + public static Vector3 Transform(Vector3 position, Matrix4x4 matrix) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)position.X * (float)(object)matrix.M11 + (float)(object)position.Y * (float)(object)matrix.M21 + (float)(object)position.Z * (float)(object)matrix.M31 + (float)(object)matrix.M41), + (T)(object)((float)(object)position.X * (float)(object)matrix.M12 + (float)(object)position.Y * (float)(object)matrix.M22 + (float)(object)position.Z * (float)(object)matrix.M32 + (float)(object)matrix.M42), + (T)(object)((float)(object)position.X * (float)(object)matrix.M13 + (float)(object)position.Y * (float)(object)matrix.M23 + (float)(object)position.Z * (float)(object)matrix.M33 + (float)(object)matrix.M43)); + } + + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)position.X * (double)(object)matrix.M11 + (double)(object)position.Y * (double)(object)matrix.M21 + (double)(object)position.Z * (double)(object)matrix.M31 + (double)(object)matrix.M41), + (T)(object)((double)(object)position.X * (double)(object)matrix.M12 + (double)(object)position.Y * (double)(object)matrix.M22 + (double)(object)position.Z * (double)(object)matrix.M32 + (double)(object)matrix.M42), + (T)(object)((double)(object)position.X * (double)(object)matrix.M13 + (double)(object)position.Y * (double)(object)matrix.M23 + (double)(object)position.Z * (double)(object)matrix.M33 + (double)(object)matrix.M43)); + } + + return default; + } + + /// + /// Transforms a vector by the given Quaternion rotation value. + /// + /// The source vector to be rotated. + /// The rotation to apply. + /// The transformed vector. + public static Vector3 Transform(Vector3 position, Quaternion rotation) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + var x2 = (float)(object)rotation.X + (float)(object)rotation.X; + var y2 = (float)(object)rotation.Y + (float)(object)rotation.Y; + var z2 = (float)(object)rotation.Z + (float)(object)rotation.Z; + + var wx2 = (float)(object)rotation.W * x2; + var wy2 = (float)(object)rotation.W * y2; + var wz2 = (float)(object)rotation.W * z2; + var xx2 = (float)(object)rotation.X * x2; + var xy2 = (float)(object)rotation.X * y2; + var xz2 = (float)(object)rotation.X * z2; + var yy2 = (float)(object)rotation.Y * y2; + var yz2 = (float)(object)rotation.Y * z2; + var zz2 = (float)(object)rotation.Z * z2; + + return new Vector3( + (T)(object)((float)(object)position.X * (1.0f - yy2 - zz2) + (float)(object)position.Y * (xy2 - wz2) + (float)(object)position.Z * (xz2 + wy2)), + (T)(object)((float)(object)position.X * (xy2 + wz2) + (float)(object)position.Y * (1.0f - xx2 - zz2) + (float)(object)position.Z * (yz2 - wx2)), + (T)(object)((float)(object)position.X * (xz2 - wy2) + (float)(object)position.Y * (yz2 + wx2) + (float)(object)position.Z * (1.0f - xx2 - yy2))); + } + + if (typeof(T) == typeof(double)) + { + double x2 = (double)(object)rotation.X + (double)(object)rotation.X; + double y2 = (double)(object)rotation.Y + (double)(object)rotation.Y; + double z2 = (double)(object)rotation.Z + (double)(object)rotation.Z; + + double wx2 = (double)(object)rotation.W * x2; + double wy2 = (double)(object)rotation.W * y2; + double wz2 = (double)(object)rotation.W * z2; + double xx2 = (double)(object)rotation.X * x2; + double xy2 = (double)(object)rotation.X * y2; + double xz2 = (double)(object)rotation.X * z2; + double yy2 = (double)(object)rotation.Y * y2; + double yz2 = (double)(object)rotation.Y * z2; + double zz2 = (double)(object)rotation.Z * z2; + + return new Vector3( + (T)(object)((double)(object)position.X * (1.0f - yy2 - zz2) + (double)(object)position.Y * (xy2 - wz2) + (double)(object)position.Z * (xz2 + wy2)), + (T)(object)((double)(object)position.X * (xy2 + wz2) + (double)(object)position.Y * (1.0f - xx2 - zz2) + (double)(object)position.Z * (yz2 - wx2)), + (T)(object)((double)(object)position.X * (xz2 - wy2) + (double)(object)position.Y * (yz2 + wx2) + (double)(object)position.Z * (1.0f - xx2 - yy2))); + } + + return default; + } + + /// + /// Transforms a vector normal by the given matrix. + /// + /// The source vector. + /// The transformation matrix. + /// The transformed vector. + public static Vector3 TransformNormal(Vector3 normal, Matrix4x4 matrix) + { + ThrowForUnsupportedVectorBaseType(); + + if (typeof(T) == typeof(float)) + { + return new Vector3( + (T)(object)((float)(object)normal.X * (float)(object)matrix.M11 + (float)(object)normal.Y * (float)(object)matrix.M21 + (float)(object)normal.Z * (float)(object)matrix.M31), + (T)(object)((float)(object)normal.X * (float)(object)matrix.M12 + (float)(object)normal.Y * (float)(object)matrix.M22 + (float)(object)normal.Z * (float)(object)matrix.M32), + (T)(object)((float)(object)normal.X * (float)(object)matrix.M13 + (float)(object)normal.Y * (float)(object)matrix.M23 + (float)(object)normal.Z * (float)(object)matrix.M33)); + } + + if (typeof(T) == typeof(double)) + { + return new Vector3( + (T)(object)((double)(object)normal.X * (double)(object)matrix.M11 + (double)(object)normal.Y * (double)(object)matrix.M21 + (double)(object)normal.Z * (double)(object)matrix.M31), + (T)(object)((double)(object)normal.X * (double)(object)matrix.M12 + (double)(object)normal.Y * (double)(object)matrix.M22 + (double)(object)normal.Z * (double)(object)matrix.M32), + (T)(object)((double)(object)normal.X * (double)(object)matrix.M13 + (double)(object)normal.Y * (double)(object)matrix.M23 + (double)(object)normal.Z * (double)(object)matrix.M33)); + } + + return default; + } + + /// + /// Copies the contents of the vector into the given array. + /// + public readonly void CopyTo(T[] array) => CopyTo(new Span(array)); + + /// + /// Copies the contents of the vector into the given array, starting from index. + /// + public readonly void CopyTo(T[] array, int index) => CopyTo(new Span(array).Slice(index)); + + /// + /// Copies the contents of the vector into the given span. + /// + /// + public readonly void CopyTo(Span destination) + { + if (destination.Length < 2) + { + ThrowHelper.ThrowArgumentOutOfRangeException(ExceptionArgument.index, ExceptionResource.ArgumentOutOfRange_SmallCapacity); + } + + destination[0] = X; + destination[1] = Y; + destination[2] = Z; + } + + /// + /// Returns a boolean indicating whether the given Object is equal to this Vector3 instance. + /// + /// The Object to compare against. + /// True if the Object is equal to this Vector3; False otherwise. + public override readonly bool Equals(object? obj) => obj is Vector3 other && Equals(other); + + /// + /// Returns a boolean indicating whether the given Vector3 is equal to this Vector3 instance. + /// + /// The Vector3 to compare this instance to. + /// True if the other Vector3 is equal to this instance; False otherwise. + public readonly bool Equals(Vector3 other) => this == other; + + /// + /// Returns the hash code for this instance. + /// + /// The hash code. + public override readonly int GetHashCode() => HashCode.Combine(X.GetHashCode(), Y.GetHashCode(), Z.GetHashCode()); + + /// + /// Returns the length of the vector. + /// + /// The vector's length. + public readonly T Length() => SquareRoot(LengthSquared()); + + /// + /// Returns the length of the vector squared. This operation is cheaper than Length(). + /// + /// The vector's length squared. + public readonly T LengthSquared() => Dot(this, this); + + /// + /// Returns a String representing this Vector3 instance. + /// + /// The string representation. + public readonly override string ToString() => ToString("G"); + + /// + /// Returns a String representing this Vector3 instance, using the specified format to format individual elements. + /// + /// The format of individual elements. + /// The string representation. + public readonly string ToString(string? format) => ToString(format, CultureInfo.CurrentCulture); + + /// + /// Returns a String representing this Vector3 instance, using the specified format to format individual elements + /// and the given IFormatProvider. + /// + /// The format of individual elements. + /// The format provider to use when formatting elements. + /// The string representation. + public readonly string ToString(string? format, IFormatProvider? formatProvider) + { + var sb = new StringBuilder(); + string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator; + sb.Append('<'); + sb.Append(X.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Y.ToString(format, formatProvider)); + sb.Append(separator); + sb.Append(' '); + sb.Append(Z.ToString(format, formatProvider)); + sb.Append('>'); + return sb.ToString(); + } + + internal static void ThrowForUnsupportedVectorBaseType() + { + if (typeof(T) != typeof(float) && typeof(T) != typeof(double)) + { + ThrowHelper.ThrowNotSupportedException(ExceptionResource.Arg_TypeNotSupported); + } + } + } +}