|
发表于 2008-6-28 19:10:26
|
显示全部楼层
给你发个换转得代码
- // HPR to Quat
- 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);
- }
复制代码
[ 本帖最后由 array 于 2008-6-28 19:36 编辑 ] |
|