|
发表于 2010-11-1 08:56:30
|
显示全部楼层
void getEulerFromQuat(osg:uat q, double& heading, double& attitude, double& bank)
{
double limit = 0.499999;
double sqx = q.x()*q.x();
double sqy = q.y()*q.y();
double sqz = q.z()*q.z();
double t = q.x()*q.y() + q.z()*q.w();
if (t>limit) // gimbal lock (万向节死锁)
{
heading = 2 * atan2(q.x(),q.w());
attitude = osg:I_2;
bank = 0;
}
else if (t<-limit)
{
heading = -2 * atan2(q.x(),q.w());
attitude = - osg::PI_2;
bank = 0;
}
else
{
heading = atan2(2*q.y()*q.w()-2*q.x()*q.z() , 1 - 2*sqy - 2*sqz);
attitude = asin(2*t);
bank = atan2(2*q.x()*q.w()-2*q.y()*q.z() , 1 - 2*sqx - 2*sqz);
}
} |
|