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 bool Point2dWithSector2d(Vector2L pos, Sector2d sector2d)
{
Vector2L distance = pos - sector2d.m_pos;
if (distance.magnitude > sector2d.m_radius)
{
return(false);
}
Vector3L sectorForward = RotateHelper.GetForward(sector2d.m_rotation);
FloatL cosTarget = Vector3L.Dot(sectorForward, distance) / sectorForward.magnitude / distance.magnitude;
FloatL cosHalfDegree = FixPointMath.Cos((FixPointMath.Deg2Rad * sector2d.m_theraDegree / 2));
return(cosTarget > cosHalfDegree);
}
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);
}
