//Get Yaw Pitch Roll
HawkVector3D HawkQuaternion::GetAsYawPitchRoll() const
{
Float fYaw = 0;
Float fPitch = 0;
Float fRoll = 0;;
Float sp = -2.0f * (Y*Z + W*X);
//万向锁
if(HawkMath::IsZero(HawkMath::Abs(sp) - 1.0f))
{
fPitch = HawkMath::HALF_PI * sp;
fYaw = HawkMath::Atan2(-X*Z + W*Y,0.5f - Y*Y - Z*Z);
fRoll = 0;
}
else
{
fPitch = HawkMath::Asin(sp);
fYaw = HawkMath::Atan2(X*Z + W*Y,0.5f - X*X - Y*Y);
fRoll = HawkMath::Atan2(X*Y + W*Z,0.5f - X*X - Z*Z);
}
return HawkVector3D(fYaw,fPitch,fRoll);
}
//默认构造
HawkLine3D::HawkLine3D() : Point(HawkVector3D()),Direction(HawkVector3D())
{
}