diff --git a/MshViewer/Source/Object.cpp b/MshViewer/Source/Object.cpp index 5d77adf..66b5317 100644 --- a/MshViewer/Source/Object.cpp +++ b/MshViewer/Source/Object.cpp @@ -574,21 +574,20 @@ void Object::readUV(Segment* dataDestination, std::streampos position) void Object::quat2eul(float &quat0, float &quat1, float & quat2, float &quat3) { - float eulX = atan2( - 2 * (quat0 * quat1 + quat2 * quat3), - 1 - 2 * (pow(quat1, 2) + pow(quat2, 2)) - ); + float ysqr = quat1 * quat1; + float t0 = -2.0f * (ysqr + quat2 * quat2) + 1.0f; + float t1 = +2.0f * (quat0 * quat1 - quat3 * quat2); + float t2 = -2.0f * (quat0 * quat2 + quat3 * quat1); + float t3 = +2.0f * (quat1 * quat2 - quat3 * quat0); + float t4 = -2.0f * (quat0 * quat0 + ysqr) + 1.0f; - float eulY = asin(2 * (quat0 * quat2 - quat3 * quat1)); - - float eulZ = atan2( - 2 * (quat0 * quat3 + quat1 * quat2), - 1 - 2 * (pow(quat2, 2) + pow(quat3, 2)) - ) - PI; + t2 = t2 > 1.0f ? 1.0f : t2; + t2 = t2 < -1.0f ? -1.0f : t2; + + quat1 = std::asin(t2); + quat0 = std::atan2(t3, t4); + quat2 = std::atan2(t1, t0); - quat0 = eulX; - quat1 = eulY; - quat2 = eulZ; }