From 19e869e8c37b0e43d3f88850179c20cf865923c3 Mon Sep 17 00:00:00 2001 From: Tada101 Date: Mon, 27 May 2024 12:02:14 +0200 Subject: [PATCH] Vector3D implementation --- PveTeam.Math/Matrix4x4D.cs | 11 +- ...Math.csproj => PveTeam.Mathematics.csproj} | 0 PveTeam.Math/QuaternionD.cs | 10 +- PveTeam.Math/Vector3D.cs | 223 +++++++++++++++++- PveTeam.Math.sln => PveTeam.Mathematics.sln | 2 +- 5 files changed, 233 insertions(+), 13 deletions(-) rename PveTeam.Math/{PveTeam.Math.csproj => PveTeam.Mathematics.csproj} (100%) rename PveTeam.Math.sln => PveTeam.Mathematics.sln (91%) diff --git a/PveTeam.Math/Matrix4x4D.cs b/PveTeam.Math/Matrix4x4D.cs index 0881648..c228af9 100644 --- a/PveTeam.Math/Matrix4x4D.cs +++ b/PveTeam.Math/Matrix4x4D.cs @@ -1,13 +1,18 @@ using System; -using System.Numerics; -namespace PveTeam.Math +namespace PveTeam.Mathematics { - public struct Matrix4x4D : IEquatable + public struct Matrix4x4D : IEquatable { 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; } } diff --git a/PveTeam.Math/PveTeam.Math.csproj b/PveTeam.Math/PveTeam.Mathematics.csproj similarity index 100% rename from PveTeam.Math/PveTeam.Math.csproj rename to PveTeam.Math/PveTeam.Mathematics.csproj diff --git a/PveTeam.Math/QuaternionD.cs b/PveTeam.Math/QuaternionD.cs index c2cd134..c93be08 100644 --- a/PveTeam.Math/QuaternionD.cs +++ b/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 { + public Double X, Y, Z, W; + + public bool Equals(QuaternionD other) + => X == other.X && Y == other.Y && Z == other.Z && W == other.W; } } diff --git a/PveTeam.Math/Vector3D.cs b/PveTeam.Math/Vector3D.cs index f03a909..8aa7aa3 100644 --- a/PveTeam.Math/Vector3D.cs +++ b/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, 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) - { - throw new NotImplementedException(); - } + => X == other.X && Y == other.Y && Z == other.Z; + #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 } } diff --git a/PveTeam.Math.sln b/PveTeam.Mathematics.sln similarity index 91% rename from PveTeam.Math.sln rename to PveTeam.Mathematics.sln index 4571458..a668f4f 100644 --- a/PveTeam.Math.sln +++ b/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