Euler4 ToEulerInHalfSingularity()
{
if (ex.x < 0)
{
// 90 deg of X, U, V
}
else if (ey.y < 0)
{
if (ez.z < 0)
{
return(new Euler4(0, Utility.Atan2(ey.z, ey.y) * Math.Rad2Deg)); // X
}
if (ew.w < 0)
{
return(new Euler4(4, Utility.Atan2(ew.y, ey.y) * Math.Rad2Deg)); // U
}
}
else if (ez.z < 0 && ew.w < 0)
{
return(new Euler4(5, Utility.Atan2(ew.z, ez.z) * Math.Rad2Deg)); // V
}
// (all diag is positive)
return(new Euler4(Math.Asin(ey.z), Math.Asin(ez.x), Math.Asin(ex.y),
Math.Asin(ew.x), Math.Asin(ew.y), Math.Asin(ew.z)) * Math.Rad2Deg);
}
Euler4 ToEulerInSingularity()
{
if (ex.x < 0)
{
if (ey.y < 0)
{
return(new Euler4(2, Utility.Atan2(ey.x, ex.x) * Math.Rad2Deg)); // Z
}
if (ez.z < 0)
{
return(new Euler4(1, Utility.Atan2(ex.z, ex.x) * Math.Rad2Deg)); // Y
}
if (ew.w < 0)
{
return(new Euler4(3, Utility.Atan2(ew.x, ex.x) * Math.Rad2Deg)); // T
}
}
else if (ey.y < 0)
{
if (ez.z < 0)
{
return(new Euler4(0, Utility.Atan2(ez.y, ey.y) * Math.Rad2Deg)); // X
}
if (ew.w < 0)
{
return(new Euler4(4, Utility.Atan2(ew.y, ey.y) * Math.Rad2Deg)); // U
}
}
else if (ez.z < 0 && ew.w < 0)
{
return(new Euler4(5, Utility.Atan2(ew.z, ez.z) * Math.Rad2Deg)); // V
}
// (all diag is positive)
return(new Euler4(Math.Asin(ez.y), Math.Asin(ex.z), Math.Asin(ey.x),
Math.Asin(ew.x), Math.Asin(ew.y), Math.Asin(ew.z)) * Math.Rad2Deg);
}
public static float Asin(float d)
{
return(Mathf.Asin(d));
}
// 四元数转欧拉角
static Vector3RightHand ToEulerRad(QuaternionRightHand q)
{
// float sqw = rotation.w * rotation.w;
// float sqx = rotation.x * rotation.x;
// float sqy = rotation.y * rotation.y;
// float sqz = rotation.z * rotation.z;
// float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
// float test = rotation.x * rotation.w - rotation.y * rotation.z;
// Vector3 v;
//
// if (test > 0.4995f * unit)
// { // singularity at north pole
// v.y = 2f * (float)Mathf.Atan2(rotation.y, rotation.x);
// v.x = (float)Mathf.PI / 2;
// v.z = 0;
// return NormalizeAngles(v * Mathf.Rad2Deg);
// }
// if (test < -0.4995f * unit)
// { // singularity at south pole
// v.y = -2f * (float)Mathf.Atan2(rotation.y, rotation.x);
// v.x = -(float)Mathf.PI / 2;
// v.z = 0;
// return NormalizeAngles(v * Mathf.Rad2Deg);
// }
// Quaternion q = new Quaternion(rotation.w, rotation.z, rotation.x, rotation.y);
// v.y = (float)Mathf.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
// v.x = (float)Mathf.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
// v.z = (float)Mathf.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
// return NormalizeAngles(v * Mathf.Rad2Deg) * Mathf.Deg2Rad;
float sqw = q.w * q.w;
float sqx = q.x * q.x;
float sqy = q.y * q.y;
float sqz = q.z * q.z;
float unit = sqx + sqy + sqz + sqw;
float test = q.x * q.y + q.z * q.w;
float yaw = 0.0f;
float pitch = 0.0f;
float roll = 0.0f;
// North pole singularity
if (test > 0.499f * unit)
{
yaw = 2.0f * Mathf.Atan2(q.x, q.w);
pitch = Mathf.PI * 0.5f;
roll = 0.0f;
}
// South pole singularity
else if (test < -0.499f * unit)
{
yaw = -2.0f * Mathf.Atan2(q.x, q.w);
pitch = -Mathf.PI * 0.5f;
roll = 0.0f;
}
else
{
yaw = Mathf.Atan2(2.0f * q.y * q.w - 2.0f * q.x * q.z, sqx - sqy - sqz + sqw);
pitch = Mathf.Asin(2.0f * test / unit);
roll = Mathf.Atan2(2.0f * q.x * q.w - 2.0f * q.y * q.z, -sqx + sqy - sqz + sqw);
}
// Keep angles [0..360].
if (Mathf.Sign(yaw) < 0)
{
yaw = Mathf.Deg2Rad * (360) + yaw;
}
if (Mathf.Sign(pitch) < 0)
{
pitch = Mathf.Deg2Rad * (360) + pitch;
}
if (Mathf.Sign(roll) < 0)
{
roll = Mathf.Deg2Rad * (360) + roll;
}
return(new Vector3RightHand(roll, yaw, pitch));
}
public static float Safe_Asin(float r)
{
return(Mathf.Asin(Mathf.Min(1.0f, Mathf.Max(-1.0f, r))));
}
