243 lines
9.9 KiB
C#
243 lines
9.9 KiB
C#
using System.Numerics;
|
|
using System;
|
|
using System.Globalization;
|
|
using System.Text;
|
|
|
|
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;
|
|
#endregion
|
|
|
|
#region Formatting
|
|
public string ToString(string format, IFormatProvider formatProvider)
|
|
{
|
|
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
|
|
}
|
|
}
|