/// <summary>
/// Performs a linear interpolation between two quaternions
/// </summary>
/// <param name="q1">Start quaternion</param>
/// <param name="q2">End quaternion</param>
/// <param name="t">Value between 0 and 1 indicating the weight of the second quaternion</param>
/// <returns>Result of the interpolation, {w, x, y, z}</returns>
public static float[] Lerp(float[] q1, float[] q2, float t)
{
float num2 = 1f - t;
float[] res = new float[4];
float num5 = (((q1[1] * q2[1]) + (q1[2] * q2[2])) + (q1[3] * q2[3])) + (q1[0] * q2[0]);
if (num5 >= 0f)
{
res[1] = (num2 * q1[1]) + (t * q2[1]);
res[2] = (num2 * q1[2]) + (t * q2[2]);
res[3] = (num2 * q1[3]) + (t * q2[3]);
res[0] = (num2 * q1[0]) + (t * q2[0]);
}
else
{
res[1] = (num2 * q1[1]) - (t * q2[1]);
res[2] = (num2 * q1[2]) - (t * q2[2]);
res[3] = (num2 * q1[3]) - (t * q2[3]);
res[0] = (num2 * q1[0]) - (t * q2[0]);
}
return(QuatUtils.Normalize(res));
}
/// <summary>
/// Conjugates and renormalizes the quaternion
/// </summary>
/// <param name="q">The quaternion to conjugate and renormalize, {w, x, y, z}</param>
/// <returns>The conjugated and renormalized quatenion, {w, x, y, z}</returns>
public static float[] Inverse(float[] q)
{
float[] res = new float[] { q[0], -q[1], -q[2], -q[3] };
float m = QuatUtils.Magnitude(q);
return(new float[] { res[0] / m, res[1] / m, res[2] / m, res[3] / m });
}
/// <summary>
/// Returns corrected quaternion with applied sensitivity, angles limits and removes rotation around Y-axis
/// </summary>
/// <returns>Corrected quaternion</returns>
/// <param name="quat">The quaternion to correct</param>
public override Quaternion UpdateQuat(Quaternion quat)
{
Vector2 tilt_el = TiltElevationConverter.Convert(quat) * Mathf.Deg2Rad;
float[] res = QuatUtils.FromYawPitchRoll(
-clamp(tilt_el.y, axis2),
0f,
-clamp(tilt_el.x, axis1));
return(new Quaternion(res[1], res[2], res[3], res[0]));
}
private static float[] Convert(float[] quat, CoordSpace coordSpace)
{
float[] res = new float[2];
if (coordSpace == CoordSpace.World)
{
quat = QuatUtils.Inverse(quat);
}
float[] forward = QuatUtils.TransformVecLH(quat, new float[] { 0f, 0f, 1f });
float lXZ = (float)Math.Sqrt(forward[0] * forward[0] + forward[2] * forward[2]);
//Azimuth
if (lXZ < 0.00001f)
{
res[0] = (float)Math.Sign(forward[0]) * (float)Math.PI;
}
else
{
res[0] = (float)Math.Asin((double)forward[0] / (double)lXZ);
}
if (forward[2] < 0)
{
res[0] = (float)Math.Sign(res[0]) * (float)Math.PI - res[0];
}
//Elevation
if (lXZ < 0.00001f)
{
res[0] = (float)(Math.PI * Math.Sign(forward[1]));
}
else
{
res[1] = (float)Math.Atan(forward[1] / lXZ);
}
if (coordSpace == CoordSpace.World)
{
res[0] = -res[0];
res[0] = -res[1];
}
//Convert radians to degrees
res[0] *= Mathf.Rad2Deg;
res[1] *= Mathf.Rad2Deg;
return(res);
}
private static float[] Convert(float[] quat, CoordSpace coordSpace)
{
float[] res = new float[2];
if (coordSpace == CoordSpace.World)
{
quat = QuatUtils.Inverse(quat);
}
float[] forward = QuatUtils.TransformVecLH(quat, new float[] { 0f, 0f, 1f });
float[] right = QuatUtils.TransformVecLH(quat, new float[] { 1f, 0f, 0f });
float lf_XZ = (float)Math.Sqrt(forward[0] * forward[0] + forward[2] * forward[2]);
float lr_XZ = (float)Math.Sqrt(right[0] * right[0] + right[2] * right[2]);
//Tilt
if (lr_XZ < 0.00001f)
{
res[0] = -(float)(Math.PI * Math.Sign(right[1]));
}
else
{
res[0] = -(float)Math.Atan(right[1] / lr_XZ);
}
//Elevation
if (lf_XZ < 0.00001f)
{
res[0] = (float)(Math.PI * Math.Sign(forward[1]));
}
else
{
res[1] = (float)Math.Atan(forward[1] / lf_XZ);
}
if (coordSpace == CoordSpace.World)
{
res[0] = -res[0];
res[0] = -res[1];
}
//Convert radians to degrees
res[0] *= Mathf.Rad2Deg;
res[1] *= Mathf.Rad2Deg;
return(res);
}
/// <summary>
/// Converts a given quaternion into a unit quaternion
/// </summary>
/// <param name="q">The quaternion to convert, {w, x, y, z}</param>
/// <returns>The converted quaternion, {w, x, y, z}</returns>
public static float[] Normalize(float[] q)
{
float m = QuatUtils.MagnitudeSquared(q);
return(new float[] { q[0] / m, q[1] / m, q[2] / m, q[3] / m });
}
