四元数、欧拉角互相转换

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

osg::Quat CcompressTextureDlg::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;
}