Vector3D implementation

PveTeam.Math/Matrix4x4D.cs

@@ -1,13 +1,18 @@
 using System;
-using System.Numerics;
 
-namespace PveTeam.Math
+namespace PveTeam.Mathematics
 {
-    public struct Matrix4x4D : IEquatable<Matrix4x4>
+    public struct Matrix4x4D : IEquatable<Matrix4x4D>
     {
         public Double M11, M12, M13, M14,
             M21, M22, M23, M24,
             M31, M32, M33, M34,
             M41, M42, M43, M44;
+
+        public bool Equals(Matrix4x4D other)
+            => M11 == other.M11 && M12 == other.M12 && M13 == other.M13 && M14 == other.M14
+            && M21 == other.M21 && M22 == other.M22 && M23 == other.M23 && M24 == other.M24
+            && M31 == other.M31 && M32 == other.M32 && M33 == other.M33 && M34 == other.M34
+            && M41 == other.M41 && M42 == other.M42 && M43 == other.M43 && M44 == other.M44;
     }
 }

PveTeam.Math/QuaternionD.cs

@@ -1,10 +1,12 @@
 using System;
-using System.Collections.Generic;
-using System.Text;
 
-namespace PveTeam.Math
+namespace PveTeam.Mathematics
 {
-    public struct QuaternionD
+    public struct QuaternionD : IEquatable<QuaternionD>
     {
+        public Double X, Y, Z, W;
+
+        public bool Equals(QuaternionD other)
+            => X == other.X && Y == other.Y && Z == other.Z && W == other.W;
     }
 }

PveTeam.Math/Vector3D.cs

@@ -1,29 +1,242 @@
 using System.Numerics;
 using System;
+using System.Globalization;
+using System.Text;
 
