public static void Test()
{
Debug.Log("IsLittleEndian " + BitConverter.IsLittleEndian);
Debug.Log(FixPointMathTest.ConvertFloatToBinStr(1f));
Debug.Log(FixPointMathTest.ConvertFloatToBinStr(17.625f));
Debug.Log(FixPointMath.SqrtOld(new FloatL(0.955d)));
Debug.Log(Mathf.Sqrt(0.955f));
Debug.Log(FixPointMath.Sqrt(new FloatL(0.955d)));
Vector3L dir = new Vector3L(-0.093d, -0.093d, 0.988d);
FloatL angle1 = Vector3L_Angle(Vector3L.forward, dir);
float angle2 = Vector3_Angle(Vector3.forward, dir.Convert());
Debug.Log("angle1 " + angle1 + " angle2 " + angle2);
Debug.Log("sin 10 " + Mathf.Sin(10 * Mathf.Deg2Rad));
Debug.Log("sinL 10 " + FixPointMath.Sin(10 * Mathf.Deg2Rad));
Debug.Log("cos 10 " + Mathf.Cos(10 * Mathf.Deg2Rad));
Debug.Log("cosL 10 " + FixPointMath.Cos(10 * Mathf.Deg2Rad));
Debug.Log("acos 0.1 " + Mathf.Acos(0.1f));
Debug.Log("acosL 0.1 " + FixPointMath.Acos(0.1f));
Debug.Log("atan2 3/2 " + Mathf.Atan2(3, 2));
Debug.Log("atan2L 3/2 " + FixPointMath.Atan2(3, 2));
Debug.Log("asin 0.6 " + Mathf.Asin(0.6f));
Debug.Log("asinL 0.6 " + FixPointMath.Asin(0.6f));
}
// 四元数转欧拉角
static Vector3L ToEulerRad(QuaternionL rotation)
{
FloatL sqw = rotation.w * rotation.w;
FloatL sqx = rotation.x * rotation.x;
FloatL sqy = rotation.y * rotation.y;
FloatL sqz = rotation.z * rotation.z;
FloatL unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
FloatL test = rotation.x * rotation.w - rotation.y * rotation.z;
Vector3L v;
if (test > 0.4995d * unit)
{ // singularity at north pole
v.y = 2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
if (test < -0.4995d * unit)
{ // singularity at south pole
v.y = -2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = -FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
QuaternionL q = new QuaternionL(rotation.w, rotation.z, rotation.x, rotation.y);
v.y = FixPointMath.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
v.x = FixPointMath.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
v.z = FixPointMath.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 * FixPointMath.Rad2Deg) * FixPointMath.Deg2Rad);
}
