OpenGl rotate custom implementation - opengl

I'm trying to code my custom implementation of Opengl glRotatef(angle,x,y,z) function.
I wrote the rotation matrix, but when I try to use it, the effect is not the same as the original function. Here is my code;
void mglRotate(float angle, float x, float y, float z)
{
float angle_rad = angle * (PI/180.0f);
float c = cos(angle_rad);
float s = sin(angle_rad);
float t = 1 - c;
float m[16] = {
c+x*x*t,y*x*t+z*s,z*x*t-y*s,0,
x*y*t-z*s,c+y*y*t,z*y*t+x*s,0,
x*z*t+y*s,y*z*t-x*s,z*z*t+c,0,
0,0,0,1
};
glMultMatrixf(m);
}
Where is my mistake?

There is a library glm, that does exactly the same thing as old openGL functions. You can compare your implementation with implementation in glm and figure it out :)
template <typename T>
GLM_FUNC_QUALIFIER detail::tmat4x4<T> rotate
(
detail::tmat4x4<T> const & m,
T const & angle,
detail::tvec3<T> const & v
)
{
T a = radians(angle);
T c = cos(a);
T s = sin(a);
detail::tvec3<T> axis = normalize(v);
detail::tvec3<T> temp = (T(1) - c) * axis;
detail::tmat4x4<T> Rotate(detail::tmat4x4<T>::null);
Rotate[0][0] = c + temp[0] * axis[0];
Rotate[0][1] = 0 + temp[0] * axis[1] + s * axis[2];
Rotate[0][2] = 0 + temp[0] * axis[2] - s * axis[1];
Rotate[1][0] = 0 + temp[1] * axis[0] - s * axis[2];
Rotate[1][1] = c + temp[1] * axis[1];
Rotate[1][2] = 0 + temp[1] * axis[2] + s * axis[0];
Rotate[2][0] = 0 + temp[2] * axis[0] + s * axis[1];
Rotate[2][1] = 0 + temp[2] * axis[1] - s * axis[0];
Rotate[2][2] = c + temp[2] * axis[2];
detail::tmat4x4<T> Result(detail::tmat4x4<T>::null);
Result[0] = m[0] * Rotate[0][0] + m[1] * Rotate[0][1] + m[2] * Rotate[0][2];
Result[1] = m[0] * Rotate[1][0] + m[1] * Rotate[1][1] + m[2] * Rotate[1][2];
Result[2] = m[0] * Rotate[2][0] + m[1] * Rotate[2][1] + m[2] * Rotate[2][2];
Result[3] = m[3];
return Result;
}
The one thing that seems wrong to me in your code is that you don't normalize the axis.

Related

Rotation Transformation implementation for OpenGL with 4x4 Matrix

So I am learning OpenGL and I'm try to project an image:
To be like
However my implentation is resulting in:
The original tutorial is using GLM mathematics library but I wanted to try and implement it myself.
Matrix44& translate(float xi, float y, float z)
{
x[0][3] = xi;
x[1][3] = y;
x[2][3] = z;
return *this;
}
Matrix44& rotateX(float angle)
{
assert(angle >= 0.0f && angle <= 360.0f);
if (angle == 0.0f || angle == 360.f)
return *this;
x[1][1] = cos(angle * RAD);
x[1][2] = sin(angle * RAD);
x[2][1] = -sin(angle * RAD);
x[2][2] = x[1][1];
return *this;
}
Matrix44& rotateY(float angle)
{
assert(angle >= 0.0f && angle <= 360.0f);
if (angle == 0.0f || angle == 360.f)
return *this;
x[0][0] = cos(angle * RAD);
x[0][2] = -sin(angle * RAD);
x[2][0] = sin(angle * RAD);
x[2][2] = x[0][0];
return *this;
}
Matrix44& rotateZ(float angle)
{
assert(angle >= 0.0f && angle <= 360.0f);
if (angle == 0.0f || angle == 360.f)
return *this;
x[0][0] = cos(angle * RAD);
x[0][1] = -sin(angle * RAD);
x[1][0] = sin(angle * RAD);
x[1][1] = x[0][0];
return *this;
}
x[][] is the 4x4 matrix. By default it is set to be the identity matrix. And in main:
Matrix44f model{};
Matrix44f view{};
model.rotateY(180).rotateX(90);
view.translate(0.0f, 0.0f, -3.0f);
To get my image but with the GLM version of:
glm::mat4 model;
glm::mat4 view;
model = glm::rotate( model, ( GLfloat)glfwGetTime( ) * 1.0f, glm::vec3( 0.5f, 1.0f, 0.0f ) );
view = glm::translate( view, glm::vec3( 0.0f, 0.0f, -3.0f ) );
I then looked at the GLM implementation: GLM Maths Github
template <typename T, precision P>
GLM_FUNC_QUALIFIER detail::tmat4x4<T, P> rotate
(
detail::tmat4x4<T, P> const & m,
T const & angle,
detail::tvec3<T, P> const & v
)
{
T const a = angle;
T const c = cos(a);
T const s = sin(a);
detail::tvec3<T, P> axis(normalize(v));
detail::tvec3<T, P> temp((T(1) - c) * axis);
detail::tmat4x4<T, P> Rotate(detail::tmat4x4<T, P>::_null);
Rotate[0][0] = c + temp[0] * axis[0];
Rotate[0][1] = 0 + temp[0] * axis[1] + s * axis[2];
Rotate[0][2] = 0 + temp[0] * axis[2] - s * axis[1];
Rotate[1][0] = 0 + temp[1] * axis[0] - s * axis[2];
Rotate[1][1] = c + temp[1] * axis[1];
Rotate[1][2] = 0 + temp[1] * axis[2] + s * axis[0];
Rotate[2][0] = 0 + temp[2] * axis[0] + s * axis[1];
Rotate[2][1] = 0 + temp[2] * axis[1] - s * axis[0];
Rotate[2][2] = c + temp[2] * axis[2];
detail::tmat4x4<T, P> Result(detail::tmat4x4<T, P>::_null);
Result[0] = m[0] * Rotate[0][0] + m[1] * Rotate[0][1] + m[2] * Rotate[0][2];
Result[1] = m[0] * Rotate[1][0] + m[1] * Rotate[1][1] + m[2] * Rotate[1][2];
Result[2] = m[0] * Rotate[2][0] + m[1] * Rotate[2][1] + m[2] * Rotate[2][2];
Result[3] = m[3];
return Result;
}
I'm so confused as to what the Result[index] accesses in the 4 x 4 matrix? Does it represent column, the row or the diagonal position?!

