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); } }