public static Quaternion EulerToQuaternion(Vector3 eulerAngle) { //角度转弧度 eulerAngle = MoMath.Deg2Rad(eulerAngle); float cX = MathF.Cos(eulerAngle.X / 2.0f); float sX = MathF.Sin(eulerAngle.X / 2.0f); float cY = MathF.Cos(eulerAngle.Y / 2.0f); float sY = MathF.Sin(eulerAngle.Y / 2.0f); float cZ = MathF.Cos(eulerAngle.Z / 2.0f); float sZ = MathF.Sin(eulerAngle.Z / 2.0f); Quaternion qX = new Quaternion(sX, 0, 0, cX); Quaternion qY = new Quaternion(0, sY, 0, cY); Quaternion qZ = new Quaternion(0, 0, sZ, cZ); Quaternion q = (qY * qX) * qZ; #if DEBUG if (!MoMath.CompareApproximate(q.LengthSquared(), 1f)) { string msg = string.Format($"EulerToQuaternion failed, {q.X} {q.Y} {q.Z} {q.W} sqrLength={q.LengthSquared()}"); MoLog.Log(ELogType.Assert, msg); } #endif return(q); }
// Right handed public static bool LookRotationToMatrix(Vector3 viewVec, Vector3 upVec, out Matrix3x3 m) { m = Matrix3x3.Identity; Vector3 z = viewVec; // compute u0 float mag = z.Length(); if (mag < Vector3Helper.Epsilon) { return(false); } z /= mag; Vector3 x = Vector3.Cross(upVec, z); mag = x.Length(); if (mag < Vector3Helper.Epsilon) { return(false); } x /= mag; Vector3 y = Vector3.Cross(z, x); if (!MoMath.CompareApproximate(y.Length(), 1.0F)) { return(false); } m.SetOrthoNormalBasis(x, y, z); return(true); }
public static Vector3 QuaternionToEuler(Quaternion quat) { Matrix3x3 m = QuaternionToMatrix(quat); Vector3 euler = MatrixToEuler(m); //弧度转角度 return(MoMath.Rad2Deg(euler)); }
/// <summary> /// check Quaternion is invalid. if value is NaN or +/- infinity /// </summary> public static void CheckInvalid(Quaternion q) { if (MoMath.IsInfinity(q)) { MoLog.Log(ELogType.Assert, "Quaternion isInfinity"); } if (MoMath.IsNaN(q)) { MoLog.Log(ELogType.Assert, "Quaternion isNaN"); } }
/// <summary> /// Rotates the transform around axis by angle /// </summary> public void Rotate(Vector3 worldAxis, float degreeAngle) { float radianAngle = MoMath.Deg2Rad(degreeAngle); Vector3 localAxis = InverseTransformDirection(worldAxis); if (localAxis.LengthSquared() > Vector3Helper.Epsilon) { localAxis = Vector3Helper.NormalizeSafe(localAxis); Quaternion q = QuaternionHelper.AxisAngleToQuaternionSafe(localAxis, radianAngle); Rotation = QuaternionHelper.NormalizeSafe(_rotation * q); } }
/// <summary> /// Rotates the transform around axis by angle /// </summary> public void Rotate(Vector3 worldPoint, Vector3 worldAxis, float degreeAngle) { // 先移动位置 Quaternion q = QuaternionHelper.AxisAngleToQuaternionSafe(worldAxis, MoMath.Deg2Rad(degreeAngle)); Vector3 dif = _position - worldPoint; dif = QuaternionHelper.RotateVectorByQuat(q, dif); Position = worldPoint + dif; // 再移动朝向 Rotate(worldAxis, degreeAngle); }
/// <summary> /// check Vector3 is invalid. if value is NaN or +/- infinity /// </summary> public static void CheckInvalid(Vector3 v) { if (MoMath.IsInfinity(v)) { MoLog.Log(ELogType.Assert, "Vector3 isInfinity"); } if (MoMath.IsNaN(v)) { MoLog.Log(ELogType.Assert, "Vector3 isNaN"); } }
private static Matrix3x3 QuaternionToMatrix(Quaternion q) { #if DEBUG // If q is guaranteed to be a unit quaternion, s will always be 1. // In that case, this calculation can be optimized out. if (!MoMath.CompareApproximate(q.LengthSquared(), 1.0F)) { string msg = string.Format($"QuaternionToMatrix conversion failed, because input Quaternion is invalid {q.X} {q.Y} {q.Z} {q.W} sqrLength={q.LengthSquared()}"); MoLog.Log(ELogType.Assert, msg); } #endif // Precalculate coordinate products float x = q.X * 2.0F; float y = q.Y * 2.0F; float z = q.Z * 2.0F; float xx = q.X * x; float yy = q.Y * y; float zz = q.Z * z; float xy = q.X * y; float xz = q.X * z; float yz = q.Y * z; float wx = q.W * x; float wy = q.W * y; float wz = q.W * z; // Calculate 3x3 matrix from orthonormal basis Matrix3x3 m = Matrix3x3.Identity; m.Data[0] = 1.0f - (yy + zz); m.Data[1] = xy + wz; m.Data[2] = xz - wy; m.Data[3] = xy - wz; m.Data[4] = 1.0f - (xx + zz); m.Data[5] = yz + wx; m.Data[6] = xz + wy; m.Data[7] = yz - wx; m.Data[8] = 1.0f - (xx + yy); return(m); }
public static float Angle(Vector3 lhs, Vector3 rhs) { float lhsMag = lhs.Length(); float rhsMag = rhs.Length(); // both vectors are non-zero if (lhsMag > Epsilon && rhsMag > Epsilon) { Vector3 lhsNorm = lhs / lhsMag; Vector3 rhsNorm = rhs / rhsMag; float dot = Vector3.Dot(lhsNorm, rhsNorm); dot = MoMath.Clamp(dot, -1f, 1f); //如果MathF.Acos()传入的值大于1f,该方法会返回NAN float radians = MathF.Acos(dot); return(MoMath.Rad2Deg(radians)); } else { return(0); } }
private static Quaternion MatrixToQuaternion(Matrix3x3 kRot) { Quaternion q = new Quaternion(); // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternionf Calculus and Fast Animation". #if DEBUG float det = kRot.GetDeterminant(); if (!MoMath.CompareApproximate(det, 1.0F, .005f)) { MoLog.Log(ELogType.Assert, "MatrixToQuaternion det assert."); } #endif float fTrace = kRot.Get(0, 0) + kRot.Get(1, 1) + kRot.Get(2, 2); float fRoot; if (fTrace > 0.0f) { // |w| > 1/2, may as well choose w > 1/2 fRoot = MathF.Sqrt(fTrace + 1.0f); // 2w q.W = 0.5f * fRoot; fRoot = 0.5f / fRoot; // 1/(4w) q.X = (kRot.Get(2, 1) - kRot.Get(1, 2)) * fRoot; q.Y = (kRot.Get(0, 2) - kRot.Get(2, 0)) * fRoot; q.Z = (kRot.Get(1, 0) - kRot.Get(0, 1)) * fRoot; } else { // |w| <= 1/2 int[] s_iNext = new int[3] { 1, 2, 0 }; int i = 0; if (kRot.Get(1, 1) > kRot.Get(0, 0)) { i = 1; } if (kRot.Get(2, 2) > kRot.Get(i, i)) { i = 2; } int j = s_iNext[i]; int k = s_iNext[j]; fRoot = MathF.Sqrt(kRot.Get(i, i) - kRot.Get(j, j) - kRot.Get(k, k) + 1.0f); float[] apkQuat = new float[3] { q.X, q.Y, q.Z }; #if DEBUG if (fRoot < Vector3Helper.Epsilon) { MoLog.Log(ELogType.Assert, "MatrixToQuaternion fRoot assert."); } #endif apkQuat[i] = 0.5f * fRoot; fRoot = 0.5f / fRoot; q.W = (kRot.Get(k, j) - kRot.Get(j, k)) * fRoot; apkQuat[j] = (kRot.Get(j, i) + kRot.Get(i, j)) * fRoot; apkQuat[k] = (kRot.Get(k, i) + kRot.Get(i, k)) * fRoot; q.X = apkQuat[0]; q.Y = apkQuat[1]; q.Z = apkQuat[2]; } q = Quaternion.Normalize(q); return(q); }