Rotation of a point about the z-axis

I have 3 vectors in 3D space. Let's call them xaxis, yaxis, and zaxis. These vectors are centered about an arbitrary point somewhere in 3D space. I am interested in rotating the xaxis and yaxis vectors about the zaxis vector a number of degrees θ.
For the following code with values being arbitrary and unimportant:
double xaxis[3], yaxis[3], zaxis[3], point[3], theta;
How would I go about rotating xaxis and yaxis about the zaxis by theta degrees?
Future Note: These attempts do not work. See my answer for the proper solution, which was found with the help of BlueRaja-DannyPflughoeft
My attempt at matrix-based rotation:
double rx[3][3];
double ry[3][3];
double rz[3][3];
double r[3][3];
rx[0][0] = 1;
rx[0][1] = 0;
rx[0][2] = 0;
rx[1][0] = 0;
rx[1][1] = cos(theta);
rx[1][2] = sin(theta);
rx[2][0] = 0;
rx[2][1] = -1.0 * sin(theta);
rx[2][2] = cos(theta);
ry[0][0] = cos(theta);
ry[0][1] = 0;
ry[0][2] = -1.0 * sin(theta);
ry[1][0] = 0;
ry[1][1] = 1;
ry[1][2] = 0;
ry[2][0] = sin(theta);
ry[2][1] = 0;
ry[2][2] = cos(theta);
//No rotation wanted on the zaxis
rz[0][0] = cos(0);
rz[0][1] = sin(0);
rz[0][2] = 0;
rz[1][0] = -1.0 * sin(0);
rz[1][1] = cos(0);
rz[1][2] = 0;
rz[2][0] = 0;
rz[2][1] = 0;
rz[2][2] = 1;
vtkMath::Multiply3x3(rx, ry, r); //Multiplies rx by ry and stores into r
vtkMath::Multiply3x3(r, rz, r); //Multiplies r by rz and stores into r
vtkMath::Multiply3x3(r, xaxis, xaxis);//multiplies a 3x3 by a 3x1
vtkMath::Multiply3x3(r, yaxis, yaxis);//multiplies a 3x3 by a 3x1
This attempt only worked when the plane was in the x-y plane:
double x, y;
x = xaxis[0];
y = xaxis[1];
xaxis[0] = x * cos(theta) - y * sin(theta);
xaxis[1] = x * sin(theta) + y * cos(theta);
x = yaxis[0];
y = yaxis[1];
yaxis[0] = x * cos(theta) - y * sin(theta);
yaxis[1] = x * sin(theta) + y * cos(theta);
Using the axis-angle approach given by BlueRaja-DannyPflughoeft:
double c = cos(theta);
double s = sin(theta);
double C = 1.0 - c;
double Q[3][3];
Q[0][0] = xaxis[0] * xaxis[0] * C + c;
Q[0][1] = xaxis[1] * xaxis[0] * C + xaxis[2] * s;
Q[0][2] = xaxis[2] * xaxis[0] * C - xaxis[1] * s;
Q[1][0] = xaxis[1] * xaxis[0] * C - xaxis[2] * s;
Q[1][1] = xaxis[1] * xaxis[1] * C + c;
Q[1][2] = xaxis[2] * xaxis[1] * C + xaxis[0] * s;
Q[2][0] = xaxis[1] * xaxis[2] * C + xaxis[1] * s;
Q[2][1] = xaxis[2] * xaxis[1] * C - xaxis[0] * s;
Q[2][2] = xaxis[2] * xaxis[2] * C + c;
double x = Q[2][1] - Q[1][2], y = Q[0][2] - Q[2][0], z = Q[1][0] - Q[0][1];
double r = sqrt(x * x + y * y + z * z);
//xaxis[0] /= r;
//xaxis[1] /= r;
//xaxis[2] /= r;
xaxis[0] = x;// ?
xaxis[1] = y;
xaxis[2] = z;
Thanks to BlueRaja - Danny Pflughoeft:
double c = cos(theta);
double s = sin(theta);
double C = 1.0 - c;
double Q[3][3];
Q[0][0] = zaxis[0] * zaxis[0] * C + c;
Q[0][1] = zaxis[1] * zaxis[0] * C + zaxis[2] * s;
Q[0][2] = zaxis[2] * zaxis[0] * C - zaxis[1] * s;
Q[1][0] = zaxis[1] * zaxis[0] * C - zaxis[2] * s;
Q[1][1] = zaxis[1] * zaxis[1] * C + c;
Q[1][2] = zaxis[2] * zaxis[1] * C + zaxis[0] * s;
Q[2][0] = zaxis[0] * zaxis[2] * C + zaxis[1] * s;
Q[2][1] = zaxis[2] * zaxis[1] * C - zaxis[0] * s;
Q[2][2] = zaxis[2] * zaxis[2] * C + c;
xaxis[0] = xaxis[0] * Q[0][0] + xaxis[0] * Q[0][1] + xaxis[0] * Q[0][2];
xaxis[1] = xaxis[1] * Q[1][0] + xaxis[1] * Q[1][1] + xaxis[1] * Q[1][2];
xaxis[2] = xaxis[2] * Q[2][0] + xaxis[2] * Q[2][1] + xaxis[2] * Q[2][2]; // Multiply a 3x3 by 3x1 and store it as the new rotated axis
yaxis[0] = yaxis[0] * Q[0][0] + yaxis[0] * Q[0][1] + yaxis[0] * Q[0][2];
yaxis[1] = yaxis[1] * Q[1][0] + yaxis[1] * Q[1][1] + yaxis[1] * Q[1][2];
yaxis[2] = yaxis[2] * Q[2][0] + yaxis[2] * Q[2][1] + yaxis[2] * Q[2][2]; // Multiply a 3x3 by 3x1 and store it as the new rotated axis
I see that following matrix multiplication is wrong!
As stated above it can be factored with xaxis[0]
xaxis[0] = xaxis[0] * Q[0][0] + xaxis[0] * Q[0][1] + xaxis[0] * Q[0][2];
xaxis[0] = xaxis[0] * (Q[0][0] + Q[0][1] + Q[0][2]);
This does not look like a matrix multiplication. It should be:
xaxis1[0] = xaxis[0] * Q[0][0] + xaxis[1] * Q[0][1] + xaxis[2] * Q[0][2];
xaxis1[1] = xaxis[0] * Q[1][0] + xaxis[1] * Q[1][1] + xaxis[2] * Q[1][2];
xaxis1[2] = xaxis[0] * Q[2][0] + xaxis[1] * Q[2][1] + xaxis[2] * Q[2][2]; // Multiply a 3x3 by 3x1 and store it as the new rotated axis
yaxis1[0] = yaxis[0] * Q[0][0] + yaxis[1] * Q[0][1] + yaxis[2] * Q[0][2];
yaxis1[1] = yaxis[0] * Q[1][0] + yaxis[1] * Q[1][1] + yaxis[2] * Q[1][2];
yaxis1[2] = yaxis[0] * Q[2][0] + yaxis[1] * Q[2][1] + yaxis[2] * Q[2][2]; // Multiply a 3x3 by 3x1 and store it as the new rotated axis

