public static LMatrix33 QuaternionToMatrix(LQuaternion quat)
{
LMatrix33 m = new LMatrix33();
LFloat x = quat.x * 2;
LFloat y = quat.y * 2;
LFloat z = quat.z * 2;
LFloat xx = quat.x * x;
LFloat yy = quat.y * y;
LFloat zz = quat.z * z;
LFloat xy = quat.x * y;
LFloat xz = quat.x * z;
LFloat yz = quat.y * z;
LFloat wx = quat.w * x;
LFloat wy = quat.w * y;
LFloat wz = quat.w * z;
m[0] = 1 - (yy + zz);
m[1] = xy + wz;
m[2] = xz - wy;
m[3] = xy - wz;
m[4] = 1 - (xx + zz);
m[5] = yz + wx;
m[6] = xz + wy;
m[7] = yz - wx;
m[8] = 1 - (xx + yy);
return(m);
}
/// <summary>
/// 球形插值(无限制)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
public static LQuaternion SlerpUnclamped(LQuaternion q1, LQuaternion q2, LFloat t)
{
LFloat dot = Dot(q1, q2);
LQuaternion tmpQuat = new LQuaternion();
if (dot < 0)
{
dot = -dot;
tmpQuat.Set(-q2.x, -q2.y, -q2.z, -q2.w);
}
else
{
tmpQuat = q2;
}
if (dot < 1)
{
LFloat angle = LMath.Acos(dot);
LFloat sinadiv, sinat, sinaomt;
sinadiv = 1 / LMath.Sin(angle);
sinat = LMath.Sin(angle * t);
sinaomt = LMath.Sin(angle * (1 - t));
tmpQuat.Set((q1.x * sinaomt + tmpQuat.x * sinat) * sinadiv,
(q1.y * sinaomt + tmpQuat.y * sinat) * sinadiv,
(q1.z * sinaomt + tmpQuat.z * sinat) * sinadiv,
(q1.w * sinaomt + tmpQuat.w * sinat) * sinadiv);
return(tmpQuat);
}
else
{
return(Lerp(q1, tmpQuat, t));
}
}
private static LQuaternion MatrixToQuaternion(LMatrix33 m)
{
LQuaternion quat = new LQuaternion();
LFloat fTrace = m[0, 0] + m[1, 1] + m[2, 2];
LFloat root;
if (fTrace > 0)
{
root = LMath.Sqrt(fTrace + 1);
quat.w = LFloat.half * root;
root = LFloat.half / root;
quat.x = (m[2, 1] - m[1, 2]) * root;
quat.y = (m[0, 2] - m[2, 0]) * root;
quat.z = (m[1, 0] - m[0, 1]) * root;
}
else
{
int[] s_iNext = new int[] { 1, 2, 0 };
int i = 0;
if (m[1, 1] > m[0, 0])
{
i = 1;
}
if (m[2, 2] > m[i, i])
{
i = 2;
}
int j = s_iNext[i];
int k = s_iNext[j];
root = LMath.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
if (root < 0)
{
throw new IndexOutOfRangeException("error!");
}
quat[i] = LFloat.half * root;
root = LFloat.half / root;
quat.w = (m[k, j] - m[j, k]) * root;
quat[j] = (m[j, i] + m[i, j]) * root;
quat[k] = (m[k, i] + m[i, k]) * root;
}
LFloat nor = LMath.Sqrt(Dot(quat, quat));
quat = new LQuaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor);
return(quat);
}
/// <summary>
/// 球形插值
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
public static LQuaternion Slerp(LQuaternion a, LQuaternion b, LFloat t)
{
if (t > 1)
{
t = LFloat.one;
}
if (t < 0)
{
t = LFloat.zero;
}
return(SlerpUnclamped(a, b, t));
}
/// <summary>
/// 轴向旋转
/// </summary>
/// <param name="angle"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static LQuaternion AngleAxis(LFloat angle, LVector3 axis)
{
axis = axis.normalized;
angle = angle * LMath.Deg2Rad;
LQuaternion q = new LQuaternion();
LFloat halfAngle = angle * LFloat.half;
LFloat s = LMath.Sin(halfAngle);
q.w = LMath.Cos(halfAngle);
q.x = s * axis.x;
q.y = s * axis.y;
q.z = s * axis.z;
return(q);
}
/// <summary>
/// 向目标角度旋转
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="maxDegreesDelta"></param>
/// <returns></returns>
public static LQuaternion RotateTowards(LQuaternion from, LQuaternion to, LFloat maxDegreesDelta)
{
LFloat num = LQuaternion.Angle(from, to);
LQuaternion result = new LQuaternion();
if (num == 0)
{
result = to;
}
else
{
LFloat t = LMath.Min(LFloat.one, maxDegreesDelta / num);
result = LQuaternion.SlerpUnclamped(from, to, t);
}
return(result);
}
/// <summary>
/// 欧拉角转四元数
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <returns></returns>
public static LQuaternion Euler(LFloat x, LFloat y, LFloat z)
{
LFloat cX = LMath.Cos(x * LMath.PI / 360);
LFloat sX = LMath.Sin(x * LMath.PI / 360);
LFloat cY = LMath.Cos(y * LMath.PI / 360);
LFloat sY = LMath.Sin(y * LMath.PI / 360);
LFloat cZ = LMath.Cos(z * LMath.PI / 360);
LFloat sZ = LMath.Sin(z * LMath.PI / 360);
LQuaternion qX = new LQuaternion(sX, LFloat.zero, LFloat.zero, cX);
LQuaternion qY = new LQuaternion(LFloat.zero, sY, LFloat.zero, cY);
LQuaternion qZ = new LQuaternion(LFloat.zero, LFloat.zero, sZ, cZ);
LQuaternion q = (qY * qX) * qZ;
return(q);
}
/// <summary>
/// 线性插值(无限制)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
public static LQuaternion LerpUnclamped(LQuaternion a, LQuaternion b, LFloat t)
{
LQuaternion tmpQuat = new LQuaternion();
if (Dot(a, b) < 0)
{
tmpQuat.Set(a.x + t * (-b.x - a.x),
a.y + t * (-b.y - a.y),
a.z + t * (-b.z - a.z),
a.w + t * (-b.w - a.w));
}
else
{
tmpQuat.Set(a.x + t * (b.x - a.x),
a.y + t * (b.y - a.y),
a.z + t * (b.z - a.z),
a.w + t * (b.w - a.w));
}
LFloat nor = LMath.Sqrt(Dot(tmpQuat, tmpQuat));
return(new LQuaternion(tmpQuat.x / nor, tmpQuat.y / nor, tmpQuat.z / nor, tmpQuat.w / nor));
}
/// <summary>
/// 四元数的逆
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
public static LQuaternion Inverse(LQuaternion rotation)
{
return(new LQuaternion(-rotation.x, -rotation.y, -rotation.z, rotation.w));
}
/// <summary>
/// 点乘
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static LFloat Dot(LQuaternion a, LQuaternion b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
/// <summary>
/// 夹角大小
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static LFloat Angle(LQuaternion a, LQuaternion b)
{
LFloat single = Dot(a, b);
return(LMath.Acos(LMath.Min(LMath.Abs(single), LFloat.one)) * 2 * (180 / LMath.PI));
}
