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 QuaternionL FromEulerRad(Vector3L euler)
{
FloatL yaw = euler.z;
FloatL pitch = euler.x;
FloatL roll = euler.y;
FloatL yawOver2 = yaw * 0.5f;
FloatL sinYawOver2 = FixPointMath.Sin(yawOver2);
FloatL cosYawOver2 = FixPointMath.Cos(yawOver2);
FloatL pitchOver2 = pitch * 0.5f;
FloatL sinPitchOver2 = FixPointMath.Sin(pitchOver2);
FloatL cosPitchOver2 = FixPointMath.Cos(pitchOver2);
FloatL rollOver2 = roll * 0.5f;
FloatL sinRollOver2 = FixPointMath.Sin(rollOver2);
FloatL cosRollOver2 = FixPointMath.Cos(rollOver2);
QuaternionL result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
result.z = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
return(result);
}
public static QuaternionL AngleAxis(FloatL degress, Vector3L axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
QuaternionL result = identity;
FloatL radians = degress * FixPointMath.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * FixPointMath.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = FixPointMath.Cos(radians);
return(Normalize(result));
}
static Vector3L RotateTo(Vector3L from, Vector3L to, FloatL angle)
{
//如果两向量角度为0
if (Vector3L.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3L n = Vector3L.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4L rotateMatrix = new Matrix4x4L();
//旋转的弧度
double radian = (angle * FixPointMath.PI / 180).ToDouble();
FloatL cosAngle = FixPointMath.Cos(radian);
FloatL sinAngle = FixPointMath.Sin(radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4L(n.x * n.x * (1 - cosAngle) + cosAngle, n.x * n.y * (1 - cosAngle) + n.z * sinAngle, n.x * n.z * (1 - cosAngle) - n.y * sinAngle, 0));
rotateMatrix.SetRow(1, new Vector4L(n.x * n.y * (1 - cosAngle) - n.z * sinAngle, n.y * n.y * (1 - cosAngle) + cosAngle, n.y * n.z * (1 - cosAngle) + n.x * sinAngle, 0));
rotateMatrix.SetRow(2, new Vector4L(n.x * n.z * (1 - cosAngle) + n.y * sinAngle, n.y * n.z * (1 - cosAngle) - n.x * sinAngle, n.z * n.z * (1 - cosAngle) + cosAngle, 0));
rotateMatrix.SetRow(3, new Vector4L(0, 0, 0, 1));
Vector4L v = Vector3L.ToVector4(from);
Vector3L vector = new Vector3L();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; j++)
{
vector[i] += v[j] * rotateMatrix[j, i];
}
}
return(vector);
}
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);
}
}