How to obtain axis-angle from rotation matrix?

I need to obtain some data from an openGL rotation matrix. I need to obtain the equivalent euler angles (already did it), the equivalent quaternion (did it, but just copying it from the Internet) and the equivalent axis-angle.
I dont know if a rotation matrix can be expresed as a single rotation of a certain angle around an certain vector. Are these equivalent? If they are, how can I obtain one from the other?
Also, i would like to understand better the meaning of a quaternion, and the insides of a rotation matrix. Where should i go to learn about this?
Yes any rotation matrix/unit quaternion is equivalent to a rotation around a single axis. If we call this axis n and the angle theta then the quaternion for this rotation is:
[n * sin(theta / 2) cos(theta / 2)]
To reconstruct this use acos on the w element of the quaternion to get theta / 2. After you have theta you can divide x,y and z component with sin(theta / 2) to reconstruct the axis.
Here's a function which converts a 3x3 matrix into an axis, angle (using a quatention, so perhaps theres a more efficient way which bypasses that step).
void axis_angle_from_mat3(float r_axis[3], float *r_angle, float mat[3][3])
{
float q[4];
/* -------------------------------------------------------------------- */
/* matrix to quaternion */
double tr, s;
float tmat[3][3];
/* work on a copy */
memcpy(tmat, mat, sizeof(tmat));
/* normalize the matrix */
int i;
for (i = 0; i < 3; i++) {
float d = (tmat[i][0] * tmat[i][0] + tmat[i][1] * tmat[i][1] + tmat[i][2] * tmat[i][2]);
if (d > 1.0e-35f) {
d = sqrtf(d);
tmat[i][0] /= d;
tmat[i][1] /= d;
tmat[i][2] /= d;
}
else {
tmat[i][0] = 0.0f;
tmat[i][1] = 0.0f;
tmat[i][2] = 0.0f;
d = 0.0f;
}
}
tr = 0.25 * (double)(1.0f + tmat[0][0] + tmat[1][1] + tmat[2][2]);
if (tr > (double)1e-4f) {
s = sqrt(tr);
q[0] = (float)s;
s = 1.0 / (4.0 * s);
q[1] = (float)((double)(tmat[1][2] - tmat[2][1]) * s);
q[2] = (float)((double)(tmat[2][0] - tmat[0][2]) * s);
q[3] = (float)((double)(tmat[0][1] - tmat[1][0]) * s);
}
else {
if (tmat[0][0] > tmat[1][1] && tmat[0][0] > tmat[2][2]) {
s = 2.0f * sqrtf(1.0f + tmat[0][0] - tmat[1][1] - tmat[2][2]);
q[1] = (float)(0.25 * s);
s = 1.0 / s;
q[0] = (float)((double)(tmat[1][2] - tmat[2][1]) * s);
q[2] = (float)((double)(tmat[1][0] + tmat[0][1]) * s);
q[3] = (float)((double)(tmat[2][0] + tmat[0][2]) * s);
}
else if (tmat[1][1] > tmat[2][2]) {
s = 2.0f * sqrtf(1.0f + tmat[1][1] - tmat[0][0] - tmat[2][2]);
q[2] = (float)(0.25 * s);
s = 1.0 / s;
q[0] = (float)((double)(tmat[2][0] - tmat[0][2]) * s);
q[1] = (float)((double)(tmat[1][0] + tmat[0][1]) * s);
q[3] = (float)((double)(tmat[2][1] + tmat[1][2]) * s);
}
else {
s = 2.0f * sqrtf(1.0f + tmat[2][2] - tmat[0][0] - tmat[1][1]);
q[3] = (float)(0.25 * s);
s = 1.0 / s;
q[0] = (float)((double)(tmat[0][1] - tmat[1][0]) * s);
q[1] = (float)((double)(tmat[2][0] + tmat[0][2]) * s);
q[2] = (float)((double)(tmat[2][1] + tmat[1][2]) * s);
}
}
/* normalize the quat */
float len;
len = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
if (len != 0.0f) {
q[0] /= len;
q[1] /= len;
q[2] /= len;
q[3] /= len;
}
else {
q[1] = 1.0f;
q[0] = q[2] = q[3] = 0.0f;
}
/* -------------------------------------------------------------------- */
/* quaternion to axis angle */
float ha, si;
ha = acosf(q[0]);
si = sinf(ha);
*r_angle = ha * 2;
if (fabsf(si) < FLT_EPSILON)
si = 1.0f;
r_axis[0] = q[1] / si;
r_axis[1] = q[2] / si;
r_axis[2] = q[3] / si;
}

Gradient algorithm produces little white dots

