//zxy for unity
//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
/*
*
* 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,
* res);
*
* void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
* res[0] = atan2( r31, r32 );
* res[1] = asin ( r21 );
* res[2] = atan2( r11, r12 );
* }
*
* r11 = -2*(q.x* q.y - q.w* q.z)
* r12 = q.w* q.w - q.x* q.x + q.y* q.y - q.z* q.z
* r21 = 2*(q.y* q.z + q.w* q.x)
* r31 = -2*(q.x* q.z - q.w* q.y)
* r32 = q.w* q.w - q.x* q.x - q.y* q.y + q.z* q.z
*
* for zxy
* y = res[0]
* x = res[1]
* z = res[2]
*/
public Fix64 GetXAngle()
{
//Fix64 r21 = (Fix64)2 * (y * z + w * x);
//return Fix64.Asin(r21);
Fix64 sinr_cosp = (Fix64)2 * (w * x + y * z);
Fix64 cosr_cosp = (Fix64)1 - (Fix64)2 * (x * x + y * y);
return(Fix64.Atan2(sinr_cosp, cosr_cosp));
}
public Fix64 GetZAngle()
{
//Fix64 r11 = -(Fix64)2 * (x * y - w * z);
//Fix64 r12 = w * w - x * x + y * y - z * z;
//return Fix64.Atan2(r11, r12);
Fix64 siny_cosp = (Fix64)2 * (w * z + x * y);
Fix64 cosy_cosp = (Fix64)1 - (Fix64)2 * (y * y + z * z);
return(Fix64.Atan2(siny_cosp, cosy_cosp));
}
//https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
public Fix64Vec3 EulerAngles1()
{
Fix64 sqw = w * w;
Fix64 sqx = x * x;
Fix64 sqy = y * y;
Fix64 sqz = z * z;
Fix64 unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
Fix64 test = x * y + z * w;
Fix64 _x = Fix64.zero;
Fix64 _y = Fix64.zero;
Fix64 _z = Fix64.zero;
Fix64 cutoff = Fix64.FromDivision(499, 1000) * unit;
_z = Fix64.Atan2((Fix64)2 * y * w - (Fix64)2 * x * z, sqx - sqy - sqz + sqw);
_y = Fix64.Asin((Fix64)2 * test / unit);
_x = Fix64.Atan2((Fix64)2 * x * w - (Fix64)2 * y * z, -sqx + sqy - sqz + sqw);
//if (test >= cutoff)
//{ // singularity at north pole
// _z = (Fix64)2 * Fix64.Atan2(x, w);
// _y = Fix64.halfPi;
// _x = Fix64.zero;
//}
//else if (test < -cutoff)
//{ // singularity at south pole
// _z = -(Fix64)2 * Fix64.Atan2(x, w);
// _y = -Fix64.halfPi;
// _x = Fix64.zero;
//}
//else
//{
// _z = Fix64.Atan2((Fix64)2 * y * w - (Fix64)2 * x * z, sqx - sqy - sqz + sqw);
// _y = Fix64.Asin((Fix64)2 * test / unit);
// _x = Fix64.Atan2((Fix64)2 * x * w - (Fix64)2 * y * z, -sqx + sqy - sqz + sqw);
//}
Fix64 xDegree = Fix64.RadToDeg(_x);
Fix64 yDegree = Fix64.RadToDeg(_y);
Fix64 zDegree = Fix64.RadToDeg(_z);
return(new Fix64Vec3(xDegree, zDegree, yDegree));
}
