public static Vector3 ToEuler(Quaternion q)
{
Vector3 angles = new Vector3();
// roll (X-axis rotation)
float sr_cp = 2.0f * (q.W * q.X + q.Y * q.Z);
float cr_cp = 1.0f - 2.0f * (q.X * q.X + q.Y * q.Y);
angles.Z = MathF.Atan2(sr_cp, cr_cp);
// pitch (Y-axis rotation)
float sp = 2.0f * (q.W * q.Y - q.Z * q.X);
if (MathF.Abs(sp) >= 1.0f)
{
angles.X = MathF.CopySign(GameSettings.PI_HALF, sp); // use 90 degrees if out of range
}
else
{
angles.X = MathF.Asin(sp);
}
// yaw (Z-axis rotation)
float sy_cp = 2.0f * (q.W * q.Z + q.X * q.Y);
float cy_cp = 1.0f - 2 * (q.Y * q.Y + q.Z * q.Z);
angles.Y = MathF.Atan2(sy_cp, cy_cp);
return(angles);
}
/// <summary>
/// This method creates an orthonormal base given a <see cref="Vec"/> object.
/// The given Vec is normalized and it counts as the "z" element of the base.
/// The other two memebers of the base are calculated via cross product.
/// </summary>
/// <returns> A list of <see cref="Vec"/> objects</returns>
public List<Vec> createONBfromZ()
{
Vec e3 = this.Normalize();
float sign = MathF.CopySign(1f, e3.z);
float a = -1.0f / (sign + e3.z);
float b = e3.x * e3.y * a;
Vec e1 = new Vec(1.0f + sign * e3.x * e3.x * a, sign * b, -sign * e3.x);
Vec e2 = new Vec(b, sign + e3.y * e3.y * a, -e3.y);
return new List<Vec>() { e1, e2, e3 };
}
public static void CopySignTest()
{
float result = 1.0f, value = -1.0f;
for (int iteration = 0; iteration < MathTests.Iterations; iteration++)
{
value += copySignDelta;
result += MathF.CopySign(result, value);
}
if (result != copySignExpectedResult)
{
throw new Exception($"Expected Result {copySignExpectedResult}; Actual Result {result}");
}
}
public static float MakeSameSignAs(this float number, float sign)
{
return(MathF.CopySign(number, sign));
}
public float Reverse(ref float value) => MathF.CopySign(1.0f, value);
/// <inheritdoc cref="MathF.CopySign(float,float)"/> public static float CopySign(this float value, float value2) => MathF.CopySign(value, value2);
