- double r11, r21, r31, r32, r33;
- double q00, q11, q22, q33;
- double tmp;
- Vector u;
-
- q00 = q.n * q.n;
- q11 = q.v.x * q.v.x;
- q22 = q.v.y * q.v.y;
- q33 = q.v.z * q.v.z;
-
- r11 = q00 + q11 - q22 - q33;
- r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
- r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
- r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
- r33 = q00 - q11 - q22 + q33;
-
- tmp = fabs(r31);
- if(tmp > 0.999999)
- {
- double r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
- double r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
-
- u.x = RadiansToDegrees(0.0f); //roll
- u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
- u.z = RadiansToDegrees((float) atan2(-r12, -r31*r13)); // yaw
- return u;
- }
-
- u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
- u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
- u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw
- return u;
+ double r11, r21, r31, r32, r33;
+ double q00, q11, q22, q33;
+ double tmp;
+ Vector u;
+
+ q00 = q.n * q.n;
+ q11 = q.v.x * q.v.x;
+ q22 = q.v.y * q.v.y;
+ q33 = q.v.z * q.v.z;
+
+ r11 = q00 + q11 - q22 - q33;
+ r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
+ r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
+ r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
+ r33 = q00 - q11 - q22 + q33;
+
+ tmp = fabs(r31);
+ if (tmp > 0.999999) {
+ double r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
+ double r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
+
+ u.x = RadiansToDegrees(0.0f); //roll
+ u.y = RadiansToDegrees((float) (-(pi / 2) * r31 / tmp)); // pitch
+ u.z = RadiansToDegrees((float) atan2(-r12, -r31 * r13)); // yaw
+ return u;
+ }
+
+ u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
+ u.y = RadiansToDegrees((float) asinf(-r31)); // pitch
+ u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw
+ return u;