diff --git a/PveTeam.Math/QuaternionD.cs b/PveTeam.Math/QuaternionD.cs index c93be08..b20fe67 100644 --- a/PveTeam.Math/QuaternionD.cs +++ b/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 + } +} \ No newline at end of file diff --git a/PveTeam.Math/Vector3D.cs b/PveTeam.Math/Vector3D.cs index 8aa7aa3..51004a0 100644 --- a/PveTeam.Math/Vector3D.cs +++ b/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 @@ -239,4 +239,4 @@ namespace PveTeam.Mathematics => ToString(format, CultureInfo.CurrentCulture); #endregion } -} +} \ No newline at end of file diff --git a/PveTeam.Math/Vector3I.cs b/PveTeam.Math/Vector3I.cs new file mode 100644 index 0000000..6e5e820 --- /dev/null +++ b/PveTeam.Math/Vector3I.cs @@ -0,0 +1,9 @@ +using System; + +namespace PveTeam.Mathematics +{ + public struct Vector3I + { + public Int32 X, Y, Z; + } +}