osgEarth根据MatrixTransform获取经纬度、欧拉值

//根据matrixTransform获取经纬度

osg::Vec3d getModelPosition(osg::ref_ptr<osg::MatrixTransform> m_pModelNode)
{
	osg::Vec3d posXYZ = osg::Vec3d();
	osg::Vec3d currentPosition = osg::Vec3d();
	if (m_pModelNode)
	{
		osg::Matrix mat = m_pModelNode->getMatrix();
		posXYZ = posXYZ * mat;
		//            posXYZ = m_pModelNode->getMatrix().getTrans();
	}
	osg::EllipsoidModel* em = new osg::EllipsoidModel();
	em->convertXYZToLatLongHeight(posXYZ.x(), posXYZ.y(), posXYZ.z(),
		currentPosition.y(), currentPosition.x(),
		currentPosition.z());
		
	currentPosition.x() = osg::RadiansToDegrees(currentPosition.x());
	currentPosition.y() = osg::RadiansToDegrees(currentPosition.y());
	return currentPosition;
}


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

//根据matrixTransform获取旋转量 heading pitch roll

 osg::Vec3d scale = osg::Vec3d();
			osg::Quat so;
			osg::Vec3d translate = osg::Vec3d();
			//经过缩放后的模型最好使用decompose矩阵分解，不然getRotate所得旋转量可能不正确
			osg::Quat rotation;
		Planemat->getMatrix().decompose(translate, rotation, scale, so);

			double headingAngle = 0.0;
			double pitchAngle = 0.0;
			double rollAngle = 0.0;
			QuatToHPR(rotation, headingAngle, pitchAngle, rollAngle);