/// <summary>
/// 四元数变化为3*3矩阵
/// </summary>
/// <param name="quat"></param>
/// <returns></returns>
public static LMatrix33 QuaternionToMatrix(LQuaternion quat)
{
LMatrix33 m = new LMatrix33();
LFloat x = quat.x * 2;
LFloat y = quat.y * 2;
LFloat z = quat.z * 2;
LFloat xx = quat.x * x;
LFloat yy = quat.y * y;
LFloat zz = quat.z * z;
LFloat xy = quat.x * y;
LFloat xz = quat.x * z;
LFloat yz = quat.y * z;
LFloat wx = quat.w * x;
LFloat wy = quat.w * y;
LFloat wz = quat.w * z;
m[0] = 1 - (yy + zz);
m[1] = xy + wz;
m[2] = xz - wy;
m[3] = xy - wz;
m[4] = 1 - (xx + zz);
m[5] = yz + wx;
m[6] = xz + wy;
m[7] = yz - wx;
m[8] = 1 - (xx + yy);
return(m);
}
/// <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);
}
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="viewVec"></param>
/// <param name="upVec"></param>
/// <returns></returns>
private static LMatrix33 LookRotationToMatrix(LVector3 viewVec, LVector3 upVec)
{
LVector3 z = viewVec;
LMatrix33 m = new LMatrix33();
LFloat mag = z.magnitude;
if (mag <= 0)
{
m = LMatrix33.identity;
}
z /= mag;
LVector3 x = Cross(upVec, z);
mag = x.magnitude;
if (mag <= 0)
{
m = LMatrix33.identity;
}
x /= mag;
LVector3 y = Cross(z, x);
m[0, 0] = x.x;
m[0, 1] = y.x;
m[0, 2] = z.x;
m[1, 0] = x.y;
m[1, 1] = y.y;
m[1, 2] = z.y;
m[2, 0] = x.z;
m[2, 1] = y.z;
m[2, 2] = z.z;
return(m);
}
/// <summary>
/// 注视旋转
/// </summary>
/// <param name="forward"></param>
/// <param name="upwards"></param>
/// <returns></returns>
public static LQuaternion LookRotation(LVector3 forward, LVector3 upwards)
{
LMatrix33 m = LookRotationToMatrix(forward, upwards);
return(MatrixToQuaternion(m));
}
