/// <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>
/// <para>Clamps the Vector2Int to the bounds given by min and max.</para>
/// </summary>
/// <param name="min"></param>
/// <param name="max"></param>
public void Clamp(LVector2Int min, LVector2Int max)
{
this.x = LMath.Max(min.x, this.x);
this.x = LMath.Min(max.x, this.x);
this.y = LMath.Max(min.y, this.y);
this.y = LMath.Min(max.y, this.y);
}
/// <summary>
///
/// </summary>
/// <param name="deg"> 弧度</param>
/// <returns></returns>
public LVector2 Rotate(LFloat deg)
{
var rad = LMath.Deg2Rad * deg;
LFloat cos, sin;
LMath.SinCos(out cos, out sin, rad);
return(new LVector2(x * cos - y * sin, x * sin + y * cos));
}
public static int Sqrt(int a)
{
if (a <= 0)
{
return(0);
}
return((int)LMath.Sqrt32((uint)a));
}
public static int Sqrt(long a)
{
if (a <= 0L)
{
return(0);
}
if (a <= (long)(0xffffffffu))
{
return((int)LMath.Sqrt32((uint)a));
}
return((int)LMath.Sqrt64((ulong)a));
}
/// <summary>
/// 3*3 矩阵转化为四元数
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
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);
}
public LVector3 RotateY(LFloat degree)
{
LFloat s;
LFloat c;
LMath.SinCos(out s, out c, new LFloat(true, degree._val * 31416L / 1800000L));
LVector3 vInt;
vInt._x = (int)(((long)this._x * s._val + (long)this._z * c._val) / LFloat.Precision);
vInt._z = (int)(((long)this._x * -c._val + (long)this._z * s._val) / LFloat.Precision);
vInt._y = 0;
return(vInt.normalized);
}
/// <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;
}
public void Normalize()
{
long num = (long)(this._x * 100);
long num2 = (long)(this._y * 100);
long num3 = num * num + num2 * num2;
if (num3 == 0L)
{
return;
}
long b = (long)LMath.Sqrt(num3);
this._x = (int)(num * 1000L / b);
this._y = (int)(num2 * 1000L / b);
}
/// <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="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 LVector3 Normalize(LFloat newMagn)
{
long num = (long)(this._x * 100);
long num2 = (long)(this._y * 100);
long num3 = (long)(this._z * 100);
long num4 = num * num + num2 * num2 + num3 * num3;
if (num4 == 0L)
{
return(this);
}
long b = (long)LMath.Sqrt(num4);
long num5 = newMagn._val;
this._x = (int)(num * num5 / b);
this._y = (int)(num2 * num5 / b);
this._z = (int)(num3 * num5 / b);
return(this);
}
private LVector3 MatrixToEuler(LMatrix33 m)
{
LVector3 v = new LVector3();
if (m[1, 2] < 1)
{
if (m[1, 2] > -1)
{
v.x = LMath.Asin(-m[1, 2]);
v.y = LMath.Atan2(m[0, 2], m[2, 2]);
v.z = LMath.Atan2(m[1, 0], m[1, 1]);
}
else
{
v.x = LMath.PI * LFloat.half;
v.y = LMath.Atan2(m[0, 1], m[0, 0]);
v.z = (LFloat)0;
}
}
else
{
v.x = -LMath.PI * LFloat.half;
v.y = LMath.Atan2(-m[0, 1], m[0, 0]);
v.z = (LFloat)0;
}
for (int i = 0; i < 3; i++)
{
if (v[i] < 0)
{
v[i] += LMath.PI2;
}
else if (v[i] > LMath.PI2)
{
v[i] -= LMath.PI2;
}
}
return(v);
}
/// <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));
}
public static LFloat Sqrt(LFloat val)
{
return(LMath.Sqrt(val));
}
/// <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));
}
public static LVector2 Max(LVector2 a, LVector2 b)
{
return(new LVector2(true, LMath.Max(a._x, b._x), LMath.Max(a._y, b._y)));
}
public void Max(ref LVector2 r)
{
this._x = LMath.Max(this._x, r._x);
this._y = LMath.Max(this._y, r._y);
}
