/// <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));
}
}
public void SetDeg(LFloat rdeg)
{
deg = rdeg;
var rad = LMath.Deg2Rad * deg;
var c = LMath.Cos(rad);
var s = LMath.Sin(rad);
up = new LVector2(-s, c);
}
public static LQuaternion AngleAxis(LFloat angle, LVector3 axis)
{
LFloat radian2 = angle * 0.5d * LMath.DegToRad;
LFloat sina = LMath.Sin(radian2);
LFloat cosa = LMath.Cos(radian2);
axis = axis.normalized;
return(new LQuaternion(sina * axis.x, sina * axis.y, sina * axis.z, cosa));
}
public static LVector3 Slerp(LVector3 from, LVector3 to, LFloat t)
{
t = LMath.Clamp(t, 0, 1);
LFloat diff = Angle(from, to) * LMath.DegToRad;
LFloat sind = LMath.Sin(diff);
LFloat sintd = LMath.Sin(t * diff);
LFloat sin1td = LMath.Sin((1 - t) * diff);
return((sin1td / sind) * from + (sintd / sind) * to);
}
public OBB(LVector2 pos, LVector2 size, LFloat deg)
{
this.pos = pos;
this.size = size;
radius = size.magnitude;
this.deg = deg;
var rad = LMath.Deg2Rad * deg;
var c = LMath.Cos(rad);
var s = LMath.Sin(rad);
up = new LVector2(-s, c);
}
/// <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);
}
public static LQuaternion Euler(LVector3 euler)
{
LFloat ex = euler.x * LMath.DegToRad / 2;
LFloat ey = euler.y * LMath.DegToRad / 2;
LFloat ez = euler.z * LMath.DegToRad / 2;
LFloat sinex = LMath.Sin(ex);
LFloat siney = LMath.Sin(ey);
LFloat sinez = LMath.Sin(ez);
LFloat cosex = LMath.Cos(ex);
LFloat cosey = LMath.Cos(ey);
LFloat cosez = LMath.Cos(ez);
LQuaternion rotation;
rotation.x = sinez * cosex * cosey - cosez * sinex * siney;
rotation.y = cosez * sinex * cosey + sinez * cosex * siney;
rotation.z = cosez * cosex * siney - sinez * sinex * cosey;
rotation.w = cosez * cosex * cosey + sinez * sinex * siney;
rotation.Normalize();
return(rotation);
}
public LVector3 GetPosition(LFloat timer)
{
var rad = (InitDeg + ((timer / Interval) - (timer / Interval).Floor()) * 360) * LMath.Deg2Rad;
return(InitPos + new LVector3(LMath.Cos(rad), 0, LMath.Sin(rad)) * Radius);
}
