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));
}
public static FloatL Vector3L_Angle(Vector3L from, Vector3L to)
{
Debug.Log("x " + to.normalized.x + " y " + to.normalized.y + " z " + to.normalized.z);
FloatL dot = Vector3L.Dot(from.normalized, to.normalized);
Debug.Log("dotL " + dot);
FloatL acos = FixPointMath.Acos(FixPointMath.Clamp(dot, -1f, 1f));
Debug.Log("acosL " + acos);
return(acos * 57.29578d);
}
static void ToAxisAngleRad(QuaternionL q, out Vector3L axis, out FloatL angle)
{
if (FixPointMath.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * FixPointMath.Acos(q.w); // angle
FloatL den = FixPointMath.Sqrt(1.0f - q.w * q.w);
if (den > 0.0001f)
{
axis = q.xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = new Vector3L(1, 0, 0);
}
}
public static FloatL Angle(Vector2L from, Vector2L to)
{
return(FixPointMath.Acos(FixPointMath.Clamp(Vector2L.Dot(from.normalized, to.normalized), -1, 1)) * 57.29578d);
}
private static QuaternionL SlerpUnclamped(QuaternionL a, QuaternionL b, FloatL t)
{
// if either input is zero, return the other.
if (a.LengthSquared == 0.0f)
{
if (b.LengthSquared == 0.0f)
{
return(identity);
}
return(b);
}
else if (b.LengthSquared == 0.0f)
{
return(a);
}
FloatL cosHalfAngle = a.w * b.w + Vector3L.Dot(a.xyz, b.xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(a);
}
else if (cosHalfAngle < 0.0f)
{
b.xyz = -b.xyz;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
FloatL blendA;
FloatL blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
FloatL halfAngle = FixPointMath.Acos(cosHalfAngle);
FloatL sinHalfAngle = FixPointMath.Sin(halfAngle);
FloatL oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = FixPointMath.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
blendB = FixPointMath.Sin(halfAngle * t) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
QuaternionL result = new QuaternionL(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
public static FloatL Angle(QuaternionL a, QuaternionL b)
{
FloatL f = QuaternionL.Dot(a, b);
return(FixPointMath.Acos(FixPointMath.Min(FixPointMath.Abs(f), 1f)) * 2f * 57.29578d);
}