I'm working on an algorithm to generate point to point linear gradients. I have a rough proof of concept implementation done:
GLuint OGLENGINEFUNCTIONS::CreateGradient( std::vector<ARGBCOLORF> &input,POINTFLOAT start, POINTFLOAT end, int width, int height,bool radial )
{
std::vector<POINT> pol;
std::vector<GLubyte> pdata(width * height * 4);
std::vector<POINTFLOAT> linearpts;
std::vector<float> lookup;
float distance = GetDistance(start,end);
RoundNumber(distance);
POINTFLOAT temp;
float incr = 1 / (distance + 1);
for(int l = 0; l < 100; l ++)
{
POINTFLOAT outA;
POINTFLOAT OutB;
float dirlen;
float perplen;
POINTFLOAT dir;
POINTFLOAT ndir;
POINTFLOAT perp;
POINTFLOAT nperp;
POINTFLOAT perpoffset;
POINTFLOAT diroffset;
dir.x = end.x - start.x;
dir.y = end.y - start.y;
dirlen = sqrt((dir.x * dir.x) + (dir.y * dir.y));
ndir.x = static_cast<float>(dir.x * 1.0 / dirlen);
ndir.y = static_cast<float>(dir.y * 1.0 / dirlen);
perp.x = dir.y;
perp.y = -dir.x;
perplen = sqrt((perp.x * perp.x) + (perp.y * perp.y));
nperp.x = static_cast<float>(perp.x * 1.0 / perplen);
nperp.y = static_cast<float>(perp.y * 1.0 / perplen);
perpoffset.x = static_cast<float>(nperp.x * l * 0.5);
perpoffset.y = static_cast<float>(nperp.y * l * 0.5);
diroffset.x = static_cast<float>(ndir.x * 0 * 0.5);
diroffset.y = static_cast<float>(ndir.y * 0 * 0.5);
outA.x = end.x + perpoffset.x + diroffset.x;
outA.y = end.y + perpoffset.y + diroffset.y;
OutB.x = start.x + perpoffset.x - diroffset.x;
OutB.y = start.y + perpoffset.y - diroffset.y;
for (float i = 0; i < 1; i += incr)
{
temp = GetLinearBezier(i,outA,OutB);
RoundNumber(temp.x);
RoundNumber(temp.y);
linearpts.push_back(temp);
lookup.push_back(i);
}
for (unsigned int j = 0; j < linearpts.size(); j++) {
if(linearpts[j].x < width && linearpts[j].x >= 0 &&
linearpts[j].y < height && linearpts[j].y >=0)
{
pdata[linearpts[j].x * 4 * width + linearpts[j].y * 4 + 0] = (GLubyte) j;
pdata[linearpts[j].x * 4 * width + linearpts[j].y * 4 + 1] = (GLubyte) j;
pdata[linearpts[j].x * 4 * width + linearpts[j].y * 4 + 2] = (GLubyte) j;
pdata[linearpts[j].x * 4 * width + linearpts[j].y * 4 + 3] = (GLubyte) 255;
}
}
lookup.clear();
linearpts.clear();
}
return CreateTexture(pdata,width,height);
}
It works as I would expect most of the time, but at certain angles it produces little white dots. I can't figure out what does this.
This is what it looks like at most angles (good) http://img9.imageshack.us/img9/5922/goodgradient.png
But once in a while it looks like this (bad): http://img155.imageshack.us/img155/760/badgradient.png
What could be causing the white dots?
Is there maybe also a better way to generate my gradients if no solution is possible for this?
Thanks
I think you have a bug indexing into the pdata byte vector. Your x domain is [0, width) but when you multiply out the indices you're doing x * 4 * width. It should probably be x * 4 + y * 4 * width or x * 4 * height + y * 4 depending on whether you're data is arranged row or column major.

Inverting a 4x4 matrix

