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