System.DoubleNumerics by weingartner machinen bau

<PackageReference Include="System.DoubleNumerics" Version="3.1.0" />

 Quaternion

public struct Quaternion : IEquatable<Quaternion>
using System.Globalization; namespace System.DoubleNumerics { public struct Quaternion : IEquatable<Quaternion> { public double X; public double Y; public double Z; public double W; public static Quaternion Identity => new Quaternion(0, 0, 0, 1); public bool IsIdentity => X == 0 && Y == 0 && Z == 0 && W == 1; public Quaternion(double x, double y, double z, double w) { X = x; Y = y; Z = z; W = w; } public Quaternion(Vector3 vectorPart, double scalarPart) { X = vectorPart.X; Y = vectorPart.Y; Z = vectorPart.Z; W = scalarPart; } public double Length() { double num = X * X + Y * Y + Z * Z + W * W; return Math.Sqrt(num); } public double LengthSquared() { return X * X + Y * Y + Z * Z + W * W; } public static Quaternion Normalize(Quaternion value) { double num = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; double num2 = 1 / Math.Sqrt(num); Quaternion result = default(Quaternion); result.X = value.X * num2; result.Y = value.Y * num2; result.Z = value.Z * num2; result.W = value.W * num2; return result; } public static Quaternion Conjugate(Quaternion value) { Quaternion result = default(Quaternion); result.X = 0 - value.X; result.Y = 0 - value.Y; result.Z = 0 - value.Z; result.W = value.W; return result; } public static Quaternion Inverse(Quaternion value) { double num = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; double num2 = 1 / num; Quaternion result = default(Quaternion); result.X = (0 - value.X) * num2; result.Y = (0 - value.Y) * num2; result.Z = (0 - value.Z) * num2; result.W = value.W * num2; return result; } public static Quaternion CreateFromAxisAngle(Vector3 axis, double angle) { double num = angle * 0.5; double num2 = Math.Sin(num); double w = Math.Cos(num); Quaternion result = default(Quaternion); result.X = axis.X * num2; result.Y = axis.Y * num2; result.Z = axis.Z * num2; result.W = w; return result; } public static Quaternion CreateFromYawPitchRoll(double yaw, double pitch, double roll) { double num = roll * 0.5; double num2 = Math.Sin(num); double num3 = Math.Cos(num); double num4 = pitch * 0.5; double num5 = Math.Sin(num4); double num6 = Math.Cos(num4); double num7 = yaw * 0.5; double num8 = Math.Sin(num7); double num9 = Math.Cos(num7); Quaternion result = default(Quaternion); result.X = num9 * num5 * num3 + num8 * num6 * num2; result.Y = num8 * num6 * num3 - num9 * num5 * num2; result.Z = num9 * num6 * num2 - num8 * num5 * num3; result.W = num9 * num6 * num3 + num8 * num5 * num2; return result; } public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix) { double num = matrix.M11 + matrix.M22 + matrix.M33; Quaternion result = default(Quaternion); if (num > 0) { double num2 = Math.Sqrt(num + 1); result.W = num2 * 0.5; num2 = 0.5 / num2; result.X = (matrix.M23 - matrix.M32) * num2; result.Y = (matrix.M31 - matrix.M13) * num2; result.Z = (matrix.M12 - matrix.M21) * num2; } else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33) { double num3 = Math.Sqrt(1 + matrix.M11 - matrix.M22 - matrix.M33); double num4 = 0.5 / num3; result.X = 0.5 * num3; result.Y = (matrix.M12 + matrix.M21) * num4; result.Z = (matrix.M13 + matrix.M31) * num4; result.W = (matrix.M23 - matrix.M32) * num4; } else if (matrix.M22 > matrix.M33) { double num5 = Math.Sqrt(1 + matrix.M22 - matrix.M11 - matrix.M33); double num6 = 0.5 / num5; result.X = (matrix.M21 + matrix.M12) * num6; result.Y = 0.5 * num5; result.Z = (matrix.M32 + matrix.M23) * num6; result.W = (matrix.M31 - matrix.M13) * num6; } else { double num7 = Math.Sqrt(1 + matrix.M33 - matrix.M11 - matrix.M22); double num8 = 0.5 / num7; result.X = (matrix.M31 + matrix.M13) * num8; result.Y = (matrix.M32 + matrix.M23) * num8; result.Z = 0.5 * num7; result.W = (matrix.M12 - matrix.M21) * num8; } return result; } public static double Dot(Quaternion quaternion1, Quaternion quaternion2) { return quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; } public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, double amount) { double num = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; bool flag = false; if (num < 0) { flag = true; num = 0 - num; } double num2; double num3; if (num > 0.999999) { num2 = 1 - amount; num3 = (flag ? (0 - amount) : amount); } else { double num4 = Math.Acos(num); double num5 = 1 / Math.Sin(num4); num2 = Math.Sin((1 - amount) * num4) * num5; num3 = (flag ? ((0 - Math.Sin(amount * num4)) * num5) : (Math.Sin(amount * num4) * num5)); } Quaternion result = default(Quaternion); result.X = num2 * quaternion1.X + num3 * quaternion2.X; result.Y = num2 * quaternion1.Y + num3 * quaternion2.Y; result.Z = num2 * quaternion1.Z + num3 * quaternion2.Z; result.W = num2 * quaternion1.W + num3 * quaternion2.W; return result; } public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, double amount) { double num = 1 - amount; Quaternion quaternion3 = default(Quaternion); double num2 = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; if (num2 >= 0) { quaternion3.X = num * quaternion1.X + amount * quaternion2.X; quaternion3.Y = num * quaternion1.Y + amount * quaternion2.Y; quaternion3.Z = num * quaternion1.Z + amount * quaternion2.Z; quaternion3.W = num * quaternion1.W + amount * quaternion2.W; } else { quaternion3.X = num * quaternion1.X - amount * quaternion2.X; quaternion3.Y = num * quaternion1.Y - amount * quaternion2.Y; quaternion3.Z = num * quaternion1.Z - amount * quaternion2.Z; quaternion3.W = num * quaternion1.W - amount * quaternion2.W; } double num3 = quaternion3.X * quaternion3.X + quaternion3.Y * quaternion3.Y + quaternion3.Z * quaternion3.Z + quaternion3.W * quaternion3.W; double num4 = 1 / Math.Sqrt(num3); quaternion3.X *= num4; quaternion3.Y *= num4; quaternion3.Z *= num4; quaternion3.W *= num4; return quaternion3; } public static Quaternion Concatenate(Quaternion value1, Quaternion value2) { double x = value2.X; double y = value2.Y; double z = value2.Z; double w = value2.W; double x2 = value1.X; double y2 = value1.Y; double z2 = value1.Z; double w2 = value1.W; double num = y * z2 - z * y2; double num2 = z * x2 - x * z2; double num3 = x * y2 - y * x2; double num4 = x * x2 + y * y2 + z * z2; Quaternion result = default(Quaternion); result.X = x * w2 + x2 * w + num; result.Y = y * w2 + y2 * w + num2; result.Z = z * w2 + z2 * w + num3; result.W = w * w2 - num4; return result; } public static Quaternion Negate(Quaternion value) { Quaternion result = default(Quaternion); result.X = 0 - value.X; result.Y = 0 - value.Y; result.Z = 0 - value.Z; result.W = 0 - value.W; return result; } public static Quaternion Add(Quaternion value1, Quaternion value2) { Quaternion result = default(Quaternion); result.X = value1.X + value2.X; result.Y = value1.Y + value2.Y; result.Z = value1.Z + value2.Z; result.W = value1.W + value2.W; return result; } public static Quaternion Subtract(Quaternion value1, Quaternion value2) { Quaternion result = default(Quaternion); result.X = value1.X - value2.X; result.Y = value1.Y - value2.Y; result.Z = value1.Z - value2.Z; result.W = value1.W - value2.W; return result; } public static Quaternion Multiply(Quaternion value1, Quaternion value2) { double x = value1.X; double y = value1.Y; double z = value1.Z; double w = value1.W; double x2 = value2.X; double y2 = value2.Y; double z2 = value2.Z; double w2 = value2.W; double num = y * z2 - z * y2; double num2 = z * x2 - x * z2; double num3 = x * y2 - y * x2; double num4 = x * x2 + y * y2 + z * z2; Quaternion result = default(Quaternion); result.X = x * w2 + x2 * w + num; result.Y = y * w2 + y2 * w + num2; result.Z = z * w2 + z2 * w + num3; result.W = w * w2 - num4; return result; } public static Quaternion Multiply(Quaternion value1, double value2) { Quaternion result = default(Quaternion); result.X = value1.X * value2; result.Y = value1.Y * value2; result.Z = value1.Z * value2; result.W = value1.W * value2; return result; } public static Quaternion Divide(Quaternion value1, Quaternion value2) { double x = value1.X; double y = value1.Y; double z = value1.Z; double w = value1.W; double num = value2.X * value2.X + value2.Y * value2.Y + value2.Z * value2.Z + value2.W * value2.W; double num2 = 1 / num; double num3 = (0 - value2.X) * num2; double num4 = (0 - value2.Y) * num2; double num5 = (0 - value2.Z) * num2; double num6 = value2.W * num2; double num7 = y * num5 - z * num4; double num8 = z * num3 - x * num5; double num9 = x * num4 - y * num3; double num10 = x * num3 + y * num4 + z * num5; Quaternion result = default(Quaternion); result.X = x * num6 + num3 * w + num7; result.Y = y * num6 + num4 * w + num8; result.Z = z * num6 + num5 * w + num9; result.W = w * num6 - num10; return result; } public static Quaternion operator -(Quaternion value) { Quaternion result = default(Quaternion); result.X = 0 - value.X; result.Y = 0 - value.Y; result.Z = 0 - value.Z; result.W = 0 - value.W; return result; } public static Quaternion operator +(Quaternion value1, Quaternion value2) { Quaternion result = default(Quaternion); result.X = value1.X + value2.X; result.Y = value1.Y + value2.Y; result.Z = value1.Z + value2.Z; result.W = value1.W + value2.W; return result; } public static Quaternion operator -(Quaternion value1, Quaternion value2) { Quaternion result = default(Quaternion); result.X = value1.X - value2.X; result.Y = value1.Y - value2.Y; result.Z = value1.Z - value2.Z; result.W = value1.W - value2.W; return result; } public static Quaternion operator *(Quaternion value1, Quaternion value2) { double x = value1.X; double y = value1.Y; double z = value1.Z; double w = value1.W; double x2 = value2.X; double y2 = value2.Y; double z2 = value2.Z; double w2 = value2.W; double num = y * z2 - z * y2; double num2 = z * x2 - x * z2; double num3 = x * y2 - y * x2; double num4 = x * x2 + y * y2 + z * z2; Quaternion result = default(Quaternion); result.X = x * w2 + x2 * w + num; result.Y = y * w2 + y2 * w + num2; result.Z = z * w2 + z2 * w + num3; result.W = w * w2 - num4; return result; } public static Quaternion operator *(Quaternion value1, double value2) { Quaternion result = default(Quaternion); result.X = value1.X * value2; result.Y = value1.Y * value2; result.Z = value1.Z * value2; result.W = value1.W * value2; return result; } public static Quaternion operator /(Quaternion value1, Quaternion value2) { double x = value1.X; double y = value1.Y; double z = value1.Z; double w = value1.W; double num = value2.X * value2.X + value2.Y * value2.Y + value2.Z * value2.Z + value2.W * value2.W; double num2 = 1 / num; double num3 = (0 - value2.X) * num2; double num4 = (0 - value2.Y) * num2; double num5 = (0 - value2.Z) * num2; double num6 = value2.W * num2; double num7 = y * num5 - z * num4; double num8 = z * num3 - x * num5; double num9 = x * num4 - y * num3; double num10 = x * num3 + y * num4 + z * num5; Quaternion result = default(Quaternion); result.X = x * num6 + num3 * w + num7; result.Y = y * num6 + num4 * w + num8; result.Z = z * num6 + num5 * w + num9; result.W = w * num6 - num10; return result; } public static bool operator ==(Quaternion value1, Quaternion value2) { return value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z && value1.W == value2.W; } public static bool operator !=(Quaternion value1, Quaternion value2) { return value1.X != value2.X || value1.Y != value2.Y || value1.Z != value2.Z || value1.W != value2.W; } public bool Equals(Quaternion other) { return X == other.X && Y == other.Y && Z == other.Z && W == other.W; } public override bool Equals(object obj) { if (!(obj is Quaternion)) return false; return Equals((Quaternion)obj); } public override string ToString() { CultureInfo currentCulture = CultureInfo.CurrentCulture; return string.Format(currentCulture, "{{X:{0} Y:{1} Z:{2} W:{3}}}", X.ToString(currentCulture), Y.ToString(currentCulture), Z.ToString(currentCulture), W.ToString(currentCulture)); } public override int GetHashCode() { return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode(); } } }