OSG四元数与欧拉角之间的转换

osg::Quat HPRToQuat(double heading, double pitch, double roll)
{
osg::Quat q(
roll, osg::Vec3d(0.0, 1.0, 0.0),
pitch, osg::Vec3d(1.0, 0.0, 0.0),
heading, osg::Vec3d(0.0, 0.0, 1.0));
return q;
}

// Quat to HPR，pitch范围：[-PI/2, PI/2]
void QuatToHPR(osg::Quat q, double& heading, double& pitch, double& roll)
{
double test = q.y() * q.z() + q.x() * q.w();
if (test > 0.4999)
{ // singularity at north pole
heading = 2.0 * atan2(q.y(), q.w());
pitch = osg::PI_2;
roll = 0.0;
return;
}
if (test < -0.4999)
{ // singularity at south pole
heading = 2.0 * atan2(q.y(), q.w());
pitch = -osg::PI_2;
roll = 0.0;
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);
}