public void AsMatrix(out Int32Matrix4x4 mtx)
{
int x, y, z, w;
AsQuarternion(out x, out y, out z, out w);
// Theoretically 1024 * * 2, but may differ slightly due to rounding
var lsq = x * x + y * y + z * z + w * w;
// Quaternion components use 10 bits, so there's no risk of overflow
#pragma warning disable SA1115 // Allow blank lines to visually separate matrix rows
mtx = new Int32Matrix4x4(
lsq - 2 * (y * y + z * z),
2 * (x * y + z * w),
2 * (x * z - y * w),
0,
2 * (x * y - z * w),
lsq - 2 * (x * x + z * z),
2 * (y * z + x * w),
0,
2 * (x * z + y * w),
2 * (y * z - x * w),
lsq - 2 * (x * x + y * y),
0,
0,
0,
0,
lsq);
#pragma warning restore SA1115
}
public static float[] MakeFloatMatrix(Int32Matrix4x4 imtx)
{
var multipler = 1f / imtx.M44;
return(new float[]
{
imtx.M11 *multipler,
imtx.M12 *multipler,
imtx.M13 *multipler,
imtx.M14 *multipler,
imtx.M21 *multipler,
imtx.M22 *multipler,
imtx.M23 *multipler,
imtx.M24 *multipler,
imtx.M31 *multipler,
imtx.M32 *multipler,
imtx.M33 *multipler,
imtx.M34 *multipler,
imtx.M41 *multipler,
imtx.M42 *multipler,
imtx.M43 *multipler,
imtx.M44 *multipler,
});
}
public WVec Rotate(ref Int32Matrix4x4 mtx)
{
var lx = (long)X;
var ly = (long)Y;
var lz = (long)Z;
return(new WVec(
(int)((lx * mtx.M11 + ly * mtx.M21 + lz * mtx.M31) / mtx.M44),
(int)((lx * mtx.M12 + ly * mtx.M22 + lz * mtx.M32) / mtx.M44),
(int)((lx * mtx.M13 + ly * mtx.M23 + lz * mtx.M33) / mtx.M44)));
}
