mirror of https://github.com/AxioDL/zeus.git
67 lines
1.8 KiB
C++
67 lines
1.8 KiB
C++
#include "zeus/CEulerAngles.hpp"
|
|
|
|
#include <cmath>
|
|
|
|
#include "zeus/CQuaternion.hpp"
|
|
#include "zeus/CTransform.hpp"
|
|
|
|
namespace zeus {
|
|
|
|
CEulerAngles::CEulerAngles(const CQuaternion& quat) {
|
|
float quatDot = quat.dot(quat);
|
|
float t0 = 0.f;
|
|
if (quatDot > 0.f)
|
|
t0 = 2.f / quatDot;
|
|
double t1 = 1.0 - (t0 * quat.x() * quat.x() + t0 * quat.z() * quat.z());
|
|
double t2 = t0 * quat.y() * quat.x() - t0 * quat.z() * quat.w();
|
|
double t3 = t1 * t1 + t2 * t2;
|
|
|
|
double t4 = 0.0;
|
|
if (t3 > 0.0)
|
|
t4 = std::sqrt(t3);
|
|
|
|
double t5 = t0 * quat.z() * quat.y() + t0 * quat.x() * quat.w();
|
|
|
|
if (std::abs(t4) > 0.00001) {
|
|
x() = -std::atan2(-t5, t4);
|
|
y() = -std::atan2(t0 * quat.z() * quat.x() - t0 * quat.y() * quat.w(),
|
|
1.0 - (t0 * quat.x() * quat.x() + t0 * quat.y() * quat.y()));
|
|
z() = -std::atan2(t2, t1);
|
|
} else {
|
|
x() = -std::atan2(-t5, t4);
|
|
y() = -std::atan2(-(t0 * quat.z() * quat.x() + t0 * quat.y() * quat.w()),
|
|
1.0 - (t0 * quat.y() * quat.y() + t0 * quat.z() * quat.z()));
|
|
z() = 0.f;
|
|
}
|
|
}
|
|
|
|
CEulerAngles::CEulerAngles(const CTransform& xf) {
|
|
float xyMagSq = xf.basis[1][1] * xf.basis[1][1] + xf.basis[1][0] * xf.basis[1][0];
|
|
float f1 = 0.f;
|
|
if (xyMagSq > 0.f) {
|
|
f1 = 1.f / std::sqrt(xyMagSq);
|
|
float f0;
|
|
|
|
for (int i = 0; i < 4; ++i) {
|
|
f0 = f1 * f1;
|
|
f1 *= 0.5f;
|
|
f0 = 3.f - xyMagSq * f0;
|
|
f1 *= f0;
|
|
}
|
|
|
|
f1 = xyMagSq * f0;
|
|
}
|
|
|
|
if (std::fabs(f1) >= 0.00001) {
|
|
x() = -std::atan2(-xf.basis[1][2], f1);
|
|
y() = -std::atan2(xf.basis[0][2], xf.basis[2][2]);
|
|
z() = -std::atan2(xf.basis[1][0], xf.basis[1][1]);
|
|
} else {
|
|
x() = -std::atan2(-xf.basis[1][2], f1);
|
|
y() = -std::atan2(-xf.basis[2][0], xf.basis[0][0]);
|
|
z() = 0.f;
|
|
}
|
|
}
|
|
|
|
} // namespace zeus
|