// <!!> Unity本体のToEulerやSpringheadのToEulerとはどう違うのか…？
Vec3d ToEuler(Quaterniond q)
{
double poleCheck = q.X() * q.Y() + q.Z() * q.W();
double heading;
double attitude;
double bank;
if (poleCheck > 0.499) // north pole
{
heading = 2 * Math.Atan2(q.X(), q.W());
attitude = 0;
bank = 0;
}
else if (poleCheck < -0.499) // south pole
{
heading = -2 * Math.Atan2(q.X(), q.W());
attitude = 0;
bank = 0;
}
else
{
heading = Math.Atan2(2 * q.Y() * q.W() - 2 * q.X() * q.Z(), 1 - 2 * q.Y() * q.Y() - 2 * q.Z() * q.Z());
attitude = Math.Asin(2 * q.X() * q.Y() + 2 * q.Z() * q.W());
bank = Math.Atan2(2 * q.X() * q.W() - 2 * q.Y() * q.Z(), 1 - 2 * q.X() * q.X() - 2 * q.Z() * q.Z());
}
//返す値がy, z, xの順
//return new Vec3d(heading, attitude, bank);
//返す値がx, y, zの順
return(new Vec3d(bank, heading, attitude));
}
