/// <summary>
/// Quaternions are subtracted.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The difference of both quaternions.</param>
public static void Subtract(ref FixQuaternion quaternion1, ref FixQuaternion quaternion2, out FixQuaternion 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>
/// Scale a quaternion
/// </summary>
/// <param name="quaternion1">The quaternion to scale.</param>
/// <param name="scaleFactor">Scale factor.</param>
/// <param name="result">The scaled quaternion.</param>
public static void Multiply(ref FixQuaternion quaternion1, Fix64 scaleFactor, out FixQuaternion result)
{
result.x = quaternion1.x * scaleFactor;
result.y = quaternion1.y * scaleFactor;
result.z = quaternion1.z * scaleFactor;
result.w = quaternion1.w * scaleFactor;
}
/// <summary>
/// Quaternions are added.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The sum of both quaternions.</param>
public static void Add(ref FixQuaternion quaternion1, ref FixQuaternion quaternion2, out FixQuaternion 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 FixQuaternion(FixQuaternion v)
{
this.x = v.x;
this.y = v.y;
this.z = v.z;
this.w = v.w;
}
/// <summary>
/// Creates a JMatrix representing an orientation from a quaternion.
/// </summary>
/// <param name="quaternion">The quaternion the matrix should be created from.</param>
/// <param name="result">JMatrix representing an orientation.</param>
public static void Rotate(ref FixQuaternion quaternion, out FixMatrix4x4 result)
{
// Precalculate coordinate products
Fix64 x = quaternion.x * 2;
Fix64 y = quaternion.y * 2;
Fix64 z = quaternion.z * 2;
Fix64 xx = quaternion.x * x;
Fix64 yy = quaternion.y * y;
Fix64 zz = quaternion.z * z;
Fix64 xy = quaternion.x * y;
Fix64 xz = quaternion.x * z;
Fix64 yz = quaternion.y * z;
Fix64 wx = quaternion.w * x;
Fix64 wy = quaternion.w * y;
Fix64 wz = quaternion.w * z;
// Calculate 3x3 matrix from orthonormal basis
result.M11 = Fix64.One - (yy + zz);
result.M21 = xy + wz;
result.M31 = xz - wy;
result.M41 = Fix64.Zero;
result.M12 = xy - wz;
result.M22 = Fix64.One - (xx + zz);
result.M32 = yz + wx;
result.M42 = Fix64.Zero;
result.M13 = xz + wy;
result.M23 = yz - wx;
result.M33 = Fix64.One - (xx + yy);
result.M43 = Fix64.Zero;
result.M14 = Fix64.Zero;
result.M24 = Fix64.Zero;
result.M34 = Fix64.Zero;
result.M44 = Fix64.One;
}
/// <summary>
/// Converts a quaternion to a euler angle.
/// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
/// </summary>
/// <param name="quaternion">The quaternion to convert from.</param>
/// <returns>An euler angle.</returns>
public static FixVec3 ToEuler(FixQuaternion quaternion)
{
FixVec3 result;
Fix t0 = 2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y);
Fix t1 = Fix.one - (2 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z));
result.z = FixMath.Atan2(t0, t1);
Fix t2 = 2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x);
if (t2 >= Fix.one)
{
result.y = FixMath.PI / 2;
}
else if (t2 <= -Fix.one)
{
result.y = -(FixMath.PI / 2);
}
else
{
result.y = FixMath.Asin(t2);
}
Fix t3 = 2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z);
Fix t4 = Fix.one - (2 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y));
result.x = FixMath.Atan2(t3, t4);
return(result);
}
public static FixQuaternion LerpUnclamped(FixQuaternion a, FixQuaternion b, Fix64 t)
{
FixQuaternion result = FixQuaternion.Multiply(a, (1 - t)) + FixQuaternion.Multiply(b, t);
result.Normalize();
return(result);
}
public static FixQuaternion FromToRotation(FixVector3 fromVector, FixVector3 toVector)
{
FixVector3 w = FixVector3.Cross(fromVector, toVector);
FixQuaternion q = new FixQuaternion(w.x, w.y, w.z, FixVector3.Dot(fromVector, toVector));
q.w += Fix64.Sqrt(fromVector.SqrMagnitude * toVector.SqrMagnitude);
q.Normalize();
return(q);
}
public static FixQuaternion Conjugate(FixQuaternion value)
{
FixQuaternion quaternion;
quaternion.x = -value.x;
quaternion.y = -value.y;
quaternion.z = -value.z;
quaternion.w = value.w;
return(quaternion);
}
public static FixQuaternion Euler(Fix64 x, Fix64 y, Fix64 z)
{
x *= Fix64.Deg2Rad;
y *= Fix64.Deg2Rad;
z *= Fix64.Deg2Rad;
FixQuaternion.CreateFromYawPitchRoll(y, x, z, out FixQuaternion rotation);
return(rotation);
}
public void EulerForwardBack(int x, int y, int z)
{
FixQuaternion quat = new FixQuaternion(new FixVec3(x, y, z));
FixVec3 fv = FixQuaternion.ToEuler(quat);
FixQuaternion final = new FixQuaternion(fv);
Assert.AreEqual(final.x.raw, quat.x.raw);
Assert.AreEqual(final.y.raw, quat.y.raw);
Assert.AreEqual(final.z.raw, quat.z.raw);
Assert.AreEqual(final.w.raw, quat.w.raw);
}
public static Fix64 Angle(FixQuaternion a, FixQuaternion b)
{
FixQuaternion aInv = FixQuaternion.Inverse(a);
FixQuaternion f = b * aInv;
Fix64 angle = Fix64.Acos(f.w) * 2 * Fix64.Rad2Deg;
if (angle > 180)
{
angle = 360 - angle;
}
return(angle);
}
public static FixQuaternion Slerp(FixQuaternion from, FixQuaternion to, Fix64 t)
{
t = FixMath.Clamp(t, 0, 1);
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
return(Multiply(Multiply(from, Fix64.Sin((1 - t) * halfTheta)) + Multiply(to, Fix64.Sin(t * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
public static void CreateFromYawPitchRoll(Fix64 yaw, Fix64 pitch, Fix64 roll, out FixQuaternion result)
{
Fix64 num9 = roll * Fix64.Half;
Fix64 num6 = Fix64.Sin(num9);
Fix64 num5 = Fix64.Cos(num9);
Fix64 num8 = pitch * Fix64.Half;
Fix64 num4 = Fix64.Sin(num8);
Fix64 num3 = Fix64.Cos(num8);
Fix64 num7 = yaw * Fix64.Half;
Fix64 num2 = Fix64.Sin(num7);
Fix64 num = Fix64.Cos(num7);
result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The product of both quaternions.</param>
public static void Multiply(ref FixQuaternion quaternion1, ref FixQuaternion quaternion2, out FixQuaternion result)
{
Fix64 x = quaternion1.x;
Fix64 y = quaternion1.y;
Fix64 z = quaternion1.z;
Fix64 w = quaternion1.w;
Fix64 num4 = quaternion2.x;
Fix64 num3 = quaternion2.y;
Fix64 num2 = quaternion2.z;
Fix64 num = quaternion2.w;
Fix64 num12 = (y * num2) - (z * num3);
Fix64 num11 = (z * num4) - (x * num2);
Fix64 num10 = (x * num3) - (y * num4);
Fix64 num9 = ((x * num4) + (y * num3)) + (z * num2);
result.x = ((x * num) + (num4 * w)) + num12;
result.y = ((y * num) + (num3 * w)) + num11;
result.z = ((z * num) + (num2 * w)) + num10;
result.w = (w * num) - num9;
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <param name="result">JQuaternion representing an orientation.</param>
public static void CreateFromMatrix(ref FixMatrix matrix, out FixQuaternion result)
{
Fix64 num8 = (matrix.M11 + matrix.M22) + matrix.M33;
if (num8 > Fix64.Zero)
{
Fix64 num = Fix64.Sqrt((num8 + Fix64.One));
result.w = num * Fix64.Half;
num = Fix64.Half / num;
result.x = (matrix.M23 - matrix.M32) * num;
result.y = (matrix.M31 - matrix.M13) * num;
result.z = (matrix.M12 - matrix.M21) * num;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
Fix64 num7 = Fix64.Sqrt((((Fix64.One + matrix.M11) - matrix.M22) - matrix.M33));
Fix64 num4 = Fix64.Half / num7;
result.x = Fix64.Half * num7;
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)
{
Fix64 num6 = Fix64.Sqrt((((Fix64.One + matrix.M22) - matrix.M11) - matrix.M33));
Fix64 num3 = Fix64.Half / num6;
result.x = (matrix.M21 + matrix.M12) * num3;
result.y = Fix64.Half * num6;
result.z = (matrix.M32 + matrix.M23) * num3;
result.w = (matrix.M31 - matrix.M13) * num3;
}
else
{
Fix64 num5 = Fix64.Sqrt((((Fix64.One + matrix.M33) - matrix.M11) - matrix.M22));
Fix64 num2 = Fix64.Half / num5;
result.x = (matrix.M31 + matrix.M13) * num2;
result.y = (matrix.M32 + matrix.M23) * num2;
result.z = Fix64.Half * num5;
result.w = (matrix.M12 - matrix.M21) * num2;
}
}
/// <summary>
/// Creates a JMatrix representing an orientation from a quaternion.
/// </summary>
/// <param name="quaternion">The quaternion the matrix should be created from.</param>
/// <param name="result">JMatrix representing an orientation.</param>
public static void CreateFromQuaternion(ref FixQuaternion quaternion, out FixMatrix result)
{
Fix64 num9 = quaternion.x * quaternion.x;
Fix64 num8 = quaternion.y * quaternion.y;
Fix64 num7 = quaternion.z * quaternion.z;
Fix64 num6 = quaternion.x * quaternion.y;
Fix64 num5 = quaternion.z * quaternion.w;
Fix64 num4 = quaternion.z * quaternion.x;
Fix64 num3 = quaternion.y * quaternion.w;
Fix64 num2 = quaternion.y * quaternion.z;
Fix64 num = quaternion.x * quaternion.w;
result.M11 = Fix64.One - (2 * (num8 + num7));
result.M12 = 2 * (num6 + num5);
result.M13 = 2 * (num4 - num3);
result.M21 = 2 * (num6 - num5);
result.M22 = Fix64.One - (2 * (num7 + num9));
result.M23 = 2 * (num2 + num);
result.M31 = 2 * (num4 + num3);
result.M32 = 2 * (num2 - num);
result.M33 = Fix64.One - (2 * (num8 + num9));
}
public static FixQuaternion RotateTowards(FixQuaternion from, FixQuaternion to, Fix64 maxDegreesDelta)
{
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
Fix64 theta = halfTheta * 2;
maxDegreesDelta *= Fix64.Deg2Rad;
if (maxDegreesDelta >= theta)
{
return(to);
}
maxDegreesDelta /= theta;
return(Multiply(Multiply(from, Fix64.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, Fix64.Sin(maxDegreesDelta * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
public static void TRS(FixVector3 translation, FixQuaternion rotation, FixVector3 scale, out FixMatrix4x4 matrix)
{
matrix = FixMatrix4x4.Translate(translation) * FixMatrix4x4.Rotate(rotation) * FixMatrix4x4.Scale(scale);
}
public static FixMatrix CreateFromYawPitchRoll(Fix64 yaw, Fix64 pitch, Fix64 roll)
{
FixQuaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out FixQuaternion quaternion);
CreateFromQuaternion(ref quaternion, out FixMatrix matrix);
return(matrix);
}
public static FixMatrix CreateFromQuaternion(FixQuaternion quaternion)
{
FixMatrix.CreateFromQuaternion(ref quaternion, out FixMatrix result);
return(result);
}
public static FixQuaternion Lerp(FixQuaternion a, FixQuaternion b, Fix64 t)
{
t = FixMath.Clamp(t, Fix64.Zero, Fix64.One);
return(LerpUnclamped(a, b, t));
}
static FixQuaternion()
{
identity = new FixQuaternion(0, 0, 0, 1);
}
public static FixQuaternion Inverse(FixQuaternion rotation)
{
Fix64 invNorm = Fix64.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));
return(FixQuaternion.Multiply(FixQuaternion.Conjugate(rotation), invNorm));
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The product of both quaternions.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
public static FixQuaternion Multiply(FixQuaternion quaternion1, FixQuaternion quaternion2)
{
FixQuaternion.Multiply(ref quaternion1, ref quaternion2, out FixQuaternion result);
return(result);
}
public static FixMatrix4x4 TRS(FixVector3 translation, FixQuaternion rotation, FixVector3 scale)
{
TRS(translation, rotation, scale, out FixMatrix4x4 result);
return(result);
}
public static FixMatrix4x4 Rotate(FixQuaternion quaternion)
{
FixMatrix4x4.Rotate(ref quaternion, out FixMatrix4x4 result);
return(result);
}
public void SetFromToRotation(FixVector3 fromDirection, FixVector3 toDirection)
{
FixQuaternion targetRotation = FixQuaternion.FromToRotation(fromDirection, toDirection);
this.Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
}
/// <summary>
/// Scale a quaternion
/// </summary>
/// <param name="quaternion1">The quaternion to scale.</param>
/// <param name="scaleFactor">Scale factor.</param>
/// <returns>The scaled quaternion.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, Fix64 scaleFactor)
public static FixQuaternion Multiply(FixQuaternion quaternion1, Fix64 scaleFactor)
{
FixQuaternion.Multiply(ref quaternion1, scaleFactor, out FixQuaternion result);
return(result);
}
/// <summary>
/// Quaternions are subtracted.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The difference of both quaternions.</returns>
#region public static JQuaternion Subtract(JQuaternion quaternion1, JQuaternion quaternion2)
public static FixQuaternion Subtract(FixQuaternion quaternion1, FixQuaternion quaternion2)
{
FixQuaternion.Subtract(ref quaternion1, ref quaternion2, out FixQuaternion result);
return(result);
}
