I converted some C++ code from this website http://bediyap.com/programming/convert-quaternion-to-euler-rotations/ to C# seems to work. Apparently swaping axis is not enough. Hope it helps for someone suffering from Unity Axis system and Rhino integration.
private void RunScript(object y, ref object A)
{
Quaternion q = new Quaternion(0.1f, 0.2f, 0.4f, 0.5f);
//Quaternion q = new Quaternion(0, 0,1, 0);
// Quaternion q = new Quaternion(0, 0,0.7, -0.7);
q.normalize();
double[] res = new double[3]{0,0,0};
quaternion2Euler(q, ref res, RotSeq.yxz);//yxz
res[0] = rad2deg(res[0]) ;
res[1] = rad2deg(res[1]) ;
res[2] = rad2deg(res[2]) ;
A = new double[]{res[1],res[2],res[0]};
}
// <Custom additional code>
///////////////////////////////
// Quaternion struct
// Simple incomplete quaternion struct for demo purpose
///////////////////////////////
public struct Quaternion{
//
// public Quaternion(){
// this.x = 0;
// this.y = 0;
// this.z = 0;
// this.w = 0;
//
// }
public Quaternion(double x, double y, double z, double w){
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public void normalize(){
double norm = Math.Sqrt(x * x + y * y + z * z + w * w);
x /= norm;
y /= norm;
z /= norm;
w /= norm;
}
public double norm(){
return Math.Sqrt(x * x + y * y + z * z + w * w);
}
public double x;
public double y;
public double z;
public double w;
};
///////////////////////////////
// Quaternion to Euler
///////////////////////////////
public enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};
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);
}
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);
}
// note:
// return values of res[] depends on rotSeq.
// i.e.
// for rotSeq zyx,
// x = res[0], y = res[1], z = res[2]
// for rotSeq xyz
// z = res[0], y = res[1], x = res[2]
// ...
void quaternion2Euler(Quaternion q, ref double[] res, RotSeq rotSeq)
{
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:
//std::cout << "Unknown rotation sequence" << std::endl;
break;
}
}
///////////////////////////////
// Helper functions
///////////////////////////////
public static Quaternion Multiply(Quaternion q1, Quaternion q2){
Quaternion q = new Quaternion(0, 0, 0, 0);
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
q.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
q.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return q;
}
double rad2deg(double rad){
return Math.Abs(rad * 180.0 / Math.PI+360)%360;
}
