public static Vector3 ExtractEulerZXY(Matrix4x4 mat, out EulerResult eulerRes)
{
Vector3 res = new Vector3();
// +- -+ +- -+
// | r00 r01 r02 | | cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz |
// | r10 r11 r12 | = | cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz |
// | r20 r21 r22 | | -cx*sy sx cx*cy |
// +- -+ +- -+
if (Get3x3ElementAtIndex(mat, 7) < 1 - EPSILON)
{
if (Get3x3ElementAtIndex(mat, 7) > -(1 - EPSILON))
{
// x_angle = asin(r21)
// z_angle = atan2(-r01,r11)
// y_angle = atan2(-r20,r22)
res[1] = (float)Math.Asin(Get3x3ElementAtIndex(mat, 7));
res[0] = (float)Math.Atan2(-Get3x3ElementAtIndex(mat, 1), Get3x3ElementAtIndex(mat, 4));
res[2] = (float)Math.Atan2(-Get3x3ElementAtIndex(mat, 6), Get3x3ElementAtIndex(mat, 8));
eulerRes = EulerResult.EA_UNIQUE;
return(res);
}
else
{
// x_angle = -pi/2
// y_angle - z_angle = atan2(r02,r00)
// WARNING. The solution is not unique. Choosing y_angle = 0.
res[1] = -(float)Math.PI / 2.0f;
res[0] = -(float)Math.Atan2(Get3x3ElementAtIndex(mat, 2), Get3x3ElementAtIndex(mat, 0));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_DIF;
return(res);
}
}
else
{
// x_angle = +pi/2
// y_angle + z_angle = atan2(r02,r00)
// WARNING. The solution is not unique. Choosing y_angle = 0.
res[1] = (float)Math.PI / 2.0f;
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 2), Get3x3ElementAtIndex(mat, 0));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_SUM;
return(res);
}
}
public static Vector3 ExtractEulerZYZ(Matrix4x4 mat, out EulerResult eulerRes)
{
Vector3 res = new Vector3();
// +- -+ +- -+
// | r00 r01 r02 | | cy*cz0*cz1-sz0*sz1 -cz1*sz0-cy*cz0*sz1 sy*cz0 |
// | r10 r11 r12 | = | cy*cz1*sz0+cz0*sz1 cz0*cz1-cy*sz0*sz1 sy*sz0 |
// | r20 r21 r22 | | -sy*cz1 sy*sz1 cy |
// +- -+ +- -+
if (Get3x3ElementAtIndex(mat, 8) < 1 - EPSILON)
{
if (Get3x3ElementAtIndex(mat, 8) > -(1 - EPSILON))
{
// y_angle = acos(r22)
// z0_angle = atan2(r12,r02)
// z1_angle = atan2(r21,-r20)
res[1] = (float)Math.Acos(Get3x3ElementAtIndex(mat, 8));
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 5), Get3x3ElementAtIndex(mat, 2));
res[2] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 7), -Get3x3ElementAtIndex(mat, 6));
eulerRes = EulerResult.EA_UNIQUE;
return(res);
}
else // r22 = -1
{
// Not a unique solution: z1_angle - z0_angle = atan2(r10,r11)
res[1] = (float)Math.PI;
res[0] = -(float)Math.Atan2(Get3x3ElementAtIndex(mat, 3), Get3x3ElementAtIndex(mat, 4));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_DIF;
return(res);
}
}
else // r22 = +1
{
// Not a unique solution: z1_angle + z0_angle = atan2(r10,r11)
res[1] = 0;
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 3), Get3x3ElementAtIndex(mat, 4));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_SUM;
return(res);
}
}
public static Vector3 ExtractEulerZXZ(Matrix4x4 mat, out EulerResult eulerRes)
{
Vector3 res = new Vector3();
// +- -+ +- -+
// | r00 r01 r02 | | cz0*cz1-cx*sz0*sz1 -cx*cz1*sz0-cz0*sz1 sx*sz0 |
// | r10 r11 r12 | = | cz1*sz0+cx*cz0*sz1 cx*cz0*cz1-sz0*sz1 -sz*cz0 |
// | r20 r21 r22 | | sx*sz1 sx*cz1 cx |
// +- -+ +- -+
if (Get3x3ElementAtIndex(mat, 8) < 1 - EPSILON)
{
if (Get3x3ElementAtIndex(mat, 8) > -(1 - EPSILON))
{
// x_angle = acos(r22)
// z0_angle = atan2(r02,-r12)
// z1_angle = atan2(r20,r21)
res[1] = (float)Math.Acos(Get3x3ElementAtIndex(mat, 8));
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 2), -Get3x3ElementAtIndex(mat, 5));
res[2] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 6), Get3x3ElementAtIndex(mat, 7));
eulerRes = EulerResult.EA_UNIQUE;
return(res);
}
else
{
// Not a unique solution: z1_angle - z0_angle = atan2(-r01,r00)
res[1] = (float)Math.PI;
res[0] = -(float)Math.Atan2(-Get3x3ElementAtIndex(mat, 1), Get3x3ElementAtIndex(mat, 0));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_DIF;
return(res);
}
}
else
{
// Not a unique solution: z1_angle + z0_angle = atan2(-r01,r00)
res[1] = 0;
res[0] = (float)Math.Atan2(-Get3x3ElementAtIndex(mat, 1), Get3x3ElementAtIndex(mat, 0));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_SUM;
return(res);
}
}
public static Vector3 ExtractEulerYZY(Matrix4x4 mat, out EulerResult eulerRes)
{
Vector3 res = new Vector3();
// +- -+ +- -+
// | r00 r01 r02 | | cz*cy0*cy1-sy0*sy1 -sz*cy0 cy1*sy0+cz*cy0*sy1 |
// | r10 r11 r12 | = | sz*cy1 cz sz*sy1 |
// | r20 r21 r22 | | -cz*cy1*sy0-cy0*sy1 sz*sy0 cy0*cy1-cz*sy0*sy1 |
// +- -+ +- -+
if (Get3x3ElementAtIndex(mat, 4) < 1 - EPSILON)
{
if (Get3x3ElementAtIndex(mat, 4) > -(1 - EPSILON))
{
// z_angle = acos(r11)
// y0_angle = atan2(r21,-r01)
// y1_angle = atan2(r12,r10)
res[1] = (float)Math.Acos(Get3x3ElementAtIndex(mat, 4));
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 7), -Get3x3ElementAtIndex(mat, 1));
res[2] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 5), Get3x3ElementAtIndex(mat, 3));
eulerRes = EulerResult.EA_UNIQUE;
return(res);
}
else
{
// Not a unique solution: y1_angle - y0_angle = atan2(-r20,r22)
res[1] = (float)Math.PI;
res[0] = -(float)Math.Atan2(-Get3x3ElementAtIndex(mat, 6), Get3x3ElementAtIndex(mat, 8));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_DIF;
return(res);
}
}
else
{
// Not a unique solution: y1_angle + y0_angle = atan2(-r20,r22)
res[1] = 0;
res[0] = (float)Math.Atan2(-Get3x3ElementAtIndex(mat, 6), Get3x3ElementAtIndex(mat, 8));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_SUM;
return(res);
}
}
public static Vector3 ExtractEulerXZX(Matrix4x4 mat, out EulerResult eulerRes)
{
Vector3 res = new Vector3();
// +- -+ +- -+
// | r00 r01 r02 | | cz -sz*cx1 sz*sx1 |
// | r10 r11 r12 | = | sz*cx0 cz*cx0*cx1-sx0*sx1 -cx1*sx0-cz*cx0*sx1 |
// | r20 r21 r22 | | sz*sx0 cz*cx1*sx0+cx0*sx1 cx0*cx1-cz*sx0*sx1 |
// +- -+ +- -+
if (Get3x3ElementAtIndex(mat, 0) < 1 - EPSILON)
{
if (Get3x3ElementAtIndex(mat, 0) > -(1 - EPSILON))
{
// z_angle = acos(r00)
// x0_angle = atan2(r20,r10)
// x1_angle = atan2(r02,-r01)
res[1] = (float)Math.Acos(Get3x3ElementAtIndex(mat, 0));
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 6), Get3x3ElementAtIndex(mat, 3));
res[2] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 2), -Get3x3ElementAtIndex(mat, 1));
eulerRes = EulerResult.EA_UNIQUE;
return(res);
}
else
{
// Not a unique solution: x1_angle - x0_angle = atan2(r21,r22)
res[1] = (float)Math.PI;
res[0] = -(float)Math.Atan2(Get3x3ElementAtIndex(mat, 7), Get3x3ElementAtIndex(mat, 8));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_DIF;
return(res);
}
}
else
{
// Not a unique solution: x1_angle + x0_angle = atan2(r21,r22)
res[1] = 0;
res[0] = (float)Math.Atan2(Get3x3ElementAtIndex(mat, 7), Get3x3ElementAtIndex(mat, 8));
res[2] = 0;
eulerRes = EulerResult.EA_NOT_UNIQUE_SUM;
return(res);
}
}
