public LSL_Vector llRot2Euler(LSL_Rotation q1)
{
m_host.AddScriptLPS(1);
LSL_Vector eul = new LSL_Vector();
double sqw = q1.s*q1.s;
double sqx = q1.x*q1.x;
double sqy = q1.z*q1.z;
double sqz = q1.y*q1.y;
double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
double test = q1.x*q1.z + q1.y*q1.s;
if (test > 0.4999*unit) { // singularity at north pole
eul.z = 2 * Math.Atan2(q1.x,q1.s);
eul.y = Math.PI/2;
eul.x = 0;
return eul;
}
if (test < -0.4999*unit) { // singularity at south pole
eul.z = -2 * Math.Atan2(q1.x,q1.s);
eul.y = -Math.PI/2;
eul.x = 0;
return eul;
}
eul.z = Math.Atan2(2*q1.z*q1.s-2*q1.x*q1.y , sqx - sqy - sqz + sqw);
eul.y = Math.Asin(2*test/unit);
eul.x = Math.Atan2(2*q1.x*q1.s-2*q1.z*q1.y , -sqx + sqy - sqz + sqw);
return eul;
}