57 lines
1.9 KiB
C++
57 lines
1.9 KiB
C++
#include "utils/OsgUtils.h"
|
|
|
|
#include <osg/Math>
|
|
|
|
#include "common/SpdLogger.h"
|
|
|
|
|
|
osg::Quat OsgUtils::HPRToQuat(const osg::Vec3& angle) {
|
|
osg::Quat q(
|
|
osg::inDegrees(angle.y()), osg::Vec3d(0.0, 1.0, 0.0),
|
|
osg::inDegrees(angle.x()), osg::Vec3d(1.0, 0.0, 0.0),
|
|
osg::inDegrees(angle.z()), osg::Vec3d(0.0, 0.0, 1.0));
|
|
return q;
|
|
}
|
|
|
|
void OsgUtils::QuatToHPR(const osg::Quat& q, osg::Vec3* angle) {
|
|
double test = q.y() * q.z() + q.x() * q.w();
|
|
double heading = 0, pitch = 0, roll = 0;
|
|
if (test > 0.4999) { // singularity at north pole
|
|
heading = 2.0 * atan2(q.y(), q.w());
|
|
pitch = osg::PI_2;
|
|
roll = 0.0;
|
|
angle->set(90.0f, roll, osg::RadiansToDegrees(heading));
|
|
return;
|
|
}
|
|
if (test < -0.4999) { // singularity at south pole
|
|
heading = 2.0 * atan2(q.y(), q.w());
|
|
pitch = -osg::PI_2;
|
|
roll = 0.0;
|
|
angle->set(-90.0f, roll, osg::RadiansToDegrees(heading));
|
|
return;
|
|
}
|
|
double sqx = q.x() * q.x();
|
|
double sqy = q.y() * q.y();
|
|
double sqz = q.z() * q.z();
|
|
heading = atan2(2.0 * q.z() * q.w() - 2.0 * q.y() * q.x(), 1.0 - 2.0 * sqz - 2.0 * sqx);
|
|
pitch = asin(2.0 * test);
|
|
roll = atan2(2.0 * q.y() * q.w() - 2.0 * q.z() * q.x(), 1.0 - 2.0 * sqy - 2.0 * sqx);
|
|
angle->set(osg::RadiansToDegrees(pitch), osg::RadiansToDegrees(roll), osg::RadiansToDegrees(heading));
|
|
}
|
|
|
|
void OsgUtils::Vec4ToQColor(const osg::Vec4& vec, QColor* color) {
|
|
int r = static_cast<int>(vec.r() * 255.0f);
|
|
int g = static_cast<int>(vec.g() * 255.0f);
|
|
int b = static_cast<int>(vec.b() * 255.0f);
|
|
int a = static_cast<int>(vec.a() * 255.0f);
|
|
color->setRgb(r, g, b, a);
|
|
}
|
|
|
|
void OsgUtils::QColorToVec4(const QColor& color, osg::Vec4* vec) {
|
|
float r = color.red() / 255.0f;
|
|
float g = color.green() / 255.0f;
|
|
float b = color.blue() / 255.0f;
|
|
float a = color.alpha() / 255.0f;
|
|
vec->set(r, g, b, a);
|
|
}
|