public Fix64 GetYAngle() { //Fix64 r31 = -(Fix64)2 * (x * z - w * y); //Fix64 r32 = w * w - x * x - y * y + z * z; //return Fix64.Atan2(r31, r32); Fix64 sinp = (Fix64)2 * (w * y - z * x); //if (Fix64.Abs(sinp) >= Fix64.one) //{ // Fix64 sign = Fix64.Sign(sinp); // return sign * Fix64.halfPi; //} return(Fix64.Asin(sinp)); }
//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)); }