ZvE
This commit is contained in:
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
|
||||
9
PveTeam.Math/Vector3I.cs
Normal file
9
PveTeam.Math/Vector3I.cs
Normal file
@@ -0,0 +1,9 @@
|
||||
using System;
|
||||
|
||||
namespace PveTeam.Mathematics
|
||||
{
|
||||
public struct Vector3I
|
||||
{
|
||||
public Int32 X, Y, Z;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user