//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)); }