/// <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 }); }