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);
}