/// <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>
public void ToAngleAxis(out LFloat angle, out LVector3 axis)
{
angle = 2 * LMath.Acos(w);
if (angle == 0)
{
axis = LVector3.right;
return;
}
LFloat div = 1 / LMath.Sqrt(1 - w * w);
axis = new LVector3(x * div, y * div, z * div);
angle = angle * 180 / LMath.PI;
}
/// <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));
}