mat3_t mat4_toInverseMat3(mat4_t mat, mat3_t dest) {
// Cache the matrix values (makes for huge speed increases!)
double a00 = mat[0], a01 = mat[1], a02 = mat[2],
a10 = mat[4], a11 = mat[5], a12 = mat[6],
a20 = mat[8], a21 = mat[9], a22 = mat[10],
b01 = a22 * a11 - a12 * a21,
b11 = -a22 * a10 + a12 * a20,
b21 = a21 * a10 - a11 * a20,
d = a00 * b01 + a01 * b11 + a02 * b21,
id;
if (!d) { return NULL; }
id = 1 / d;
if (!dest) { dest = mat3_create(NULL); }
dest[0] = b01 * id;
dest[1] = (-a22 * a01 + a02 * a21) * id;
dest[2] = (a12 * a01 - a02 * a11) * id;
dest[3] = b11 * id;
dest[4] = (a22 * a00 - a02 * a20) * id;
dest[5] = (-a12 * a00 + a02 * a10) * id;
dest[6] = b21 * id;
dest[7] = (-a21 * a00 + a01 * a20) * id;
dest[8] = (a11 * a00 - a01 * a10) * id;
return dest;
}
mat3_t quat_toMat3(quat_t quat, mat3_t dest) {
if (!dest) { dest = mat3_create(NULL); }
float x = quat[0], y = quat[1], z = quat[2], w = quat[3],
x2 = x + x,
y2 = y + y,
z2 = z + z,
xx = x * x2,
xy = x * y2,
xz = x * z2,
yy = y * y2,
yz = y * z2,
zz = z * z2,
wx = w * x2,
wy = w * y2,
wz = w * z2;
dest[0] = 1 - (yy + zz);
dest[1] = xy + wz;
dest[2] = xz - wy;
dest[3] = xy - wz;
dest[4] = 1 - (xx + zz);
dest[5] = yz + wx;
dest[6] = xz + wy;
dest[7] = yz - wx;
dest[8] = 1 - (xx + yy);
return dest;
}
mat3_t mat3_identity(mat3_t dest) {
if (!dest) { dest = mat3_create(NULL); }
dest[0] = 1;
dest[1] = 0;
dest[2] = 0;
dest[3] = 0;
dest[4] = 1;
dest[5] = 0;
dest[6] = 0;
dest[7] = 0;
dest[8] = 1;
return dest;
}
mat3_t mat4_toMat3(mat4_t mat, mat3_t dest) {
if (!dest) { dest = mat3_create(NULL); }
dest[0] = mat[0];
dest[1] = mat[1];
dest[2] = mat[2];
dest[3] = mat[4];
dest[4] = mat[5];
dest[5] = mat[6];
dest[6] = mat[8];
dest[7] = mat[9];
dest[8] = mat[10];
return dest;
}