ZvE

PveTeam.Math/QuaternionD.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Numerics;
 
 namespace PveTeam.Mathematics
 {
@@ -6,7 +7,531 @@ namespace PveTeam.Mathematics
     {
         public Double X, Y, Z, W;
 
+        #region Static fields
+        public static QuaternionD Identity => new QuaternionD(0, 0, 0, 1);
+        public static QuaternionD Zero => new QuaternionD(0, 0, 0, 0);
+        #endregion
+
+        #region Constructors
+        public QuaternionD(double value) : this(value, value, value, value) { }
+
+        public QuaternionD(Quaternion value) : this(value.X, value.Y, value.Z, value.W) { }
+
+        public QuaternionD(Double x, Double y, Double z, Double w)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+            W = w;
+        }
+
+        public QuaternionD(Vector3 vectorPart, float scalarPart)
+        {
+            X = vectorPart.X;
+            Y = vectorPart.Y;
+            Z = vectorPart.Z;
+            W = scalarPart;
+        }
+
+        public QuaternionD(Vector3D vectorPart, double scalarPart)
+        {
+            X = vectorPart.X;
+            Y = vectorPart.Y;
+            Z = vectorPart.Z;
+            W = scalarPart;
+        }
+        #endregion
+
+        #region Instance methods
+        public readonly double Length()
+            => Math.Sqrt(LengthSquared());
+        public readonly double LengthSquared()
+            => Math.Sqrt(X*X + Y*Y + Z*Z + W*W);
+        #endregion
+
+        #region Static methods
+        public static QuaternionD Normalize(QuaternionD value)
+        {
+            QuaternionD ans;
+
+            Double ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
+
+            Double invNorm = 1.0 / Math.Sqrt(ls);
+
+            ans.X = value.X * invNorm;
+            ans.Y = value.Y * invNorm;
+            ans.Z = value.Z * invNorm;
+            ans.W = value.W * invNorm;
+
+            return ans;
+        }
+
+        public static Quaternion Conjugate(Quaternion value)
+            => new Quaternion(-value.X, -value.Y, -value.Z, value.W);
+
+        public static QuaternionD Inverse(Quaternion value)
+        {
+            QuaternionD ans;
+
+            Double ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
+            Double invNorm = 1.0f / ls;
+
+            ans.X = -value.X * invNorm;
+            ans.Y = -value.Y * invNorm;
+            ans.Z = -value.Z * invNorm;
+            ans.W = value.W * invNorm;
+
+            return ans;
+        }
+
+        public static QuaternionD CreateFromAxisAngle(Vector3D axis, Double angle)
+        {
+            QuaternionD ans;
+
+            Double halfAngle = angle * 0.5;
+            Double s = Math.Sin(halfAngle);
+            Double c = Math.Cos(halfAngle);
+
+            ans.X = axis.X * s;
+            ans.Y = axis.Y * s;
+            ans.Z = axis.Z * s;
+            ans.W = c;
+
+            return ans;
+        }
+
+        public static QuaternionD CreateFromAxisAngle(Vector3 axis, Single angle)
+        {
+            QuaternionD ans;
+
+            Single halfAngle = angle * 0.5f;
+            Single s = MathF.Sin(halfAngle);
+            Single c = MathF.Cos(halfAngle);
+
+            ans.X = axis.X * s;
+            ans.Y = axis.Y * s;
+            ans.Z = axis.Z * s;
+            ans.W = c;
+
+            return ans;
+        }
+
+        public static QuaternionD CreateFromYawPitchRoll(Double yaw, Double pitch, Double roll)
+        {
+            //  Roll first, about axis the object is facing, then
+            //  pitch upward, then yaw to face into the new heading
+            Double sr, cr, sp, cp, sy, cy;
+
+            Double halfRoll = roll * 0.5;
+            sr = Math.Sin(halfRoll);
+            cr = Math.Cos(halfRoll);
+
+            Double halfPitch = pitch * 0.5;
+            sp = Math.Sin(halfPitch);
+            cp = Math.Cos(halfPitch);
+
+            Double halfYaw = yaw * 0.5;
+            sy = Math.Sin(halfYaw);
+            cy = Math.Cos(halfYaw);
+
+            QuaternionD result;
+
+            result.X = cy * sp * cr + sy * cp * sr;
+            result.Y = sy * cp * cr - cy * sp * sr;
+            result.Z = cy * cp * sr - sy * sp * cr;
+            result.W = cy * cp * cr + sy * sp * sr;
+
+            return result;
+        }
+
+        public static QuaternionD CreateFromRotationMatrix(Matrix4x4D matrix)
+        {
+            Double trace = matrix.M11 + matrix.M22 + matrix.M33;
+
+            QuaternionD q = new QuaternionD();
+
+            if (trace > 0.0)
+            {
+                Double s = Math.Sqrt(trace + 1.0);
+                q.W = s * 0.5;
+                s = 0.5 / s;
+                q.X = (matrix.M23 - matrix.M32) * s;
+                q.Y = (matrix.M31 - matrix.M13) * s;
+                q.Z = (matrix.M12 - matrix.M21) * s;
+            }
+            else
+            {
+                if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
+                {
+                    Double s = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
+                    Double invS = 0.5 / s;
+                    q.X = 0.5 * s;
+                    q.Y = (matrix.M12 + matrix.M21) * invS;
+                    q.Z = (matrix.M13 + matrix.M31) * invS;
+                    q.W = (matrix.M23 - matrix.M32) * invS;
+                }
+                else if (matrix.M22 > matrix.M33)
+                {
+                    Double s = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
+                    Double invS = 0.5 / s;
+                    q.X = (matrix.M21 + matrix.M12) * invS;
+                    q.Y = 0.5 * s;
+                    q.Z = (matrix.M32 + matrix.M23) * invS;
+                    q.W = (matrix.M31 - matrix.M13) * invS;
+                }
+                else
+                {
+                    Double s = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
+                    Double invS = 0.5 / s;
+                    q.X = (matrix.M31 + matrix.M13) * invS;
+                    q.Y = (matrix.M32 + matrix.M23) * invS;
+                    q.Z = 0.5 * s;
+                    q.W = (matrix.M12 - matrix.M21) * invS;
+                }
+            }
+
+            return q;
+        }
+
+        public static double Dot(QuaternionD value1, QuaternionD value2)
+            => value1.X * value2.X + value1.Y * value2.Y + value1.Z * value2.Z + value1.W * value2.W;
+
+        public static QuaternionD Slerp(QuaternionD value1, QuaternionD value2, Double amount)
+        {
+            Double t = amount;
+
+            Double cosOmega = value1.X * value2.X + value1.Y * value2.Y +
+                             value1.Z * value2.Z + value1.W * value2.W;
+
+            bool flip = false;
+
+            if (cosOmega < 0.0f)
+            {
+                flip = true;
+                cosOmega = -cosOmega;
+            }
+
+            Double s1, s2;
+
+            if (cosOmega > (1.0 - double.Epsilon))
+            {
+                // Too close, do straight linear interpolation.
+                s1 = 1.0 - t;
+                s2 = (flip) ? -t : t;
+            }
+            else
+            {
+                Double omega = Math.Acos(cosOmega);
+                Double invSinOmega = 1.0 / Math.Sin(omega);
+
+                s1 = Math.Sin((1.0 - t) * omega) * invSinOmega;
+                s2 = flip
+                    ?  - Math.Sin(t * omega) * invSinOmega
+                    : Math.Sin(t * omega) * invSinOmega;
+            }
+
+            QuaternionD ans;
+
+            ans.X = s1 * value1.X + s2 * value2.X;
+            ans.Y = s1 * value1.Y + s2 * value2.Y;
+            ans.Z = s1 * value1.Z + s2 * value2.Z;
+            ans.W = s1 * value1.W + s2 * value2.W;
+
+            return ans;
+        }
+
+        public static QuaternionD Concatenate(QuaternionD value1, QuaternionD value2)
+        {
+            QuaternionD ans;
+
+            // Concatenate rotation is actually q2 * q1 instead of q1 * q2.
+            // So that's why value2 goes q1 and value1 goes q2.
+            Double q1x = value2.X;
+            Double q1y = value2.Y;
+            Double q1z = value2.Z;
+            Double q1w = value2.W;
+
+            Double q2x = value1.X;
+            Double q2y = value1.Y;
+            Double q2z = value1.Z;
+            Double q2w = value1.W;
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+
+        public static QuaternionD Lerp(QuaternionD value1, QuaternionD value2, Double amount)
+        {
+            Double t = amount;
+            Double t1 = 1.0f - t;
+
+            QuaternionD r = new QuaternionD();
+
+            Double dot = value1.X * value2.X + value1.Y * value2.Y +
+                        value1.Z * value2.Z + value1.W * value2.W;
+
+            if (dot >= 0.0f)
+            {
+                r.X = t1 * value1.X + t * value2.X;
+                r.Y = t1 * value1.Y + t * value2.Y;
+                r.Z = t1 * value1.Z + t * value2.Z;
+                r.W = t1 * value1.W + t * value2.W;
+            }
+            else
+            {
+                r.X = t1 * value1.X - t * value2.X;
+                r.Y = t1 * value1.Y - t * value2.Y;
+                r.Z = t1 * value1.Z - t * value2.Z;
+                r.W = t1 * value1.W - t * value2.W;
+            }
+
+            // Normalize it.
+            Double ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W;
+            Double invNorm = 1.0 / Math.Sqrt(ls);
+
+            r.X *= invNorm;
+            r.Y *= invNorm;
+            r.Z *= invNorm;
+            r.W *= invNorm;
+
+            return r;
+        }
+
+        public static QuaternionD Concatenate(Quaternion value1, Quaternion value2)
+        {
+            QuaternionD ans;
+
+            // Concatenate rotation is actually q2 * q1 instead of q1 * q2.
+            // So that's why value2 goes q1 and value1 goes q2.
+            Double q1x = value2.X;
+            Double q1y = value2.Y;
+            Double q1z = value2.Z;
+            Double q1w = value2.W;
+
+            Double q2x = value1.X;
+            Double q2y = value1.Y;
+            Double q2z = value1.Z;
+            Double q2w = value1.W;
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+        #endregion
+
+        #region Operators
+        public static QuaternionD operator -(QuaternionD value)
+            => new QuaternionD(-value.X, -value.Y, -value.X, -value.W);
+
+        public static QuaternionD operator +(QuaternionD left, QuaternionD right)
+            => new QuaternionD(left.X + right.X, left.Y + right.Y, left.Z + right.Z, left.W + right.W);
+
+        public static QuaternionD operator -(QuaternionD left, QuaternionD right)
+            => new QuaternionD(left.X - right.X, left.Y - right.Y, left.Z - right.Z, left.W - right.W);
+
+        public static QuaternionD operator *(QuaternionD left, QuaternionD right)
+        {
+            QuaternionD ans;
+
+            Double q1x = left.X;
+            Double q1y = left.Y;
+            Double q1z = left.Z;
+            Double q1w = left.W;
+
+            Double q2x = right.X;
+            Double q2y = right.Y;
+            Double q2z = right.Z;
+            Double q2w = right.W;
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+
+        public static QuaternionD operator *(QuaternionD left, double right)
+            => new QuaternionD(left.X * right, left.Y * right, left.Z * right, left.W * right);
+
+        public static QuaternionD operator /(QuaternionD left, QuaternionD right)
+        {
+            QuaternionD ans;
+
+            Double q1x = left.X;
+            Double q1y = left.Y;
+            Double q1z = left.Z;
+            Double q1w = left.W;
+
+            //-------------------------------------
+            // Inverse part.
+            Double ls = right.X * right.X + right.Y * right.Y +
+                       right.Z * right.Z + right.W * right.W;
+            Double invNorm = 1.0f / ls;
+
+            Double q2x = -right.X * invNorm;
+            Double q2y = -right.Y * invNorm;
+            Double q2z = -right.Z * invNorm;
+            Double q2w = right.W * invNorm;
+
+            //-------------------------------------
+            // Multiply part.
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+
+        public static bool operator ==(QuaternionD left, QuaternionD right)
+            => left.X == right.X && left.Y == right.Y && left.Z == right.Z && left.W == right.W;
+
+        public static bool operator !=(QuaternionD left, QuaternionD right)
+            => left.X != right.X || left.Y != right.Y || left.Z != right.Z || left.W != right.W;
+
+        #endregion
+
+        #region Operator methods
+        public static QuaternionD Negate(QuaternionD value)
+            => new QuaternionD(-value.X, -value.Y, -value.Z, -value.W);
+
+        public static QuaternionD Add(QuaternionD value1, QuaternionD value2)
+            => new QuaternionD(value1.X + value2.X, value1.Y + value2.Y, value1.Z + value2.Z, value1.W + value2.W);
+
+        public static QuaternionD Subtract(QuaternionD value1, QuaternionD value2)
+            => new QuaternionD(value1.X - value2.X, value1.Y - value2.Y, value1.Z - value2.Z, value1.W - value2.W);
+
+        public static QuaternionD Multiply(QuaternionD value1, QuaternionD value2)
+        {
+            QuaternionD ans;
+
+            Double q1x = value1.X;
+            Double q1y = value1.Y;
+            Double q1z = value1.Z;
+            Double q1w = value1.W;
+
+            Double q2x = value2.X;
+            Double q2y = value2.Y;
+            Double q2z = value2.Z;
+            Double q2w = value2.W;
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+
+        public static QuaternionD Multiply(QuaternionD value1, Double value2)
+        {
+            QuaternionD ans;
+
+            ans.X = value1.X * value2;
+            ans.Y = value1.Y * value2;
+            ans.Z = value1.Z * value2;
+            ans.W = value1.W * value2;
+
+            return ans;
+        }
+
+        public static QuaternionD Divide(QuaternionD value1, QuaternionD value2)
+        {
+            QuaternionD ans;
+
+            Double q1x = value1.X;
+            Double q1y = value1.Y;
+            Double q1z = value1.Z;
+            Double q1w = value1.W;
+
+            //-------------------------------------
+            // Inverse part.
+            Double ls = value2.X * value2.X + value2.Y * value2.Y +
+                       value2.Z * value2.Z + value2.W * value2.W;
+            Double invNorm = 1.0 / ls;
+
+            Double q2x = -value2.X * invNorm;
+            Double q2y = -value2.Y * invNorm;
+            Double q2z = -value2.Z * invNorm;
+            Double q2w = value2.W * invNorm;
+
+            //-------------------------------------
+            // Multiply part.
+
+            // cross(av, bv)
+            Double cx = q1y * q2z - q1z * q2y;
+            Double cy = q1z * q2x - q1x * q2z;
+            Double cz = q1x * q2y - q1y * q2x;
+
+            Double dot = q1x * q2x + q1y * q2y + q1z * q2z;
+
+            ans.X = q1x * q2w + q2x * q1w + cx;
+            ans.Y = q1y * q2w + q2y * q1w + cy;
+            ans.Z = q1z * q2w + q2z * q1w + cz;
+            ans.W = q1w * q2w - dot;
+
+            return ans;
+        }
+        #endregion
+
+        #region Equals
+
+        public override int GetHashCode()
+            => X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode();
+
+        public override bool Equals(object obj)
+        {
+            if (!(obj is QuaternionD))
+                return false;
+            return Equals((QuaternionD)obj);
+        }
+
         public bool Equals(QuaternionD other)
             => X == other.X && Y == other.Y && Z == other.Z && W == other.W;
+        #endregion
     } 
 }

PveTeam.Math/Vector3D.cs

@@ -31,10 +31,10 @@ namespace PveTeam.Mathematics
         #endregion
 
         #region Instance methods
-        public double Length()
+        public readonly double Length()
             => Math.Sqrt(LengthSquared());
 
-        public double LengthSquared()
+        public readonly double LengthSquared()
             => X * X + Y * Y + Z * Z;
         #endregion
 

PveTeam.Math/Vector3I.cs

@@ -0,0 +1,9 @@
+using System;
+
+namespace PveTeam.Mathematics
+{
+    public struct Vector3I
+    {
+        public Int32 X, Y, Z;
+    }
+}