diff --git a/CHANGELOG.md b/CHANGELOG.md index 4d659e0..f3668c7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ - X10D: Added `Size.ToPoint()` - X10D: Added `Size.ToPointF()` - X10D: Added `Size.ToVector2()` +- X10D: Added `Quaternion.Multiply(Vector3)` - this functions as an equivalent to Unity's `Quaternion * Vector3` operator - X10D: Added `Vector2.Deconstruct()` - X10D: Added `Vector2.ToPointF()` - X10D: Added `Vector2.ToSizeF()` diff --git a/X10D/src/Numerics/QuaternionExtensions.cs b/X10D/src/Numerics/QuaternionExtensions.cs new file mode 100644 index 0000000..a8e8b5b --- /dev/null +++ b/X10D/src/Numerics/QuaternionExtensions.cs @@ -0,0 +1,41 @@ +using System.Numerics; + +namespace X10D.Numerics; + +/// +/// Numeric-related extension methods for . +/// +public static class QuaternionExtensions +{ + /// + /// Rotates the specified point with the specified rotation. + /// + /// The rotation. + /// The point. + /// The rotated point. + public static Vector3 Multiply(this in Quaternion rotation, in Vector3 point) + { + // the internet wrote it, I just handed it in. + // https://github.com/Unity-Technologies/UnityCsReference/blob/master/Runtime/Export/Math/Quaternion.cs + float x = rotation.X * 2.0f; + float y = rotation.Y * 2.0f; + float z = rotation.Z * 2.0f; + float xx = rotation.X * x; + float yy = rotation.Y * y; + float zz = rotation.Z * z; + float xy = rotation.X * y; + float xz = rotation.X * z; + float yz = rotation.Y * z; + float wx = rotation.W * x; + float wy = rotation.W * y; + float wz = rotation.W * z; + + (float px, float py, float pz) = point; + + return new Vector3( + (1.0f - (yy + zz)) * px + (xy - wz) * py + (xz + wy) * pz, + (xy + wz) * px + (1.0f - (xx + zz)) * py + (yz - wx) * pz, + (xz - wy) * px + (yz + wx) * py + (1.0f - (xx + yy)) * pz + ); + } +}