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