public enum RotSeq { zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy, xzx };
private static void twoaxisrot(double r11, double r12, double r21, double r31, double r32, ref double[] res)
{
res[0] = Math.Atan2(r11, r12);
res[1] = Math.Acos(r21);
res[2] = Math.Atan2(r31, r32);
}
private static void threeaxisrot(double r11, double r12, double r21, double r31, double r32, ref double[] res)
{
res[0] = Math.Atan2(r31, r32);
res[1] = Math.Asin(r21);
res[2] = Math.Atan2(r11, r12);
}
public static void quaternion2euler(Quaternion q, ref double[] res, RotSeq rotSeq)
{
double q_x, q_y, q_z, q_w;
q_x = q.X;
q_y = q.Y;
q_z = q.Z;
q_w = q.W;
switch (rotSeq)
{
case RotSeq.zyx:
threeaxisrot(2 * (q_x * q_y + q_w * q_z),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
-2 * (q_x * q_z - q_w * q_y),
2 * (q_y * q_z + q_w * q_x),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
ref res);
break;
case RotSeq.zyz:
twoaxisrot(2 * (q_y * q_z - q_w * q_x),
2 * (q_x * q_z + q_w * q_y),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
2 * (q_y * q_z + q_w * q_x),
-2 * (q_x * q_z - q_w * q_y),
ref res);
break;
case RotSeq.zxy:
threeaxisrot(-2 * (q_x * q_y - q_w * q_z),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
2 * (q_y * q_z + q_w * q_x),
-2 * (q_x * q_z - q_w * q_y),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
ref res);
break;
case RotSeq.zxz:
twoaxisrot(2 * (q_x * q_z + q_w * q_y),
-2 * (q_y * q_z - q_w * q_x),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
2 * (q_x * q_z - q_w * q_y),
2 * (q_y * q_z + q_w * q_x),
ref res);
break;
case RotSeq.yxz:
threeaxisrot(2 * (q_x * q_z + q_w * q_y),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
-2 * (q_y * q_z - q_w * q_x),
2 * (q_x * q_y + q_w * q_z),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
ref res);
break;
case RotSeq.yxy:
twoaxisrot(2 * (q_x * q_y - q_w * q_z),
2 * (q_y * q_z + q_w * q_x),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
2 * (q_x * q_y + q_w * q_z),
-2 * (q_y * q_z - q_w * q_x),
ref res);
break;
case RotSeq.yzx:
threeaxisrot(-2 * (q_x * q_z - q_w * q_y),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
2 * (q_x * q_y + q_w * q_z),
-2 * (q_y * q_z - q_w * q_x),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
ref res);
break;
case RotSeq.yzy:
twoaxisrot(2 * (q_y * q_z + q_w * q_x),
-2 * (q_x * q_y - q_w * q_z),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
2 * (q_y * q_z - q_w * q_x),
2 * (q_x * q_y + q_w * q_z),
ref res);
break;
case RotSeq.xyz:
threeaxisrot(-2 * (q_y * q_z - q_w * q_x),
q_w * q_w - q_x * q_x - q_y * q_y + q_z * q_z,
2 * (q_x * q_z + q_w * q_y),
-2 * (q_x * q_y - q_w * q_z),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
ref res);
break;
case RotSeq.xyx:
twoaxisrot(2 * (q_x * q_y + q_w * q_z),
-2 * (q_x * q_z - q_w * q_y),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
2 * (q_x * q_y - q_w * q_z),
2 * (q_x * q_z + q_w * q_y),
ref res);
break;
case RotSeq.xzy:
threeaxisrot(2 * (q_y * q_z + q_w * q_x),
q_w * q_w - q_x * q_x + q_y * q_y - q_z * q_z,
-2 * (q_x * q_y - q_w * q_z),
2 * (q_x * q_z + q_w * q_y),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
ref res);
break;
case RotSeq.xzx:
twoaxisrot(2 * (q_x * q_z - q_w * q_y),
2 * (q_x * q_y + q_w * q_z),
q_w * q_w + q_x * q_x - q_y * q_y - q_z * q_z,
2 * (q_x * q_z + q_w * q_y),
-2 * (q_x * q_y - q_w * q_z),
ref res);
break;
default:
Trace.WriteLine("Unknown rotation sequence");
break;
}
}
Created
March 23, 2023 10:40
-
-
Save ivanjimenez/397c6501a29c934c8c5754fbeb6fa975 to your computer and use it in GitHub Desktop.
Rotations
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment