public void EulerToQuaternion_QuaternionToEuler_ResultingOrientationIsCloseToOriginal(
[Values] math.RotationOrder rotationOrder,
[Values(-90f, -45, 0f, 45, 90f)] float x,
[Values(-90f, -45, 0f, 45, 90f)] float y,
[Values(-90f, -45, 0f, 45, 90f)] float z
)
{
var inputEulerAngles = new EulerAngles {
RotationOrder = rotationOrder, Value = new float3(x, y, z)
};
var inputQuaternion = (quaternion)inputEulerAngles;
Assume.That(math.abs(math.length(inputQuaternion.value)), Is.EqualTo(1.0f).Within(1e-05));
EulerAngles outputEulerAngles = new EulerAngles {
RotationOrder = inputEulerAngles.RotationOrder
};
outputEulerAngles.SetValue(inputQuaternion);
quaternion outputQuaternion = (quaternion)outputEulerAngles;
Assume.That(math.abs(math.length(outputQuaternion.value)), Is.EqualTo(1.0f).Within(1e-05));
Assert.That(outputQuaternion, Is.OrientedEquivalentTo(inputQuaternion));
}
public void SetValue_WhenRotationOrder_ReturnsExpectedValue(
math.RotationOrder rotationOrder, quaternion value, float3 expectedEulerAngles
)
{
var eulerAngles = new EulerAngles {
RotationOrder = rotationOrder
};
eulerAngles.SetValue(value);
Assert.That(eulerAngles.Value, Is.PrettyCloseTo(expectedEulerAngles));
}
static float3 ToEulerAngles(quaternion q, math.RotationOrder order)
{
var iOrder = (int)order;
var rotationMatrix = new float3x3(math.normalizesafe(q));
var pitchAngle = math.asin(math.clamp(
k_PitchScalars[iOrder] * rotationMatrix[k_YawAxes[iOrder]][k_RollAxes[iOrder]], -1f, 1f
));
float rollAngle, yawAngle;
const float kPiOver2 = math.PI * 0.5f;
if (pitchAngle < kPiOver2)
{
if (pitchAngle > -kPiOver2)
{
yawAngle = math.atan2(
-k_PitchScalars[iOrder] * rotationMatrix[k_PitchAxes[iOrder]][k_RollAxes[iOrder]],
rotationMatrix[k_RollAxes[iOrder]][k_RollAxes[iOrder]]
);
rollAngle = math.atan2(
-k_PitchScalars[iOrder] * rotationMatrix[k_YawAxes[iOrder]][k_PitchAxes[iOrder]],
rotationMatrix[k_YawAxes[iOrder]][k_YawAxes[iOrder]]
);
}
else // non-unique solution
{
rollAngle = 0f;
yawAngle = rollAngle - math.atan2(
k_PitchScalars[iOrder] * rotationMatrix[k_PitchAxes[iOrder]][k_YawAxes[iOrder]],
rotationMatrix[k_PitchAxes[iOrder]][k_PitchAxes[iOrder]]
);
}
}
else // non-unique solution
{
rollAngle = 0f;
yawAngle = math.atan2(
k_PitchScalars[iOrder] * rotationMatrix[k_PitchAxes[iOrder]][k_YawAxes[iOrder]],
rotationMatrix[k_PitchAxes[iOrder]][k_PitchAxes[iOrder]]
) - rollAngle;
}
// pack the angles into a vector and return the result
return(math.degrees(new float3
{
[k_RollAxes[iOrder]] = rollAngle,
[k_YawAxes[iOrder]] = yawAngle,
[k_PitchAxes[iOrder]] = pitchAngle
}));
}
internal static float3 eulerReorderBack(float3 euler, math.RotationOrder order)
{
switch (order)
{
case math.RotationOrder.XZY:
return(euler.xzy);
case math.RotationOrder.YZX:
return(euler.zxy);
case math.RotationOrder.YXZ:
return(euler.yxz);
case math.RotationOrder.ZXY:
return(euler.yzx);
case math.RotationOrder.ZYX:
return(euler.zyx);
case math.RotationOrder.XYZ:
default:
return(euler);
}
}
public static float3 toEuler(quaternion q, math.RotationOrder order = math.RotationOrder.Default)
{
const float epsilon = 1e-6f;
//prepare the data
var qv = q.value;
var d1 = qv * qv.wwww * new float4(2.0f); //xw, yw, zw, ww
var d2 = qv * qv.yzxw * new float4(2.0f); //xy, yz, zx, ww
var d3 = qv * qv;
var euler = new float3(0.0f);
const float CUTOFF = (1.0f - 2.0f * epsilon) * (1.0f - 2.0f * epsilon);
switch (order)
{
case math.RotationOrder.ZYX:
{
var y1 = d2.z + d1.y;
if (y1 * y1 < CUTOFF)
{
var x1 = -d2.x + d1.z;
var x2 = d3.x + d3.w - d3.y - d3.z;
var z1 = -d2.y + d1.x;
var z2 = d3.z + d3.w - d3.y - d3.x;
euler = new float3(math.atan2(x1, x2), math.asin(y1), math.atan2(z1, z2));
}
else //zxz
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.z, d1.y, d2.y, d1.x);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), math.asin(y1), 0.0f);
}
break;
}
case math.RotationOrder.ZXY:
{
var y1 = d2.y - d1.x;
if (y1 * y1 < CUTOFF)
{
var x1 = d2.x + d1.z;
var x2 = d3.y + d3.w - d3.x - d3.z;
var z1 = d2.z + d1.y;
var z2 = d3.z + d3.w - d3.x - d3.y;
euler = new float3(math.atan2(x1, x2), -math.asin(y1), math.atan2(z1, z2));
}
else //zxz
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.z, d1.y, d2.y, d1.x);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), -math.asin(y1), 0.0f);
}
break;
}
case math.RotationOrder.YXZ:
{
var y1 = d2.y + d1.x;
if (y1 * y1 < CUTOFF)
{
var x1 = -d2.z + d1.y;
var x2 = d3.z + d3.w - d3.x - d3.y;
var z1 = -d2.x + d1.z;
var z2 = d3.y + d3.w - d3.z - d3.x;
euler = new float3(math.atan2(x1, x2), math.asin(y1), math.atan2(z1, z2));
}
else //yzy
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.x, d1.z, d2.y, d1.x);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), math.asin(y1), 0.0f);
}
break;
}
case math.RotationOrder.YZX:
{
var y1 = d2.x - d1.z;
if (y1 * y1 < CUTOFF)
{
var x1 = d2.z + d1.y;
var x2 = d3.x + d3.w - d3.z - d3.y;
var z1 = d2.y + d1.x;
var z2 = d3.y + d3.w - d3.x - d3.z;
euler = new float3(math.atan2(x1, x2), -math.asin(y1), math.atan2(z1, z2));
}
else //yxy
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.x, d1.z, d2.y, d1.x);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), -math.asin(y1), 0.0f);
}
break;
}
case math.RotationOrder.XZY:
{
var y1 = d2.x + d1.z;
if (y1 * y1 < CUTOFF)
{
var x1 = -d2.y + d1.x;
var x2 = d3.y + d3.w - d3.z - d3.x;
var z1 = -d2.z + d1.y;
var z2 = d3.x + d3.w - d3.y - d3.z;
euler = new float3(math.atan2(x1, x2), math.asin(y1), math.atan2(z1, z2));
}
else //xyx
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.x, d1.z, d2.z, d1.y);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), math.asin(y1), 0.0f);
}
break;
}
case math.RotationOrder.XYZ:
{
var y1 = d2.z - d1.y;
if (y1 * y1 < CUTOFF)
{
var x1 = d2.y + d1.x;
var x2 = d3.z + d3.w - d3.y - d3.x;
var z1 = d2.x + d1.z;
var z2 = d3.x + d3.w - d3.y - d3.z;
euler = new float3(math.atan2(x1, x2), -math.asin(y1), math.atan2(z1, z2));
}
else //xzx
{
y1 = math.clamp(y1, -1.0f, 1.0f);
var abcd = new float4(d2.z, d1.y, d2.x, d1.z);
var x1 = 2.0f * (abcd.x * abcd.w + abcd.y * abcd.z); //2(ad+bc)
var x2 = math.csum(abcd * abcd * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
euler = new float3(math.atan2(x1, x2), -math.asin(y1), 0.0f);
}
break;
}
}
return(eulerReorderBack(euler, order));
}
internal static float3 ToEulerAngles(this quaternion q, math.RotationOrder order = math.RotationOrder.XYZ)
{
return(toEuler(q, order));
}
