/// <summary>
/// 四元数转矩阵
/// </summary>
/// <param name="m">输出矩阵</param>
/// <param name="q">输入四元数</param>
/// <returns></returns>
public static FixedMatrix4x4 QuatMatrix(this FixedMatrix4x4 m, FixedQuaternion q)
{
Fixed xx, xy, xz, xw;
Fixed yy, yz, yw;
Fixed zz, zw;
xx = q[0] * q[0];
xy = q[0] * q[1];
xz = q[0] * q[2];
xw = q[0] * q[3];
yy = q[1] * q[1];
yz = q[1] * q[2];
yw = q[1] * q[3];
zz = q[2] * q[2];
zw = q[2] * q[3];
m.m00 = 1 - 2 * (yy + zz);
m.m01 = 2 * (xy - zw);
m.m02 = 2 * (xz + yw);
m.m03 = 0;
m.m10 = 2 * (xy + zw);
m.m11 = 1 - 2 * (xx + zz);
m.m12 = 2 * (yz - xw);
m.m13 = 0;
m.m20 = 2 * (xz - yw);
m.m21 = 2 * (yz + xw);
m.m22 = 1 - 2 * (xx + yy);
m.m23 = 0;
m.m30 = 0;
m.m31 = 0;
m.m32 = 0;
m.m33 = 1;
return(m);
}
/// <summary>
/// 欧拉角转矩阵
/// </summary>
/// <param name="angles"></param>
/// <param name="m"></param>
/// <returns></returns>
public static FixedMatrix4x4 AngleMatrix(this FixedMatrix4x4 m, FixedVector3 angles)
{
Fixed cx, sx, cy, sy, cz, sz;
Fixed yx, yy;
cx = FixedMathf.Cos(angles.x * FixedMathf.Deg2Rad);
sx = FixedMathf.Sin(angles.x * FixedMathf.Deg2Rad);
cy = FixedMathf.Cos(angles.y * FixedMathf.Deg2Rad);
sy = FixedMathf.Sin(angles.y * FixedMathf.Deg2Rad);
cz = FixedMathf.Cos(angles.z * FixedMathf.Deg2Rad);
sz = FixedMathf.Sin(angles.z * FixedMathf.Deg2Rad);
yx = sy * cx;
yy = sy * sx;
m.m00 = cy * cz;
m.m01 = -cy * sz;
m.m02 = sy;
m.m03 = 0;
m.m10 = yy * cz + cx * sz;
m.m11 = -yy * sz + cx * cz;
m.m12 = -sx;
m.m13 = 0;
m.m20 = -yx * cz + sx * sz;
m.m21 = yx * sz + sx * cz;
m.m22 = cx * cy;
m.m23 = 0;
m.m30 = 0;
m.m31 = 0;
m.m32 = 0;
m.m33 = 1;
return(m);
}
/// <summary>
/// 矩阵转欧拉角
/// </summary>
/// <param name="m"></param>
/// <param name="angles">3个角度</param>
/// <returns></returns>
public static FixedVector3 MatrixAngle(this FixedMatrix4x4 m, FixedVector3 angles)
{
Fixed c;
Fixed tx, ty;
angles = new FixedVector3();
angles.y = FixedMathf.Asin(m.m02);
c = FixedMathf.Cos(angles.y);
if (FixedMathf.Abs(c) > 0.005f)
{
tx = m.m22 / c;
ty = -m.m12 / c;
angles.x = FixedMathf.Atan2(ty, tx);
tx = m.m00 / c;
ty = -m.m01 / c;
angles.z = FixedMathf.Atan2(ty, tx);
}
else
{
angles.x = 0;
tx = m.m11;
ty = m.m10;
angles.z = FixedMathf.Atan2(ty, tx);
}
return(angles);
}
/// <summary>
/// 矩阵转四元数
/// </summary>
/// <param name="q">输出</param>
/// <param name="m">输入矩阵</param>
/// <returns></returns>
public static FixedQuaternion MatrixQuat(this FixedMatrix4x4 m)
{
FixedQuaternion q = new FixedQuaternion();
Fixed s;
Fixed F25 = new Fixed(25, 100);
if (m.m00 > m.m11 && m.m00 > m.m22)
{
s = 2 * FixedMathf.Sqrt(1f + m.m00 - m.m11 - m.m22);
q[0] = F25 * s;
q[1] = (m.m10 + m.m01) / s;
q[2] = (m.m02 + m.m20) / s;
q[3] = (m.m21 - m.m12) / s;
}
else if (m.m11 > m.m22)
{
s = 2 * FixedMathf.Sqrt(1f + m.m11 - m.m00 - m.m22);
q[0] = (m.m10 + m.m01) / s;
q[1] = F25 * s;
q[2] = (m.m21 + m.m12) / s;
q[3] = (m.m02 - m.m20) / s;
}
else
{
s = 2 * FixedMathf.Sqrt(1f + m.m22 - m.m00 - m.m11);
q[0] = (m.m02 + m.m20) / s;
q[1] = (m.m21 + m.m12) / s;
q[2] = F25 * s;
q[3] = (m.m10 - m.m01) / s;
}
return(q);
}
/// <summary>
/// 围绕指定轴旋转
/// </summary>
/// <param name="m">输出矩阵</param>
/// <param name="axis">轴向量</param>
/// <param name="angle">角度</param>
/// <returns></returns>
public static FixedMatrix4x4 MatrixRotation(this FixedMatrix4x4 m, FixedVector3 axis, Fixed angle)
{
m.m00 = axis.x * axis.x * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
m.m01 = axis.x * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + axis.z * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m02 = axis.x * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.y * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m10 = axis.x * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.z * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m11 = axis.y * axis.y * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
m.m12 = axis.y * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + axis.x * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m20 = axis.x * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - axis.y * FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m21 = axis.y * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) - FixedMathf.Sin(angle * FixedMathf.Deg2Rad);
m.m22 = axis.z * axis.z * (1 - FixedMathf.Cos(angle * FixedMathf.Deg2Rad)) + FixedMathf.Cos(angle * FixedMathf.Deg2Rad);
return(m);
}
