/// <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));
}
}
/// <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="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);
}