-namespace PveTeam.Math
+namespace PveTeam.Mathematics
 {
     public struct Vector3D : IEquatable<Vector3>, IFormattable
     {
         public Double X, Y, Z;
 
+        #region Static fields
+        public static Vector3D Zero => new Vector3D(0);
+        public static Vector3D One => new Vector3D(1);
+        public static Vector3D UnitX => new Vector3D(1, 0, 0);
+        public static Vector3D UnitY => new Vector3D(0, 1, 0);
+        public static Vector3D UnitZ => new Vector3D(0, 0, 1);
+        #endregion
+
+        #region Constructors
         public Vector3D(double value) : this(value, value, value) { }
 
+        public Vector3D(Vector3 value) : this(value.X, value.Y, value.Z) { }
+
         public Vector3D(Double x, Double y, Double z)
         {
             X = x;
             Y = y;
             Z = z;
         }
+        #endregion
+
+        #region Instance methods
+        public double Length()
+            => Math.Sqrt(LengthSquared());
+
+        public double LengthSquared()
+            => X * X + Y * Y + Z * Z;
+        #endregion
+
+        #region Static methods
+
+        public static double Distance(Vector3D value1, Vector3D value2)
+            => (value1 - value2).Length();
+
+        public static double DistanceSquared(Vector3D value1, Vector3D value2)
+            => (value1 - value2).LengthSquared();
+
+        public static Vector3D Normalize(Vector3D value)
+            => value / value.Length();
+
+        public static Vector3D Cross(Vector3D value1, Vector3D value2)
+            => new Vector3D(value1.Y * value2.Z - value1.Z * value2.Y,
+                value1.Z * value2.X - value1.X * value2.Z,
+                value1.X * value2.Y - value1.Y * value2.X);
+
+        public static Double Dot(Vector3D value1, Vector3D value2)
+            => value1.X * value2.X + value1.Y * value2.Y + value1.Z * value2.Z;
+
+        public static Vector3D Reflect(Vector3D vector, Vector3D normal)
+        {
+            Double dot = Dot(vector, normal);
+            Double tempX = normal.X * dot * 2f;
+            Double tempY = normal.Y * dot * 2f;
+            Double tempZ = normal.Z * dot * 2f;
+            return new Vector3D(vector.X - tempX, vector.Y - tempY, vector.Z - tempZ);
+        }
+
+        public static Vector3D Clamp(Vector3D value1, Vector3D min, Vector3D max)
+        {
+            double x = value1.X;
+            x = (x > max.X) ? max.X : x;
+            x = (x < min.X) ? min.X : x;
+
+            double y = value1.Y;
+            y = (y > max.Y) ? max.Y : y;
+            y = (y < min.Y) ? min.Y : y;
+
+            double z = value1.Z;
+            z = (z > max.Z) ? max.Z : z;
+            z = (z < min.Z) ? min.Z : z;
+
+            return new Vector3D(x, y, z);
+        }
+
+        public static Vector3D Lerp(Vector3D value1, Vector3D value2, double amount)
+            => new Vector3D(value1.X + (value2.X - value1.X) * amount,
+                    value1.Y + (value2.Y - value1.Y) * amount,
+                    value1.Z + (value2.Z - value1.Z) * amount);
+
+        public static Vector3D Transform(Vector3D position, Matrix4x4 matrix)
+            => new Vector3D(position.X * matrix.M11 + position.Y * matrix.M21 + position.Z * matrix.M31 + matrix.M41,
+                position.X * matrix.M12 + position.Y * matrix.M22 + position.Z * matrix.M32 + matrix.M42,
+                position.X * matrix.M13 + position.Y * matrix.M23 + position.Z * matrix.M33 + matrix.M43);
+
+        public static Vector3D Transform(Vector3D position, Matrix4x4D matrix)
+            => new Vector3D(position.X * matrix.M11 + position.Y * matrix.M21 + position.Z * matrix.M31 + matrix.M41,
+                position.X * matrix.M12 + position.Y * matrix.M22 + position.Z * matrix.M32 + matrix.M42,
+                position.X * matrix.M13 + position.Y * matrix.M23 + position.Z * matrix.M33 + matrix.M43);
+
+        public static Vector3D TransformNormal(Vector3D normal, Matrix4x4 matrix)
+            => new Vector3D(normal.X * matrix.M11 + normal.Y * matrix.M21 + normal.Z * matrix.M31,
+                normal.X * matrix.M12 + normal.Y * matrix.M22 + normal.Z * matrix.M32,
+                normal.X * matrix.M13 + normal.Y * matrix.M23 + normal.Z * matrix.M33);
+
+        public static Vector3D TransformNormal(Vector3D normal, Matrix4x4D matrix)
+            => new Vector3D(normal.X * matrix.M11 + normal.Y * matrix.M21 + normal.Z * matrix.M31,
+                normal.X * matrix.M12 + normal.Y * matrix.M22 + normal.Z * matrix.M32,
+                normal.X * matrix.M13 + normal.Y * matrix.M23 + normal.Z * matrix.M33);
+
+        public static Vector3D Transform(Vector3D value, Quaternion rotation)
+        {
+
+            Single x2 = rotation.X + rotation.X;
+            Single y2 = rotation.Y + rotation.Y;
+            Single z2 = rotation.Z + rotation.Z;
+
+            Single wx2 = rotation.W * x2;
+            Single wy2 = rotation.W * y2;
+            Single wz2 = rotation.W * z2;
+            Single xx2 = rotation.X * x2;
+            Single xy2 = rotation.X * y2;
+            Single xz2 = rotation.X * z2;
+            Single yy2 = rotation.Y * y2;
+            Single yz2 = rotation.Y * z2;
+            Single zz2 = rotation.Z * z2;
+
+            return new Vector3D(
+                value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2),
+                value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2),
+                value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2));
+        }
+
+        public static Vector3D Transform(Vector3D value, QuaternionD rotation)
+        {
+
+            Double x2 = rotation.X + rotation.X;
+            Double y2 = rotation.Y + rotation.Y;
+            Double z2 = rotation.Z + rotation.Z;
+
+            Double wx2 = rotation.W * x2;
+            Double wy2 = rotation.W * y2;
+            Double wz2 = rotation.W * z2;
+            Double xx2 = rotation.X * x2;
+            Double xy2 = rotation.X * y2;
+            Double xz2 = rotation.X * z2;
+            Double yy2 = rotation.Y * y2;
+            Double yz2 = rotation.Y * z2;
+            Double zz2 = rotation.Z * z2;
+
+            return new Vector3D(
+                value.X * (1.0 - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2),
+                value.X * (xy2 + wz2) + value.Y * (1.0 - xx2 - zz2) + value.Z * (yz2 - wx2),
+                value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0 - xx2 - yy2));
+        }
+        #endregion
+
+        #region Operators
+        public static Vector3D operator +(Vector3D left, Vector3D right)
+            => new Vector3D(left.X + right.X, left.Y + right.Y, left.Z + right.Z);
+        public static Vector3D operator -(Vector3D left, Vector3D right)
+            => new Vector3D(left.X - right.X, left.Y - right.Y, left.Z - right.Z);
+        public static Vector3D operator *(Vector3D left, Vector3D right)
+            => new Vector3D(left.X * right.X, left.Y * right.Y, left.Z * right.Z);
+        public static Vector3D operator *(Vector3D left, Double right)
+            => left * new Vector3D(right);
+        public static Vector3D operator *(Double left, Vector3D right)
+            => new Vector3D(left) * right;
+        public static Vector3D operator /(Vector3D left, Vector3D right)
+            => new Vector3D(left.X / right.X, left.Y / right.Y, left.Z / right.Z);
+        public static Vector3D operator /(Vector3D left, Double right)
+        {
+            Double invert = 1.0 / right;
+            return new Vector3D(left.X * invert, left.Y * invert, left.Z * invert);
+        }
+        public static Vector3D operator -(Vector3D value)
+            => Zero - value;
+        public static bool operator ==(Vector3D left, Vector3D right)
+            => left.X == right.X && left.Y == right.Y && left.Z == right.Z;
+        public static bool operator !=(Vector3D left, Vector3D right)
+            => left.X != right.X || left.Y != right.Y || left.Z != right.Z;
+
+        #endregion
+
+        #region Operator methods
+        public static Vector3D Add(Vector3D left, Vector3D right)
+            => left + right;
+        public static Vector3D Subtract(Vector3D left, Vector3D right)
+            => left - right;
+        public static Vector3D Multiply(Vector3D left, Vector3D right)
+            => left * right;
+        public static Vector3D Multiply(Vector3D left, double right)
+            => left * right;
+        public static Vector3D Multiply(double left, Vector3D right)
+                    => left * right;
+        public static Vector3D Divide(Vector3D left, Vector3D right)
+            => left / right;
+        public static Vector3D Divide(Vector3D left, double right)
+            => left / right;
+        #endregion
+
+        #region Equals
+
+        public override int GetHashCode()
+            => X.GetHashCode() ^ Y.GetHashCode() << 2 ^ Z.GetHashCode() >> 2;
+
+        public override bool Equals(object obj)
+        {
+            if (!(obj is Vector3))
+                return false;
+            return Equals((Vector3)obj);
+        }
 
         public bool Equals(Vector3 other)
