OSG 四元数与欧拉角互转

1. // HPR to Quat
2. osg::Quat HPRToQuat(double heading, double pitch, double roll)
3. {
4. osg::Quat q(
5.   roll,osg::Vec3d(0.0, 1.0, 0.0),
6.   pitch,osg::Vec3d(1.0, 0.0, 0.0),
7.   heading,osg::Vec3d(0.0, 0.0, 1.0));
10. return q;
11. }
13. // Quat to HPR，pitch范围：[-PI/2, PI/2]
14. void QuatToHPR(osg::Quat q, double& heading, double& pitch, double& roll)
15. {
17.         double test = q.y() * q.z() + q.x() * q.w();
19.         if (test > 0.4999)
20.         { // singularity at north pole
21.         heading = 2.0 * atan2(q.y(), q.w());
22.         pitch   = osg::PI_2;
23.         roll    = 0.0;
24.         return;
25.         }
26.         if (test < -0.4999)
27.         { // singularity at south pole
28.         heading = 2.0 * atan2(q.y(), q.w());
29.         pitch   = -osg::PI_2;
30.         roll    = 0.0;
31.         return;
32.         }
34.         double sqx = q.x() * q.x();
35.         double sqy = q.y() * q.y();
36.         double sqz = q.z() * q.z();
38.         heading = atan2(2.0 * q.z() * q.w() - 2.0 * q.y() * q.x(), 1.0 - 2.0 * sqz - 2.0 * sqx);
39.         pitch   = asin(2.0 * test);
40.         roll    = atan2(2.0 * q.y() * q.w() - 2.0 * q.z() * q.x(), 1.0 - 2.0 * sqy - 2.0 * sqx);
42. }