I am looking for a sample code implementation on how to invert a 4x4 matrix. I know there is Gaussian eleminiation, LU decomposition, etc., but instead of looking at them in detail I am really just looking for the code to do this.
Language ideally C++, data is available in array of 16 floats in column-major order.
here:
bool gluInvertMatrix(const double m[16], double invOut[16])
{
double inv[16], det;
int i;
inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] +
m[9] * m[7] * m[14] +
m[13] * m[6] * m[11] -
m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] +
m[4] * m[11] * m[14] +
m[8] * m[6] * m[15] -
m[8] * m[7] * m[14] -
m[12] * m[6] * m[11] +
m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] -
m[4] * m[11] * m[13] -
m[8] * m[5] * m[15] +
m[8] * m[7] * m[13] +
m[12] * m[5] * m[11] -
m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] +
m[4] * m[10] * m[13] +
m[8] * m[5] * m[14] -
m[8] * m[6] * m[13] -
m[12] * m[5] * m[10] +
m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] +
m[1] * m[11] * m[14] +
m[9] * m[2] * m[15] -
m[9] * m[3] * m[14] -
m[13] * m[2] * m[11] +
m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] -
m[0] * m[11] * m[14] -
m[8] * m[2] * m[15] +
m[8] * m[3] * m[14] +
m[12] * m[2] * m[11] -
m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] +
m[0] * m[11] * m[13] +
m[8] * m[1] * m[15] -
m[8] * m[3] * m[13] -
m[12] * m[1] * m[11] +
m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] -
m[0] * m[10] * m[13] -
m[8] * m[1] * m[14] +
m[8] * m[2] * m[13] +
m[12] * m[1] * m[10] -
m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] -
m[1] * m[7] * m[14] -
m[5] * m[2] * m[15] +
m[5] * m[3] * m[14] +
m[13] * m[2] * m[7] -
m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] +
m[0] * m[7] * m[14] +
m[4] * m[2] * m[15] -
m[4] * m[3] * m[14] -
m[12] * m[2] * m[7] +
m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] -
m[0] * m[7] * m[13] -
m[4] * m[1] * m[15] +
m[4] * m[3] * m[13] +
m[12] * m[1] * m[7] -
m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] +
m[0] * m[6] * m[13] +
m[4] * m[1] * m[14] -
m[4] * m[2] * m[13] -
m[12] * m[1] * m[6] +
m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] +
m[1] * m[7] * m[10] +
m[5] * m[2] * m[11] -
m[5] * m[3] * m[10] -
m[9] * m[2] * m[7] +
m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] -
m[0] * m[7] * m[10] -
m[4] * m[2] * m[11] +
m[4] * m[3] * m[10] +
m[8] * m[2] * m[7] -
m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] +
m[0] * m[7] * m[9] +
m[4] * m[1] * m[11] -
m[4] * m[3] * m[9] -
m[8] * m[1] * m[7] +
m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] -
m[0] * m[6] * m[9] -
m[4] * m[1] * m[10] +
m[4] * m[2] * m[9] +
m[8] * m[1] * m[6] -
m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if (det == 0)
return false;
det = 1.0 / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
return true;
}
This was lifted from MESA implementation of the GLU library.
If anyone looking for more costumized code and "easier to read", then I got this
var A2323 = m.m22 * m.m33 - m.m23 * m.m32 ;
var A1323 = m.m21 * m.m33 - m.m23 * m.m31 ;
var A1223 = m.m21 * m.m32 - m.m22 * m.m31 ;
var A0323 = m.m20 * m.m33 - m.m23 * m.m30 ;
var A0223 = m.m20 * m.m32 - m.m22 * m.m30 ;
var A0123 = m.m20 * m.m31 - m.m21 * m.m30 ;
var A2313 = m.m12 * m.m33 - m.m13 * m.m32 ;
var A1313 = m.m11 * m.m33 - m.m13 * m.m31 ;
var A1213 = m.m11 * m.m32 - m.m12 * m.m31 ;
var A2312 = m.m12 * m.m23 - m.m13 * m.m22 ;
var A1312 = m.m11 * m.m23 - m.m13 * m.m21 ;
var A1212 = m.m11 * m.m22 - m.m12 * m.m21 ;
var A0313 = m.m10 * m.m33 - m.m13 * m.m30 ;
var A0213 = m.m10 * m.m32 - m.m12 * m.m30 ;
var A0312 = m.m10 * m.m23 - m.m13 * m.m20 ;
var A0212 = m.m10 * m.m22 - m.m12 * m.m20 ;
var A0113 = m.m10 * m.m31 - m.m11 * m.m30 ;
var A0112 = m.m10 * m.m21 - m.m11 * m.m20 ;
var det = m.m00 * ( m.m11 * A2323 - m.m12 * A1323 + m.m13 * A1223 )
- m.m01 * ( m.m10 * A2323 - m.m12 * A0323 + m.m13 * A0223 )
+ m.m02 * ( m.m10 * A1323 - m.m11 * A0323 + m.m13 * A0123 )
- m.m03 * ( m.m10 * A1223 - m.m11 * A0223 + m.m12 * A0123 ) ;
det = 1 / det;
return new Matrix4x4() {
m00 = det * ( m.m11 * A2323 - m.m12 * A1323 + m.m13 * A1223 ),
m01 = det * - ( m.m01 * A2323 - m.m02 * A1323 + m.m03 * A1223 ),
m02 = det * ( m.m01 * A2313 - m.m02 * A1313 + m.m03 * A1213 ),
m03 = det * - ( m.m01 * A2312 - m.m02 * A1312 + m.m03 * A1212 ),
m10 = det * - ( m.m10 * A2323 - m.m12 * A0323 + m.m13 * A0223 ),
m11 = det * ( m.m00 * A2323 - m.m02 * A0323 + m.m03 * A0223 ),
m12 = det * - ( m.m00 * A2313 - m.m02 * A0313 + m.m03 * A0213 ),
m13 = det * ( m.m00 * A2312 - m.m02 * A0312 + m.m03 * A0212 ),
m20 = det * ( m.m10 * A1323 - m.m11 * A0323 + m.m13 * A0123 ),
m21 = det * - ( m.m00 * A1323 - m.m01 * A0323 + m.m03 * A0123 ),
m22 = det * ( m.m00 * A1313 - m.m01 * A0313 + m.m03 * A0113 ),
m23 = det * - ( m.m00 * A1312 - m.m01 * A0312 + m.m03 * A0112 ),
m30 = det * - ( m.m10 * A1223 - m.m11 * A0223 + m.m12 * A0123 ),
m31 = det * ( m.m00 * A1223 - m.m01 * A0223 + m.m02 * A0123 ),
m32 = det * - ( m.m00 * A1213 - m.m01 * A0213 + m.m02 * A0113 ),
m33 = det * ( m.m00 * A1212 - m.m01 * A0212 + m.m02 * A0112 ),
};
I don't write the code, but my program did. I made a small program to make a program that calculate the determinant and inverse of any N-matrix.
I do it because once in the past I need a code that inverses 5x5 matrix, but nobody in the earth have done this so I made one.
Take a look about the program here.
EDIT: The matrix layout is row-by-row (meaning m01 is in the first row and second column). Also the language is C#, but should be easy to convert into C.
If you need a C++ matrix library with a lot of functions, have a look at Eigen library - http://eigen.tuxfamily.org
I 'rolled up' the MESA implementation (also wrote a couple of unit tests to ensure it actually works).
Here:
float invf(int i,int j,const float* m){
int o = 2+(j-i);
i += 4+o;
j += 4-o;
#define e(a,b) m[ ((j+b)%4)*4 + ((i+a)%4) ]
float inv =
+ e(+1,-1)*e(+0,+0)*e(-1,+1)
+ e(+1,+1)*e(+0,-1)*e(-1,+0)
+ e(-1,-1)*e(+1,+0)*e(+0,+1)
- e(-1,-1)*e(+0,+0)*e(+1,+1)
- e(-1,+1)*e(+0,-1)*e(+1,+0)
- e(+1,-1)*e(-1,+0)*e(+0,+1);
return (o%2)?inv : -inv;
#undef e
}
bool inverseMatrix4x4(const float *m, float *out)
{
float inv[16];
for(int i=0;i<4;i++)
for(int j=0;j<4;j++)
inv[j*4+i] = invf(i,j,m);
double D = 0;
for(int k=0;k<4;k++) D += m[k] * inv[k*4];
if (D == 0) return false;
D = 1.0 / D;
for (int i = 0; i < 16; i++)
out[i] = inv[i] * D;
return true;
}
I wrote a little about this and display the pattern of positive/negative factors on my blog.
As suggested by #LiraNuna, on many platforms hardware accelerated versions of such routines are available so I'm happy to have a 'backup version' that's readable and concise.
Note: this may run 3.5 times slower or worse than the MESA implementation. You can shift the pattern of factors to remove some additions etc... but it would lose in readability and still won't be very fast.
This is the C++ version for #willnode's answer
template<typename Matrix>
static inline void InvertMatrix4(const Matrix& m, Matrix& im, double& det)
{
double A2323 = m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2);
double A1323 = m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1);
double A1223 = m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1);
double A0323 = m(2, 0) * m(3, 3) - m(2, 3) * m(3, 0);
double A0223 = m(2, 0) * m(3, 2) - m(2, 2) * m(3, 0);
double A0123 = m(2, 0) * m(3, 1) - m(2, 1) * m(3, 0);
double A2313 = m(1, 2) * m(3, 3) - m(1, 3) * m(3, 2);
double A1313 = m(1, 1) * m(3, 3) - m(1, 3) * m(3, 1);
double A1213 = m(1, 1) * m(3, 2) - m(1, 2) * m(3, 1);
double A2312 = m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2);
double A1312 = m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1);
double A1212 = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
double A0313 = m(1, 0) * m(3, 3) - m(1, 3) * m(3, 0);
double A0213 = m(1, 0) * m(3, 2) - m(1, 2) * m(3, 0);
double A0312 = m(1, 0) * m(2, 3) - m(1, 3) * m(2, 0);
double A0212 = m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0);
double A0113 = m(1, 0) * m(3, 1) - m(1, 1) * m(3, 0);
double A0112 = m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0);
det = m(0, 0) * ( m(1, 1) * A2323 - m(1, 2) * A1323 + m(1, 3) * A1223 )
- m(0, 1) * ( m(1, 0) * A2323 - m(1, 2) * A0323 + m(1, 3) * A0223 )
+ m(0, 2) * ( m(1, 0) * A1323 - m(1, 1) * A0323 + m(1, 3) * A0123 )
- m(0, 3) * ( m(1, 0) * A1223 - m(1, 1) * A0223 + m(1, 2) * A0123 );
det = 1 / det;
im(0, 0) = det * ( m(1, 1) * A2323 - m(1, 2) * A1323 + m(1, 3) * A1223 );
im(0, 1) = det * - ( m(0, 1) * A2323 - m(0, 2) * A1323 + m(0, 3) * A1223 );
im(0, 2) = det * ( m(0, 1) * A2313 - m(0, 2) * A1313 + m(0, 3) * A1213 );
im(0, 3) = det * - ( m(0, 1) * A2312 - m(0, 2) * A1312 + m(0, 3) * A1212 );
im(1, 0) = det * - ( m(1, 0) * A2323 - m(1, 2) * A0323 + m(1, 3) * A0223 );
im(1, 1) = det * ( m(0, 0) * A2323 - m(0, 2) * A0323 + m(0, 3) * A0223 );
im(1, 2) = det * - ( m(0, 0) * A2313 - m(0, 2) * A0313 + m(0, 3) * A0213 );
im(1, 3) = det * ( m(0, 0) * A2312 - m(0, 2) * A0312 + m(0, 3) * A0212 );
im(2, 0) = det * ( m(1, 0) * A1323 - m(1, 1) * A0323 + m(1, 3) * A0123 );
im(2, 1) = det * - ( m(0, 0) * A1323 - m(0, 1) * A0323 + m(0, 3) * A0123 );
im(2, 2) = det * ( m(0, 0) * A1313 - m(0, 1) * A0313 + m(0, 3) * A0113 );
im(2, 3) = det * - ( m(0, 0) * A1312 - m(0, 1) * A0312 + m(0, 3) * A0112 );
im(3, 0) = det * - ( m(1, 0) * A1223 - m(1, 1) * A0223 + m(1, 2) * A0123 );
im(3, 1) = det * ( m(0, 0) * A1223 - m(0, 1) * A0223 + m(0, 2) * A0123 );
im(3, 2) = det * - ( m(0, 0) * A1213 - m(0, 1) * A0213 + m(0, 2) * A0113 );
im(3, 3) = det * ( m(0, 0) * A1212 - m(0, 1) * A0212 + m(0, 2) * A0112 );
}
You can use the GNU Scientific Library or look the code up in it.
Edit: You seem to want the Linear Algebra section.
Here is a small (just one header) C++ vector math library (geared towards 3D programming). If you use it, keep in mind that layout of its matrices in memory is inverted comparing to what OpenGL expects, I had fun time figuring it out...
Inspired by #shoosh to check out MESA implementations, I found that matrix inversion looks quite different in more recent mesa releases. I suppose those are good improvements. Here's the matrix inversion code from Mesa-17.3.9:
/* Returns true for success, false for failure (singular matrix) */
bool DirectVolumeRenderer::_mesa_invert_matrix_general( GLfloat out[16], const GLfloat in[16] )
{
/**
* References an element of 4x4 matrix.
* Calculate the linear storage index of the element and references it.
*/
#define MAT(m,r,c) (m)[(c)*4+(r)]
/**
* Swaps the values of two floating point variables.
*/
#define SWAP_ROWS(a, b) { GLfloat *_tmp = a; (a)=(b); (b)=_tmp; }
const GLfloat *m = in;
GLfloat wtmp[4][8];
GLfloat m0, m1, m2, m3, s;
GLfloat *r0, *r1, *r2, *r3;
r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
/* choose pivot - or die */
if (fabsf(r3[0])>fabsf(r2[0])) SWAP_ROWS(r3, r2);
if (fabsf(r2[0])>fabsf(r1[0])) SWAP_ROWS(r2, r1);
if (fabsf(r1[0])>fabsf(r0[0])) SWAP_ROWS(r1, r0);
if (0.0F == r0[0])
return false;
/* eliminate first variable */
m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
s = r0[4];
if (s != 0.0F) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
s = r0[5];
if (s != 0.0F) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
s = r0[6];
if (s != 0.0F) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
s = r0[7];
if (s != 0.0F) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
/* choose pivot - or die */
if (fabsf(r3[1])>fabsf(r2[1])) SWAP_ROWS(r3, r2);
if (fabsf(r2[1])>fabsf(r1[1])) SWAP_ROWS(r2, r1);
if (0.0F == r1[1])
return false;
/* eliminate second variable */
m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
s = r1[4]; if (0.0F != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
s = r1[5]; if (0.0F != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
s = r1[6]; if (0.0F != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
s = r1[7]; if (0.0F != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
/* choose pivot - or die */
if (fabsf(r3[2])>fabsf(r2[2])) SWAP_ROWS(r3, r2);
if (0.0F == r2[2])
return false;
/* eliminate third variable */
m3 = r3[2]/r2[2];
r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
r3[7] -= m3 * r2[7];
/* last check */
if (0.0F == r3[3])
return false;
s = 1.0F/r3[3]; /* now back substitute row 3 */
r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
m2 = r2[3]; /* now back substitute row 2 */
s = 1.0F/r2[2];
r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
m1 = r1[3];
r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
m0 = r0[3];
r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
m1 = r1[2]; /* now back substitute row 1 */
s = 1.0F/r1[1];
r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
m0 = r0[2];
r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
m0 = r0[1]; /* now back substitute row 0 */
s = 1.0F/r0[0];
r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5],
MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7],
MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5],
MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7],
MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5],
MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7],
MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5],
MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7];
#undef SWAP_ROWS
#undef MAT
return true;
}
Note: you can find this piece of code in the mesa code base: mesa-17.3.9/src/mesa/math/m_matrix.c.
You can make it faster according to this blog.
#define SUBP(i,j) input[i][j]
#define SUBQ(i,j) input[i][2+j]
#define SUBR(i,j) input[2+i][j]
#define SUBS(i,j) input[2+i][2+j]
#define OUTP(i,j) output[i][j]
#define OUTQ(i,j) output[i][2+j]
#define OUTR(i,j) output[2+i][j]
#define OUTS(i,j) output[2+i][2+j]
#define INVP(i,j) invP[i][j]
#define INVPQ(i,j) invPQ[i][j]
#define RINVP(i,j) RinvP[i][j]
#define INVPQ(i,j) invPQ[i][j]
#define RINVPQ(i,j) RinvPQ[i][j]
#define INVPQR(i,j) invPQR[i][j]
#define INVS(i,j) invS[i][j]
#define MULTI(MAT1, MAT2, MAT3) \
MAT3(0,0)=MAT1(0,0)*MAT2(0,0) + MAT1(0,1)*MAT2(1,0); \
MAT3(0,1)=MAT1(0,0)*MAT2(0,1) + MAT1(0,1)*MAT2(1,1); \
MAT3(1,0)=MAT1(1,0)*MAT2(0,0) + MAT1(1,1)*MAT2(1,0); \
MAT3(1,1)=MAT1(1,0)*MAT2(0,1) + MAT1(1,1)*MAT2(1,1);
#define INV(MAT1, MAT2) \
_det = 1.0 / (MAT1(0,0) * MAT1(1,1) - MAT1(0,1) * MAT1(1,0)); \
MAT2(0,0) = MAT1(1,1) * _det; \
MAT2(1,1) = MAT1(0,0) * _det; \
MAT2(0,1) = -MAT1(0,1) * _det; \
MAT2(1,0) = -MAT1(1,0) * _det; \
#define SUBTRACT(MAT1, MAT2, MAT3) \
MAT3(0,0)=MAT1(0,0) - MAT2(0,0); \
MAT3(0,1)=MAT1(0,1) - MAT2(0,1); \
MAT3(1,0)=MAT1(1,0) - MAT2(1,0); \
MAT3(1,1)=MAT1(1,1) - MAT2(1,1);
#define NEGATIVE(MAT) \
MAT(0,0)=-MAT(0,0); \
MAT(0,1)=-MAT(0,1); \
MAT(1,0)=-MAT(1,0); \
MAT(1,1)=-MAT(1,1);
void getInvertMatrix(complex<double> input[4][4], complex<double> output[4][4]) {
complex<double> _det;
complex<double> invP[2][2];
complex<double> invPQ[2][2];
complex<double> RinvP[2][2];
complex<double> RinvPQ[2][2];
complex<double> invPQR[2][2];
complex<double> invS[2][2];
INV(SUBP, INVP);
MULTI(SUBR, INVP, RINVP);
MULTI(INVP, SUBQ, INVPQ);
MULTI(RINVP, SUBQ, RINVPQ);
SUBTRACT(SUBS, RINVPQ, INVS);
INV(INVS, OUTS);
NEGATIVE(OUTS);
MULTI(OUTS, RINVP, OUTR);
MULTI(INVPQ, OUTS, OUTQ);
MULTI(INVPQ, OUTR, INVPQR);
SUBTRACT(INVP, INVPQR, OUTP);
}
This is not a complete implementation because P may not be invertible, but you can combine this code with MESA implementation to get a better performance.
If you want to compute the inverse matrix of 4x4 matrix, then I recommend to use a library like OpenGL Mathematics (GLM) :
Anyway, you can do it from scratch. The following implementation is similar to the implementation of glm::inverse, but it is not as highly optimized:
bool InverseMat44( const GLfloat m[16], GLfloat invOut[16] )
{
float inv[16], det;
int i;
inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if (det == 0) return false;
det = 1.0 / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
return true;
}
Adding a 2d case which might be useful for someone else:
inline bool invert4by4matrix(mat m, mat inv_m)
{
inv_m->v[0][0] = m->v[1][1] * m->v[2][2] * m->v[3][3] -
m->v[1][1] * m->v[2][3] * m->v[3][2] -
m->v[2][1] * m->v[1][2] * m->v[3][3] +
m->v[2][1] * m->v[1][3] * m->v[3][2] +
m->v[3][1] * m->v[1][2] * m->v[2][3] -
m->v[3][1] * m->v[1][3] * m->v[2][2];
inv_m->v[1][0] = -m->v[1][0] * m->v[2][2] * m->v[3][3] +
m->v[1][0] * m->v[2][3] * m->v[3][2] +
m->v[2][0] * m->v[1][2] * m->v[3][3] -
m->v[2][0] * m->v[1][3] * m->v[3][2] -
m->v[3][0] * m->v[1][2] * m->v[2][3] +
m->v[3][0] * m->v[1][3] * m->v[2][2];
inv_m->v[2][0] = m->v[1][0] * m->v[2][1] * m->v[3][3] -
m->v[1][0] * m->v[2][3] * m->v[3][1] -
m->v[2][0] * m->v[1][1] * m->v[3][3] +
m->v[2][0] * m->v[1][3] * m->v[3][1] +
m->v[3][0] * m->v[1][1] * m->v[2][3] -
m->v[3][0] * m->v[1][3] * m->v[2][1];
inv_m->v[3][0] = -m->v[1][0] * m->v[2][1] * m->v[3][2] +
m->v[1][0] * m->v[2][2] * m->v[3][1] +
m->v[2][0] * m->v[1][1] * m->v[3][2] -
m->v[2][0] * m->v[1][2] * m->v[3][1] -
m->v[3][0] * m->v[1][1] * m->v[2][2] +
m->v[3][0] * m->v[1][2] * m->v[2][1];
inv_m->v[0][1] = -m->v[0][1] * m->v[2][2] * m->v[3][3] +
m->v[0][1] * m->v[2][3] * m->v[3][2] +
m->v[2][1] * m->v[0][2] * m->v[3][3] -
m->v[2][1] * m->v[0][3] * m->v[3][2] -
m->v[3][1] * m->v[0][2] * m->v[2][3] +
m->v[3][1] * m->v[0][3] * m->v[2][2];
inv_m->v[1][1] = m->v[0][0] * m->v[2][2] * m->v[3][3] -
m->v[0][0] * m->v[2][3] * m->v[3][2] -
m->v[2][0] * m->v[0][2] * m->v[3][3] +
m->v[2][0] * m->v[0][3] * m->v[3][2] +
m->v[3][0] * m->v[0][2] * m->v[2][3] -
m->v[3][0] * m->v[0][3] * m->v[2][2];
inv_m->v[2][1] = -m->v[0][0] * m->v[2][1] * m->v[3][3] +
m->v[0][0] * m->v[2][3] * m->v[3][1] +
m->v[2][0] * m->v[0][1] * m->v[3][3] -
m->v[2][0] * m->v[0][3] * m->v[3][1] -
m->v[3][0] * m->v[0][1] * m->v[2][3] +
m->v[3][0] * m->v[0][3] * m->v[2][1];
inv_m->v[3][1] = m->v[0][0] * m->v[2][1] * m->v[3][2] -
m->v[0][0] * m->v[2][2] * m->v[3][1] -
m->v[2][0] * m->v[0][1] * m->v[3][2] +
m->v[2][0] * m->v[0][2] * m->v[3][1] +
m->v[3][0] * m->v[0][1] * m->v[2][2] -
m->v[3][0] * m->v[0][2] * m->v[2][1];
inv_m->v[0][2] = m->v[0][1] * m->v[1][2] * m->v[3][3] -
m->v[0][1] * m->v[1][3] * m->v[3][2] -
m->v[1][1] * m->v[0][2] * m->v[3][3] +
m->v[1][1] * m->v[0][3] * m->v[3][2] +
m->v[3][1] * m->v[0][2] * m->v[1][3] -
m->v[3][1] * m->v[0][3] * m->v[1][2];
inv_m->v[1][2] = -m->v[0][0] * m->v[1][2] * m->v[3][3] +
m->v[0][0] * m->v[1][3] * m->v[3][2] +
m->v[1][0] * m->v[0][2] * m->v[3][3] -
m->v[1][0] * m->v[0][3] * m->v[3][2] -
m->v[3][0] * m->v[0][2] * m->v[1][3] +
m->v[3][0] * m->v[0][3] * m->v[1][2];
inv_m->v[2][2] = m->v[0][0] * m->v[1][1] * m->v[3][3] -
m->v[0][0] * m->v[1][3] * m->v[3][1] -
m->v[1][0] * m->v[0][1] * m->v[3][3] +
m->v[1][0] * m->v[0][3] * m->v[3][1] +
m->v[3][0] * m->v[0][1] * m->v[1][3] -
m->v[3][0] * m->v[0][3] * m->v[1][1];
inv_m->v[3][2] = -m->v[0][0] * m->v[1][1] * m->v[3][2] +
m->v[0][0] * m->v[1][2] * m->v[3][1] +
m->v[1][0] * m->v[0][1] * m->v[3][2] -
m->v[1][0] * m->v[0][2] * m->v[3][1] -
m->v[3][0] * m->v[0][1] * m->v[1][2] +
m->v[3][0] * m->v[0][2] * m->v[1][1];
inv_m->v[0][3] = -m->v[0][1] * m->v[1][2] * m->v[2][3] +
m->v[0][1] * m->v[1][3] * m->v[2][2] +
m->v[1][1] * m->v[0][2] * m->v[2][3] -
m->v[1][1] * m->v[0][3] * m->v[2][2] -
m->v[2][1] * m->v[0][2] * m->v[1][3] +
m->v[2][1] * m->v[0][3] * m->v[1][2];
inv_m->v[1][3] = m->v[0][0] * m->v[1][2] * m->v[2][3] -
m->v[0][0] * m->v[1][3] * m->v[2][2] -
m->v[1][0] * m->v[0][2] * m->v[2][3] +
m->v[1][0] * m->v[0][3] * m->v[2][2] +
m->v[2][0] * m->v[0][2] * m->v[1][3] -
m->v[2][0] * m->v[0][3] * m->v[1][2];
inv_m->v[2][3] = -m->v[0][0] * m->v[1][1] * m->v[2][3] +
m->v[0][0] * m->v[1][3] * m->v[2][1] +
m->v[1][0] * m->v[0][1] * m->v[2][3] -
m->v[1][0] * m->v[0][3] * m->v[2][1] -
m->v[2][0] * m->v[0][1] * m->v[1][3] +
m->v[2][0] * m->v[0][3] * m->v[1][1];
inv_m->v[3][3] = m->v[0][0] * m->v[1][1] * m->v[2][2] -
m->v[0][0] * m->v[1][2] * m->v[2][1] -
m->v[1][0] * m->v[0][1] * m->v[2][2] +
m->v[1][0] * m->v[0][2] * m->v[2][1] +
m->v[2][0] * m->v[0][1] * m->v[1][2] -
m->v[2][0] * m->v[0][2] * m->v[1][1];
double det = m->v[0][0] * inv_m->v[0][0] +
m->v[0][1] * inv_m->v[1][0] +
m->v[0][2] * inv_m->v[2][0] +
m->v[0][3] * inv_m->v[3][0];
if (det == 0)
return false;
det = 1.0 / det;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
inv_m->v[i][j] = inv_m->v[i][j] * det;
}
}
return true;
}