-        {
+            => X == other.X && Y == other.Y && Z == other.Z;
-            throw new NotImplementedException();
+        #endregion
-        }
 
+        #region Formatting
         public string ToString(string format, IFormatProvider formatProvider)
         {
-            throw new NotImplementedException();
+            StringBuilder sb = new StringBuilder();
+            string separator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator;
+            sb.Append('<');
+            sb.Append(((IFormattable)this.X).ToString(format, formatProvider));
+            sb.Append(separator);
+            sb.Append(' ');
+            sb.Append(((IFormattable)this.Y).ToString(format, formatProvider));
+            sb.Append(separator);
+            sb.Append(' ');
+            sb.Append(((IFormattable)this.Z).ToString(format, formatProvider));
+            sb.Append('>');
+            return sb.ToString();
         }
+
+        public override string ToString()
+            => ToString("G", CultureInfo.CurrentCulture);
+
+        public string ToString(string format)
+            => ToString(format, CultureInfo.CurrentCulture);
+        #endregion
     }
 }

PveTeam.Mathematics.sln

@@ -3,7 +3,7 @@ Microsoft Visual Studio Solution File, Format Version 12.00
 # Visual Studio Version 17
 VisualStudioVersion = 17.8.34309.116
 MinimumVisualStudioVersion = 10.0.40219.1
-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "PveTeam.Math", "PveTeam.Math\PveTeam.Math.csproj", "{D875C780-DB5A-4252-94DE-6C6BA7C78A0F}"
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "PveTeam.Mathematics", "PveTeam.Math\PveTeam.Mathematics.csproj", "{D875C780-DB5A-4252-94DE-6C6BA7C78A0F}"
 EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution