/// <summary>
/// Calculates the dot product of two Quaternions.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param>
public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2)
{
return (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
}
private Vector3D GetAngleVelocity(QuaternionD q1, QuaternionD q2)
{
q1.Conjugate();
QuaternionD r = q2 * q1;
double angle = 2 * System.Math.Acos(MathHelper.Clamp(r.W, -1, 1));
if (angle > Math.PI)
{
angle -= 2.0 * Math.PI;
}
Vector3D velocity = angle * new Vector3D(r.X, r.Y, r.Z) /
System.Math.Sqrt(r.X * r.X + r.Y * r.Y + r.Z * r.Z);
return velocity;
}
/// <summary>
/// Creates a QuaternionD from a rotation MatrixD.
/// </summary>
/// <param name="matrix">The rotation MatrixD to create the QuaternionD from.</param>
public static QuaternionD CreateFromRotationMatrix(MatrixD matrix)
{
double num1 = matrix.M11 + matrix.M22 + matrix.M33;
QuaternionD quaternion = new QuaternionD();
if (num1 > 0.0)
{
double num2 = Math.Sqrt(num1 + 1.0);
quaternion.W = num2 * 0.5;
double num3 = 0.5 / num2;
quaternion.X = (matrix.M23 - matrix.M32) * num3;
quaternion.Y = (matrix.M31 - matrix.M13) * num3;
quaternion.Z = (matrix.M12 - matrix.M21) * num3;
}
else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
{
double num2 = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
double num3 = 0.5 / num2;
quaternion.X = 0.5 * num2;
quaternion.Y = (matrix.M12 + matrix.M21) * num3;
quaternion.Z = (matrix.M13 + matrix.M31) * num3;
quaternion.W = (matrix.M23 - matrix.M32) * num3;
}
else if (matrix.M22 > matrix.M33)
{
double num2 = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
double num3 = 0.5 / num2;
quaternion.X = (matrix.M21 + matrix.M12) * num3;
quaternion.Y = 0.5 * num2;
quaternion.Z = (matrix.M32 + matrix.M23) * num3;
quaternion.W = (matrix.M31 - matrix.M13) * num3;
}
else
{
double num2 = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
double num3 = 0.5 / num2;
quaternion.X = (matrix.M31 + matrix.M13) * num3;
quaternion.Y = (matrix.M32 + matrix.M23) * num3;
quaternion.Z = 0.5 * num2;
quaternion.W = (matrix.M12 - matrix.M21) * num3;
}
return quaternion;
}
/// <summary>
/// Multiplies two quaternions.
/// </summary>
/// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = quaternion2.X;
double num6 = quaternion2.Y;
double num7 = quaternion2.Z;
double num8 = quaternion2.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
result.X = (num1 * num8 + num5 * num4) + num9;
result.Y = (num2 * num8 + num6 * num4) + num10;
result.Z = (num3 * num8 + num7 * num4) + num11;
result.W = num4 * num8 - num12;
}
/// <summary>
/// Divides a QuaternionD by another QuaternionD.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param><param name="result">[OutAttribute] Result of the division.</param>
public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
double num6 = -quaternion2.X * num5;
double num7 = -quaternion2.Y * num5;
double num8 = -quaternion2.Z * num5;
double num9 = quaternion2.W * num5;
double num10 = (num2 * num8 - num3 * num7);
double num11 = (num3 * num6 - num1 * num8);
double num12 = (num1 * num7 - num2 * num6);
double num13 = (num1 * num6 + num2 * num7 + num3 * num8);
result.X = (num1 * num9 + num6 * num4) + num10;
result.Y = (num2 * num9 + num7 * num4) + num11;
result.Z = (num3 * num9 + num8 * num4) + num12;
result.W = num4 * num9 - num13;
}
/// <summary>
/// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
/// </summary>
/// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param><param name="result">[OutAttribute] The QuaternionD rotation representing the concatenation of value1 followed by value2.</param>
public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
{
double num1 = value2.X;
double num2 = value2.Y;
double num3 = value2.Z;
double num4 = value2.W;
double num5 = value1.X;
double num6 = value1.Y;
double num7 = value1.Z;
double num8 = value1.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
result.X = (num1 * num8 + num5 * num4) + num9;
result.Y = (num2 * num8 + num6 * num4) + num10;
result.Z = (num3 * num8 + num7 * num4) + num11;
result.W = num4 * num8 - num12;
}
/// <summary>
/// Adds two Quaternions.
/// </summary>
/// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param><param name="result">[OutAttribute] Result of adding the Quaternions.</param>
public static void Add(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
result.X = quaternion1.X + quaternion2.X;
result.Y = quaternion1.Y + quaternion2.Y;
result.Z = quaternion1.Z + quaternion2.Z;
result.W = quaternion1.W + quaternion2.W;
}
public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double x = quaternion1.X;
double y = quaternion1.Y;
double z = quaternion1.Z;
double w = quaternion1.W;
double num5 = quaternion2.X;
double num6 = quaternion2.Y;
double num7 = quaternion2.Z;
double num8 = quaternion2.W;
double num9 = (y * num7) - (z * num6);
double num10 = (z * num5) - (x * num7);
double num11 = (x * num6) - (y * num5);
double num12 = ((x * num5) + (y * num6)) + (z * num7);
result.X = ((x * num8) + (num5 * w)) + num9;
result.Y = ((y * num8) + (num6 * w)) + num10;
result.Z = ((z * num8) + (num7 * w)) + num11;
result.W = (w * num8) - num12;
}
public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double x = quaternion1.X;
double y = quaternion1.Y;
double z = quaternion1.Z;
double w = quaternion1.W;
double num5 = 1.0 / ((((quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y)) + (quaternion2.Z * quaternion2.Z)) + (quaternion2.W * quaternion2.W));
double num6 = -quaternion2.X * num5;
double num7 = -quaternion2.Y * num5;
double num8 = -quaternion2.Z * num5;
double num9 = quaternion2.W * num5;
double num10 = (y * num8) - (z * num7);
double num11 = (z * num6) - (x * num8);
double num12 = (x * num7) - (y * num6);
double num13 = ((x * num6) + (y * num7)) + (z * num8);
result.X = ((x * num9) + (num6 * w)) + num10;
result.Y = ((y * num9) + (num7 * w)) + num11;
result.Z = ((z * num9) + (num8 * w)) + num12;
result.W = (w * num9) - num13;
}
public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num = amount;
double num2 = 1.0 - num;
if (((((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W)) >= 0.0)
{
result.X = (num2 * quaternion1.X) + (num * quaternion2.X);
result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y);
result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z);
result.W = (num2 * quaternion1.W) + (num * quaternion2.W);
}
else
{
result.X = (num2 * quaternion1.X) - (num * quaternion2.X);
result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y);
result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z);
result.W = (num2 * quaternion1.W) - (num * quaternion2.W);
}
double num3 = 1.0 / Math.Sqrt((((result.X * result.X) + (result.Y * result.Y)) + (result.Z * result.Z)) + (result.W * result.W));
result.X *= num3;
result.Y *= num3;
result.Z *= num3;
result.W *= num3;
}
public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
{
double x = value2.X;
double y = value2.Y;
double z = value2.Z;
double w = value2.W;
double num5 = value1.X;
double num6 = value1.Y;
double num7 = value1.Z;
double num8 = value1.W;
double num9 = (y * num7) - (z * num6);
double num10 = (z * num5) - (x * num7);
double num11 = (x * num6) - (y * num5);
double num12 = ((x * num5) + (y * num6)) + (z * num7);
result.X = ((x * num8) + (num5 * w)) + num9;
result.Y = ((y * num8) + (num6 * w)) + num10;
result.Z = ((z * num8) + (num7 * w)) + num11;
result.W = (w * num8) - num12;
}
public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num3;
double num4;
double num = amount;
double d = (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W);
bool flag = false;
if (d < 0.0)
{
flag = true;
d = -d;
}
if (d > 0.999998986721039)
{
num3 = 1.0 - num;
num4 = flag ? -num : num;
}
else
{
double a = Math.Acos(d);
double num6 = 1.0 / Math.Sin(a);
num3 = Math.Sin((1.0 - num) * a) * num6;
num4 = flag ? (-Math.Sin(num * a) * num6) : (Math.Sin(num * a) * num6);
}
result.X = (num3 * quaternion1.X) + (num4 * quaternion2.X);
result.Y = (num3 * quaternion1.Y) + (num4 * quaternion2.Y);
result.Z = (num3 * quaternion1.Z) + (num4 * quaternion2.Z);
result.W = (num3 * quaternion1.W) + (num4 * quaternion2.W);
}
public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2) => ((((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W));
static QuaternionD()
{
Identity = new QuaternionD(0.0, 0.0, 0.0, 1.0);
}
/// <summary>
/// Interpolates between two quaternions, using spherical linear interpolation.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value that indicates how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] Result of the interpolation.</param>
public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num1 = amount;
double num2 = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
bool flag = false;
if (num2 < 0.0)
{
flag = true;
num2 = -num2;
}
double num3;
double num4;
if (num2 > 0.999998986721039)
{
num3 = 1.0 - num1;
num4 = flag ? -num1 : num1;
}
else
{
double num5 = Math.Acos(num2);
double num6 = (1.0 / Math.Sin(num5));
num3 = Math.Sin((1.0 - num1) * num5) * num6;
num4 = flag ? -Math.Sin(num1 * num5) * num6 : Math.Sin(num1 * num5) * num6;
}
result.X = (num3 * quaternion1.X + num4 * quaternion2.X);
result.Y = (num3 * quaternion1.Y + num4 * quaternion2.Y);
result.Z = (num3 * quaternion1.Z + num4 * quaternion2.Z);
result.W = (num3 * quaternion1.W + num4 * quaternion2.W);
}
public static bool IsZero(QuaternionD value) => IsZero(value, 0.0001);
/// <summary>
/// Linearly interpolates between two quaternions.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] The resulting quaternion.</param>
public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num1 = amount;
double num2 = 1.0 - num1;
if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
{
result.X = (num2 * quaternion1.X + num1 * quaternion2.X);
result.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
result.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
result.W = (num2 * quaternion1.W + num1 * quaternion2.W);
}
else
{
result.X = (num2 * quaternion1.X - num1 * quaternion2.X);
result.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
result.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
result.W = (num2 * quaternion1.W - num1 * quaternion2.W);
}
double num3 = 1.0 / Math.Sqrt(result.X * result.X + result.Y * result.Y + result.Z * result.Z + result.W * result.W);
result.X *= num3;
result.Y *= num3;
result.Z *= num3;
result.W *= num3;
}
public static bool IsZero(QuaternionD value, double epsilon) => ((((Math.Abs(value.X) < epsilon) && (Math.Abs(value.Y) < epsilon)) && (Math.Abs(value.Z) < epsilon)) && (Math.Abs(value.W) < epsilon));
/// <summary>
/// Flips the sign of each component of the quaternion.
/// </summary>
/// <param name="quaternion">Source quaternion.</param><param name="result">[OutAttribute] Negated quaternion.</param>
public static void Negate(ref QuaternionD quaternion, out QuaternionD result)
{
result.X = -quaternion.X;
result.Y = -quaternion.Y;
result.Z = -quaternion.Z;
result.W = -quaternion.W;
}
public static void CreateFromTwoVectors(ref Vector3D firstVector, ref Vector3D secondVector, out QuaternionD result)
{
Vector3D vectord;
Vector3D.Cross(ref firstVector, ref secondVector, out vectord);
result = new QuaternionD(vectord.X, vectord.Y, vectord.Z, (double)Vector3.Dot((Vector3)firstVector, (Vector3)secondVector));
result.W += result.Length();
result.Normalize();
}
/// <summary>
/// Subtracts a quaternion from another quaternion.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="result">[OutAttribute] Result of the subtraction.</param>
public static void Subtract(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
result.X = quaternion1.X - quaternion2.X;
result.Y = quaternion1.Y - quaternion2.Y;
result.Z = quaternion1.Z - quaternion2.Z;
result.W = quaternion1.W - quaternion2.W;
}
// Per component IsZero, returns 1 for each component which equals to 0
public static bool IsZero(QuaternionD value, double epsilon)
{
return(Math.Abs(value.X) < epsilon && Math.Abs(value.Y) < epsilon && Math.Abs(value.Z) < epsilon && Math.Abs(value.W) < epsilon);
}
/// <summary>
/// Multiplies a quaternion by a scalar value.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="scaleFactor">Scalar value.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
public static void Multiply(ref QuaternionD quaternion1, double scaleFactor, out QuaternionD result)
{
result.X = quaternion1.X * scaleFactor;
result.Y = quaternion1.Y * scaleFactor;
result.Z = quaternion1.Z * scaleFactor;
result.W = quaternion1.W * scaleFactor;
}
/// <summary>
/// Creates a new QuaternionD from specified yaw, pitch, and roll angles.
/// </summary>
/// <param name="yaw">The yaw angle, in radians, around the y-axis.</param><param name="pitch">The pitch angle, in radians, around the x-axis.</param><param name="roll">The roll angle, in radians, around the z-axis.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to express the specified yaw, pitch, and roll angles.</param>
public static void CreateFromYawPitchRoll(double yaw, double pitch, double roll, out QuaternionD result)
{
double num1 = roll * 0.5;
double num2 = Math.Sin(num1);
double num3 = Math.Cos(num1);
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);
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);
}
public static bool IsZero(QuaternionD value)
{
return(IsZero(value, 0.0001));
}
/// <summary>
/// Calculates the dot product of two Quaternions.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param>
public static double Dot(QuaternionD quaternion1, QuaternionD quaternion2)
{
return(quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
}
public bool Equals(QuaternionD other) => ((((this.X == other.X) && (this.Y == other.Y)) && (this.Z == other.Z)) && (this.W == other.W));
/// <summary>
/// Calculates the dot product of two Quaternions.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param><param name="result">[OutAttribute] Dot product of the Quaternions.</param>
public static void Dot(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out double result)
{
result = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
}
/// <summary>
/// Creates a QuaternionD from a rotation MatrixD.
/// </summary>
/// <param name="matrix">The rotation MatrixD to create the QuaternionD from.</param><param name="result">[OutAttribute] The created QuaternionD.</param>
public static void CreateFromRotationMatrix(ref MatrixD matrix, out QuaternionD result)
{
double num1 = matrix.M11 + matrix.M22 + matrix.M33;
if (num1 > 0.0)
{
double num2 = Math.Sqrt(num1 + 1.0);
result.W = num2 * 0.5;
double num3 = 0.5 / num2;
result.X = (matrix.M23 - matrix.M32) * num3;
result.Y = (matrix.M31 - matrix.M13) * num3;
result.Z = (matrix.M12 - matrix.M21) * num3;
}
else if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33)
{
double num2 = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
double num3 = 0.5 / num2;
result.X = 0.5 * num2;
result.Y = (matrix.M12 + matrix.M21) * num3;
result.Z = (matrix.M13 + matrix.M31) * num3;
result.W = (matrix.M23 - matrix.M32) * num3;
}
else if (matrix.M22 > matrix.M33)
{
double num2 = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
double num3 = 0.5 / num2;
result.X = (matrix.M21 + matrix.M12) * num3;
result.Y = 0.5 * num2;
result.Z = (matrix.M32 + matrix.M23) * num3;
result.W = (matrix.M31 - matrix.M13) * num3;
}
else
{
double num2 = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
double num3 = 0.5 / num2;
result.X = (matrix.M31 + matrix.M13) * num3;
result.Y = (matrix.M32 + matrix.M23) * num3;
result.Z = 0.5 * num2;
result.W = (matrix.M12 - matrix.M21) * num3;
}
}
/// <summary>
/// Interpolates between two quaternions, using spherical linear interpolation.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value that indicates how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] Result of the interpolation.</param>
public static void Slerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num1 = amount;
double num2 = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
bool flag = false;
if (num2 < 0.0)
{
flag = true;
num2 = -num2;
}
double num3;
double num4;
if (num2 > 0.999998986721039)
{
num3 = 1.0 - num1;
num4 = flag ? -num1 : num1;
}
else
{
double num5 = Math.Acos(num2);
double num6 = (1.0 / Math.Sin(num5));
num3 = Math.Sin((1.0 - num1) * num5) * num6;
num4 = flag ? -Math.Sin(num1 * num5) * num6 : Math.Sin(num1 * num5) * num6;
}
result.X = (num3 * quaternion1.X + num4 * quaternion2.X);
result.Y = (num3 * quaternion1.Y + num4 * quaternion2.Y);
result.Z = (num3 * quaternion1.Z + num4 * quaternion2.Z);
result.W = (num3 * quaternion1.W + num4 * quaternion2.W);
}
/// <summary>
/// Calculates the dot product of two Quaternions.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">Source QuaternionD.</param><param name="result">[OutAttribute] Dot product of the Quaternions.</param>
public static void Dot(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out double result)
{
result = (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W);
}
/// <summary>
/// Linearly interpolates between two quaternions.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param><param name="result">[OutAttribute] The resulting quaternion.</param>
public static void Lerp(ref QuaternionD quaternion1, ref QuaternionD quaternion2, double amount, out QuaternionD result)
{
double num1 = amount;
double num2 = 1.0 - num1;
if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
{
result.X = (num2 * quaternion1.X + num1 * quaternion2.X);
result.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
result.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
result.W = (num2 * quaternion1.W + num1 * quaternion2.W);
}
else
{
result.X = (num2 * quaternion1.X - num1 * quaternion2.X);
result.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
result.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
result.W = (num2 * quaternion1.W - num1 * quaternion2.W);
}
double num3 = 1.0 / Math.Sqrt(result.X * result.X + result.Y * result.Y + result.Z * result.Z + result.W * result.W);
result.X *= num3;
result.Y *= num3;
result.Z *= num3;
result.W *= num3;
}
/// <summary>
/// Linearly interpolates between two quaternions.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="amount">Value indicating how far to interpolate between the quaternions.</param>
public static QuaternionD Lerp(QuaternionD quaternion1, QuaternionD quaternion2, double amount)
{
double num1 = amount;
double num2 = 1.0 - num1;
QuaternionD quaternion = new QuaternionD();
if (quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W >= 0.0)
{
quaternion.X = (num2 * quaternion1.X + num1 * quaternion2.X);
quaternion.Y = (num2 * quaternion1.Y + num1 * quaternion2.Y);
quaternion.Z = (num2 * quaternion1.Z + num1 * quaternion2.Z);
quaternion.W = (num2 * quaternion1.W + num1 * quaternion2.W);
}
else
{
quaternion.X = (num2 * quaternion1.X - num1 * quaternion2.X);
quaternion.Y = (num2 * quaternion1.Y - num1 * quaternion2.Y);
quaternion.Z = (num2 * quaternion1.Z - num1 * quaternion2.Z);
quaternion.W = (num2 * quaternion1.W - num1 * quaternion2.W);
}
double num3 = 1.0 / Math.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
quaternion.X *= num3;
quaternion.Y *= num3;
quaternion.Z *= num3;
quaternion.W *= num3;
return quaternion;
}
/// <summary>
/// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
/// </summary>
/// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param><param name="result">[OutAttribute] The QuaternionD rotation representing the concatenation of value1 followed by value2.</param>
public static void Concatenate(ref QuaternionD value1, ref QuaternionD value2, out QuaternionD result)
{
double num1 = value2.X;
double num2 = value2.Y;
double num3 = value2.Z;
double num4 = value2.W;
double num5 = value1.X;
double num6 = value1.Y;
double num7 = value1.Z;
double num8 = value1.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
result.X = (num1 * num8 + num5 * num4) + num9;
result.Y = (num2 * num8 + num6 * num4) + num10;
result.Z = (num3 * num8 + num7 * num4) + num11;
result.W = num4 * num8 - num12;
}
/// <summary>
/// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
/// </summary>
/// <param name="value1">The first QuaternionD rotation in the series.</param><param name="value2">The second QuaternionD rotation in the series.</param>
public static QuaternionD Concatenate(QuaternionD value1, QuaternionD value2)
{
double num1 = value2.X;
double num2 = value2.Y;
double num3 = value2.Z;
double num4 = value2.W;
double num5 = value1.X;
double num6 = value1.Y;
double num7 = value1.Z;
double num8 = value1.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
QuaternionD quaternion;
quaternion.X = (num1 * num8 + num5 * num4) + num9;
quaternion.Y = (num2 * num8 + num6 * num4) + num10;
quaternion.Z = (num3 * num8 + num7 * num4) + num11;
quaternion.W = num4 * num8 - num12;
return quaternion;
}
/// <summary>
/// Adds two Quaternions.
/// </summary>
/// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param><param name="result">[OutAttribute] Result of adding the Quaternions.</param>
public static void Add(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
result.X = quaternion1.X + quaternion2.X;
result.Y = quaternion1.Y + quaternion2.Y;
result.Z = quaternion1.Z + quaternion2.Z;
result.W = quaternion1.W + quaternion2.W;
}
/// <summary>
/// Flips the sign of each component of the quaternion.
/// </summary>
/// <param name="quaternion">Source quaternion.</param>
public static QuaternionD Negate(QuaternionD quaternion)
{
QuaternionD quaternion1;
quaternion1.X = -quaternion.X;
quaternion1.Y = -quaternion.Y;
quaternion1.Z = -quaternion.Z;
quaternion1.W = -quaternion.W;
return quaternion1;
}
/// <summary>
/// Subtracts a quaternion from another quaternion.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param><param name="result">[OutAttribute] Result of the subtraction.</param>
public static void Subtract(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
result.X = quaternion1.X - quaternion2.X;
result.Y = quaternion1.Y - quaternion2.Y;
result.Z = quaternion1.Z - quaternion2.Z;
result.W = quaternion1.W - quaternion2.W;
}
/// <summary>
/// Adds two Quaternions.
/// </summary>
/// <param name="quaternion1">QuaternionD to add.</param><param name="quaternion2">QuaternionD to add.</param>
public static QuaternionD Add(QuaternionD quaternion1, QuaternionD quaternion2)
{
QuaternionD quaternion;
quaternion.X = quaternion1.X + quaternion2.X;
quaternion.Y = quaternion1.Y + quaternion2.Y;
quaternion.Z = quaternion1.Z + quaternion2.Z;
quaternion.W = quaternion1.W + quaternion2.W;
return quaternion;
}
/// <summary>
/// Multiplies two quaternions.
/// </summary>
/// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param><param name="result">[OutAttribute] The result of the multiplication.</param>
public static void Multiply(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = quaternion2.X;
double num6 = quaternion2.Y;
double num7 = quaternion2.Z;
double num8 = quaternion2.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
result.X = (num1 * num8 + num5 * num4) + num9;
result.Y = (num2 * num8 + num6 * num4) + num10;
result.Z = (num3 * num8 + num7 * num4) + num11;
result.W = num4 * num8 - num12;
}
/// <summary>
/// Subtracts a quaternion from another quaternion.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="quaternion2">Source quaternion.</param>
public static QuaternionD Subtract(QuaternionD quaternion1, QuaternionD quaternion2)
{
QuaternionD quaternion;
quaternion.X = quaternion1.X - quaternion2.X;
quaternion.Y = quaternion1.Y - quaternion2.Y;
quaternion.Z = quaternion1.Z - quaternion2.Z;
quaternion.W = quaternion1.W - quaternion2.W;
return quaternion;
}
/// <summary>
/// Returns the conjugate of a specified QuaternionD.
/// </summary>
/// <param name="value">The QuaternionD of which to return the conjugate.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to be the conjugate of the specified one.</param>
public static void Conjugate(ref QuaternionD value, out QuaternionD result)
{
result.X = -value.X;
result.Y = -value.Y;
result.Z = -value.Z;
result.W = value.W;
}
/// <summary>
/// Multiplies two quaternions.
/// </summary>
/// <param name="quaternion1">The quaternion on the left of the multiplication.</param><param name="quaternion2">The quaternion on the right of the multiplication.</param>
public static QuaternionD Multiply(QuaternionD quaternion1, QuaternionD quaternion2)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = quaternion2.X;
double num6 = quaternion2.Y;
double num7 = quaternion2.Z;
double num8 = quaternion2.W;
double num9 = (num2 * num7 - num3 * num6);
double num10 = (num3 * num5 - num1 * num7);
double num11 = (num1 * num6 - num2 * num5);
double num12 = (num1 * num5 + num2 * num6 + num3 * num7);
QuaternionD quaternion;
quaternion.X = (num1 * num8 + num5 * num4) + num9;
quaternion.Y = (num2 * num8 + num6 * num4) + num10;
quaternion.Z = (num3 * num8 + num7 * num4) + num11;
quaternion.W = num4 * num8 - num12;
return quaternion;
}
/// <summary>
/// Returns the inverse of a QuaternionD.
/// </summary>
/// <param name="quaternion">Source QuaternionD.</param>
public static QuaternionD Inverse(QuaternionD quaternion)
{
double num = 1.0 / (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
QuaternionD quaternion1;
quaternion1.X = -quaternion.X * num;
quaternion1.Y = -quaternion.Y * num;
quaternion1.Z = -quaternion.Z * num;
quaternion1.W = quaternion.W * num;
return quaternion1;
}
/// <summary>
/// Multiplies a quaternion by a scalar value.
/// </summary>
/// <param name="quaternion1">Source quaternion.</param><param name="scaleFactor">Scalar value.</param>
public static QuaternionD Multiply(QuaternionD quaternion1, double scaleFactor)
{
QuaternionD quaternion;
quaternion.X = quaternion1.X * scaleFactor;
quaternion.Y = quaternion1.Y * scaleFactor;
quaternion.Z = quaternion1.Z * scaleFactor;
quaternion.W = quaternion1.W * scaleFactor;
return quaternion;
}
/// <summary>
/// Returns the inverse of a QuaternionD.
/// </summary>
/// <param name="quaternion">Source QuaternionD.</param><param name="result">[OutAttribute] The inverse of the QuaternionD.</param>
public static void Inverse(ref QuaternionD quaternion, out QuaternionD result)
{
double num = 1.0 / (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W);
result.X = -quaternion.X * num;
result.Y = -quaternion.Y * num;
result.Z = -quaternion.Z * num;
result.W = quaternion.W * num;
}
/// <summary>
/// Divides a QuaternionD by another QuaternionD.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param>
public static QuaternionD Divide(QuaternionD quaternion1, QuaternionD quaternion2)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
double num6 = -quaternion2.X * num5;
double num7 = -quaternion2.Y * num5;
double num8 = -quaternion2.Z * num5;
double num9 = quaternion2.W * num5;
double num10 = (num2 * num8 - num3 * num7);
double num11 = (num3 * num6 - num1 * num8);
double num12 = (num1 * num7 - num2 * num6);
double num13 = (num1 * num6 + num2 * num7 + num3 * num8);
QuaternionD quaternion;
quaternion.X = (num1 * num9 + num6 * num4) + num10;
quaternion.Y = (num2 * num9 + num7 * num4) + num11;
quaternion.Z = (num3 * num9 + num8 * num4) + num12;
quaternion.W = num4 * num9 - num13;
return quaternion;
}
/// <summary>
/// Creates a QuaternionD from a vector and an angle to rotate about the vector.
/// </summary>
/// <param name="axis">The vector to rotate around.</param><param name="angle">The angle to rotate around the vector.</param><param name="result">[OutAttribute] The created QuaternionD.</param>
public static void CreateFromAxisAngle(ref Vector3D axis, double angle, out QuaternionD result)
{
double num1 = angle * 0.5;
double num2 = Math.Sin(num1);
double num3 = Math.Cos(num1);
result.X = axis.X * num2;
result.Y = axis.Y * num2;
result.Z = axis.Z * num2;
result.W = num3;
}
/// <summary>
/// Divides a QuaternionD by another QuaternionD.
/// </summary>
/// <param name="quaternion1">Source QuaternionD.</param><param name="quaternion2">The divisor.</param><param name="result">[OutAttribute] Result of the division.</param>
public static void Divide(ref QuaternionD quaternion1, ref QuaternionD quaternion2, out QuaternionD result)
{
double num1 = quaternion1.X;
double num2 = quaternion1.Y;
double num3 = quaternion1.Z;
double num4 = quaternion1.W;
double num5 = 1.0 / (quaternion2.X * quaternion2.X + quaternion2.Y * quaternion2.Y + quaternion2.Z * quaternion2.Z + quaternion2.W * quaternion2.W);
double num6 = -quaternion2.X * num5;
double num7 = -quaternion2.Y * num5;
double num8 = -quaternion2.Z * num5;
double num9 = quaternion2.W * num5;
double num10 = (num2 * num8 - num3 * num7);
double num11 = (num3 * num6 - num1 * num8);
double num12 = (num1 * num7 - num2 * num6);
double num13 = (num1 * num6 + num2 * num7 + num3 * num8);
result.X = (num1 * num9 + num6 * num4) + num10;
result.Y = (num2 * num9 + num7 * num4) + num11;
result.Z = (num3 * num9 + num8 * num4) + num12;
result.W = num4 * num9 - num13;
}
/// <summary>
/// Creates a new QuaternionD from specified yaw, pitch, and roll angles.
/// </summary>
/// <param name="yaw">The yaw angle, in radians, around the y-axis.</param><param name="pitch">The pitch angle, in radians, around the x-axis.</param><param name="roll">The roll angle, in radians, around the z-axis.</param><param name="result">[OutAttribute] An existing QuaternionD filled in to express the specified yaw, pitch, and roll angles.</param>
public static void CreateFromYawPitchRoll(double yaw, double pitch, double roll, out QuaternionD result)
{
double num1 = roll * 0.5;
double num2 = Math.Sin(num1);
double num3 = Math.Cos(num1);
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);
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);
}
private void UpdateViewRange(MyPlanet planet)
{
var pos = MySector.MainCamera.Position;
double dist = double.MaxValue;
foreach (var p in MyPlanets.GetPlanets())
{
double dsq = Vector3D.DistanceSquared(pos, p.WorldMatrix.Translation);
if (dsq < dist)
{
planet = p;
dist = dsq;
}
}
var radius = planet.MinimumRadius;
double altitude;
m_height = planet.MaximumRadius - radius;
var center = planet.WorldMatrix.Translation;
m_radius = HyperSphereHelpers.DistanceToTangentProjected(ref center, ref pos, radius, out altitude);
var up = center - pos;
up.Normalize();
m_center = pos + up * altitude;
var forward = Vector3D.CalculatePerpendicularVector(up);
m_orientation = QuaternionD.CreateFromForwardUp(forward, up);
}
/// <summary>
/// Works for normalized vectors only
/// </summary>
public static QuaternionD CreateFromForwardUp(Vector3D forward, Vector3D up)
{
Vector3D vector = -forward;
Vector3D vector2 = Vector3D.Cross(up, vector);
Vector3D vector3 = Vector3D.Cross(vector, vector2);
var m00 = vector2.X;
var m01 = vector2.Y;
var m02 = vector2.Z;
var m10 = vector3.X;
var m11 = vector3.Y;
var m12 = vector3.Z;
var m20 = vector.X;
var m21 = vector.Y;
var m22 = vector.Z;
double num8 = (m00 + m11) + m22;
var quaternion = new QuaternionD();
if (num8 > 0)
{
var num = Math.Sqrt(num8 + 1.0);
quaternion.W = num * 0.5;
num = 0.5 / num;
quaternion.X = (m12 - m21) * num;
quaternion.Y = (m20 - m02) * num;
quaternion.Z = (m01 - m10) * num;
return quaternion;
}
if ((m00 >= m11) && (m00 >= m22))
{
var num7 = Math.Sqrt(((1.0 + m00) - m11) - m22);
var num4 = 0.5 / num7;
quaternion.X = 0.5 * num7;
quaternion.Y = (m01 + m10) * num4;
quaternion.Z = (m02 + m20) * num4;
quaternion.W = (m12 - m21) * num4;
return quaternion;
}
if (m11 > m22)
{
var num6 = Math.Sqrt(((1.0 + m11) - m00) - m22);
var num3 = 0.5 / num6;
quaternion.X = (m10 + m01) * num3;
quaternion.Y = 0.5 * num6;
quaternion.Z = (m21 + m12) * num3;
quaternion.W = (m20 - m02) * num3;
return quaternion;
}
var num5 = Math.Sqrt(((1.0 + m22) - m00) - m11);
var num2 = 0.5 / num5;
quaternion.X = (m20 + m02) * num2;
quaternion.Y = (m21 + m12) * num2;
quaternion.Z = 0.5 * num5;
quaternion.W = (m01 - m10) * num2;
return quaternion;
}
public static Vector3D Transform(this Vector3D value, QuaternionD rotation)
{
double num = (double)(rotation.X + rotation.X);
double num2 = (double)(rotation.Y + rotation.Y);
double num3 = (double)(rotation.Z + rotation.Z);
double num4 = (double)rotation.W * num;
double num5 = (double)rotation.W * num2;
double num6 = (double)rotation.W * num3;
double num7 = (double)rotation.X * num;
double num8 = (double)rotation.X * num2;
double num9 = (double)rotation.X * num3;
double num10 = (double)rotation.Y * num2;
double num11 = (double)rotation.Y * num3;
double num12 = (double)rotation.Z * num3;
double x = value.X * (1.0 - num10 - num12) + value.Y * (num8 - num6) + value.Z * (num9 + num5);
double y = value.X * (num8 + num6) + value.Y * (1.0 - num7 - num12) + value.Z * (num11 - num4);
double z = value.X * (num9 - num5) + value.Y * (num11 + num4) + value.Z * (1.0 - num7 - num10);
Vector3D result;
result.X = x;
result.Y = y;
result.Z = z;
return result;
}
public static bool IsEqual(QuaternionD value1, QuaternionD value2) => (((IsZero((double)(value1.X - value2.X), 1E-05f) && IsZero((double)(value1.Y - value2.Y), 1E-05f)) && IsZero((double)(value1.Z - value2.Z), 1E-05f)) && IsZero((double)(value1.W - value2.W), 1E-05f));
