Inverting a 4x4 matrix - c++

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;
}

Related

C++ code runs fine in Visual Studio (windows) but gives a segmentation fault in CodeLite (Linux)

My code compiles and runs without error in Visual Studio, however I need to to run in CodeLite on Linux and it gives me a segmentation fault for the same code.
For reference this is my code:
#include <string>
#include <iostream>
#include <cmath>
#include <fstream>
#include <vector>
#include <algorithm>
#include <iterator>
#include <tuple>
using namespace std;
tuple<vector<double>, vector<double>, vector<double>> RK4() {
//open parameters.txt, put data into a vector
ifstream fin("parameters.txt");
vector<double> data;
data.reserve(8);
double element;
while (fin >> element) {
data.push_back(element);
}
//define tspan
vector<double> tspan(2);
tspan[0] = 0.0;
tspan[1] = data[7];
//define y0
vector<double> y0(4);
//CHANGE TO DATA[4], DATA[5]
const double a = 3.141592653589793238462643383279;
y0[0] = data[4];
y0[1] = data[5];
y0[2] = 0.0;
y0[3] = 0.0;
double theta1 = y0[0];
double theta2 = y0[1];
double omega1 = y0[2];
double omega2 = y0[3];
//define stepSize
double stepSize;
stepSize = data[6];
//define range
int range = int(tspan[1] / stepSize);
//define other constants
double m1, m2, l1, l2;
m1 = data[0];
m2 = data[1];
l1 = data[2];
l2 = data[3];
double g = 9.81;
//define y, t vectors
vector<double> y1(range);
vector<double> y2(range);
vector<double> y3(range);
vector<double> y4(range);
vector<double> t(range);
for (double i = 0.0; i < 1.0 * range; i++) {
t[i] = i * stepSize;
}
//enter y0 into first value
y1[0] = theta1;
y2[0] = theta2;
y3[0] = omega1;
y4[0] = omega2;
//loop to find y, t vectors
for (int i = 0; i < range - 1; i++) {
//finding all k values:
//k1
double dTheta1_1 = y3[i];
double dOmega1_1 = (-g * (2 * m1 + m2) * sin(y1[i]) - m2 * g * sin(y1[i] - 2 * y2[i]) - 2 * sin(y1[i] - y2[i]) * m2 * (pow(y4[i], 2) * l2 + pow(y3[i], 2) * l1 * cos(y1[i] - y2[i]))) / (l1 * (2 * m1 + m2 - m2 * cos(2 * y1[i] - 2 * y2[i])));
double dTheta2_1 = y4[i];
double dOmega2_1 = (2 * sin(y1[i] - y2[i]) * (pow(y3[i], 2) * l1 * (m1 + m2) + g * (m1 + m2) * cos(y1[i]) + pow(y4[i], 2) * l2 * m2 * cos(y1[i] - y2[i]))) / (l2 * (2 * m1 + m2 - m2 * cos(2 * y1[i] - 2 * y2[i])));
//k2
double dTheta1_2 = y3[i] + 0.5 * stepSize * dTheta1_1;
double dOmega1_2 = (-g * (2 * m1 + m2) * sin(y1[i] + 0.5 * stepSize * dTheta1_1) - m2 * g * sin((y1[i] + 0.5 * stepSize * dTheta1_1) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_1)) - 2 * sin((y1[i] + 0.5 * stepSize * dTheta1_1) - (y2[i] + 0.5 * stepSize * dTheta2_1)) * m2 * (pow(y4[i] + 0.5 * stepSize * dOmega2_1, 2) * l2 + pow(y3[i] + 0.5 * stepSize * dOmega1_1, 2) * l1 * cos((y1[i] + 0.5 * stepSize * dTheta1_1) - (y2[i] + 0.5 * stepSize * dTheta2_1)))) / (l1 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + 0.5 * stepSize * dTheta1_1) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_1))));
double dTheta2_2 = y4[i] + 0.5 * stepSize * dTheta2_1;
double dOmega2_2 = (2 * sin((y1[i] + 0.5 * stepSize * dTheta1_1) - (y2[i] + 0.5 * stepSize * dTheta2_1)) * (pow(y3[i] + 0.5 * stepSize * dOmega1_1, 2) * l1 * (m1 + m2) + g * (m1 + m2) * cos(y1[i] + 0.5 * stepSize * dTheta1_1) + pow(y4[i] + 0.5 * stepSize * dOmega2_1, 2) * l2 * m2 * cos((y1[i] + 0.5 * stepSize * dTheta1_1) - (y2[i] + 0.5 * stepSize * dTheta2_1)))) / (l2 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + 0.5 * stepSize * dTheta1_1) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_1))));
//k3
double dTheta1_3 = y3[i] + 0.5 * stepSize * dTheta1_2;
double dOmega1_3 = (-g * (2 * m1 + m2) * sin(y1[i] + 0.5 * stepSize * dTheta1_2) - m2 * g * sin((y1[i] + 0.5 * stepSize * dTheta1_2) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_2)) - 2 * sin((y1[i] + 0.5 * stepSize * dTheta1_2) - (y2[i] + 0.5 * stepSize * dTheta2_2)) * m2 * (pow(y4[i] + 0.5 * stepSize * dOmega2_2, 2) * l2 + pow(y3[i] + 0.5 * stepSize * dOmega1_2, 2) * l1 * cos((y1[i] + 0.5 * stepSize * dTheta1_2) - (y2[i] + 0.5 * stepSize * dTheta2_2)))) / (l1 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + 0.5 * stepSize * dTheta1_2) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_2))));
double dTheta2_3 = y4[i] + 0.5 * stepSize * dTheta2_2;
double dOmega2_3 = (2 * sin((y1[i] + 0.5 * stepSize * dTheta1_2) - (y2[i] + 0.5 * stepSize * dTheta2_2)) * (pow(y3[i] + 0.5 * stepSize * dOmega1_2, 2) * l1 * (m1 + m2) + g * (m1 + m2) * cos(y1[i] + 0.5 * stepSize * dTheta1_2) + pow(y4[i] + 0.5 * stepSize * dOmega2_2, 2) * l2 * m2 * cos((y1[i] + 0.5 * stepSize * dTheta1_2) - (y2[i] + 0.5 * stepSize * dTheta2_2)))) / (l2 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + 0.5 * stepSize * dTheta1_2) - 2 * (y2[i] + 0.5 * stepSize * dTheta2_2))));
//k4
double dTheta1_4 = y3[i] + stepSize * dTheta1_3;
double dOmega1_4 = (-g * (2 * m1 + m2) * sin(y1[i] + stepSize * dTheta1_3) - m2 * g * sin((y1[i] + stepSize * dTheta1_3) - 2 * (y2[i] + stepSize * dTheta2_3)) - 2 * sin((y1[i] + stepSize * dTheta1_3) - (y2[i] + stepSize * dTheta2_3)) * m2 * (pow(y4[i] + stepSize * dOmega2_3, 2) * l2 + pow(y3[i] + stepSize * dOmega1_3, 2) * l1 * cos((y1[i] + stepSize * dTheta1_3) - (y2[i] + stepSize * dTheta2_3)))) / (l1 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + stepSize * dTheta1_3) - 2 * (y2[i] + stepSize * dTheta2_3))));
double dTheta2_4 = y4[i] + stepSize * dTheta2_3;
double dOmega2_4 = (2 * sin((y1[i] + stepSize * dTheta1_3) - (y2[i] + stepSize * dTheta2_3)) * (pow(y3[i] + stepSize * dOmega1_3, 2) * l1 * (m1 + m2) + g * (m1 + m2) * cos(y1[i] + stepSize * dTheta1_3) + pow(y4[i] + stepSize * dOmega2_3, 2) * l2 * m2 * cos((y1[i] + stepSize * dTheta1_3) - (y2[i] + stepSize * dTheta2_3)))) / (l2 * (2 * m1 + m2 - m2 * cos(2 * (y1[i] + stepSize * dTheta1_3) - 2 * (y2[i] + stepSize * dTheta2_3))));
double theta1New = y1[i] + (stepSize / 6.0) * (dTheta1_1 + 2 * dTheta1_2 + 2 * dTheta1_3 + dTheta1_4);
double omega1New = y3[i] + (stepSize / 6.0) * (dOmega1_1 + 2 * dOmega1_2 + 2 * dOmega1_3 + dOmega1_4);
double theta2New = y2[i] + (stepSize / 6.0) * (dTheta2_1 + 2 * dTheta2_2 + 2 * dTheta2_3 + dTheta2_4);
double omega2New = y4[i] + (stepSize / 6.0) * (dOmega2_1 + 2 * dOmega2_2 + 2 * dOmega2_3 + dOmega2_4);
// updating y arrays
y1[i + 1] = theta1New;
y2[i + 1] = theta2New;
y3[i + 1] = omega1New;
y4[i + 1] = omega2New;
}
return make_tuple(y1, y2, t);
}
int main() {
//open parameters.txt, put data into a vector
ifstream fin("parameters.txt");
vector<double> data;
data.reserve(8);
double element;
while (fin >> element) {
data.push_back(element);
}
//define tspan
vector<double> tspan(2);
tspan[0] = 0.0;
tspan[1] = data[7];
//define stepSize
double stepSize = data[6];
//define other constants
double l1 = data[2];
double l2 = data[3];
//get y1, y2, t from RK4 function
auto temp = RK4();
vector<double> y1 = get<0>(temp);
vector<double> y2 = get<1>(temp);
vector<double> t = get<2>(temp);
//define range
int const range = static_cast<int>(y1.size());
vector<double> x_1(range), y_1(range), x_2(range), y_2(range);
//define x_1, x_2, y_1, y_2
for (int i = 0; i < range; i++) {
x_1[i] = { sin(y1[i]) * l1 };
y_1[i] = { -cos(y1[i]) * l1 };
x_2[i] = { sin(y1[i]) * l1 + sin(y2[i]) * l2 };
y_2[i] = { -cos(y1[i]) * l1 - cos(y2[i]) * l2 };
}
//writing x,y positions at time t to output.txt
ofstream myfile;
myfile.open("output.txt");
if (myfile.is_open()) {
myfile << "t: " << endl;
for (int i = 0; i < range; i++) {
myfile << t[i] << " ";
}
cout << endl;
myfile << "x_1: " << endl;
for (int i = 0; i < range; i++) {
myfile << x_1[i] << " ";
}
cout << endl;
myfile << "y_1: " << endl;
for (int i = 0; i < range; i++) {
myfile << y_1[i] << " ";
}
cout << endl;
myfile << "x_2: " << endl;
for (int i = 0; i < range; i++) {
myfile << x_2[i] << " ";
}
cout << endl;
myfile << "y_2: " << endl;
for (int i = 0; i < range; i++) {
myfile << y_2[i] << " ";
}
cout << endl;
myfile.close();
}
else { cout << "Unable to open file"; }
return 0;
}
In both cases "parameters.txt" is in the working directory. Why does the operating system/compiler I use affect the outcome? What is the problem?

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?!

3by3 matrix multiplication

I have a 3by3 matrix class that isn't working properly. When I multiply using a third instance to store the answer and multiply two others instances it works properly. But when I try to do *= it gives me weird numbers.
Here's the regular * and *= functions:
threeby3matrix operator*(threeby3matrix& multiplier)
{
threeby3matrix m1;
m1[0] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
m1[1] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
m1[2] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
m1[3] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
m1[4] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
m1[5] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
m1[6] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
m1[7] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
m1[8] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
return m1;
}
threeby3matrix& operator*=(threeby3matrix& multiplier)
{
matrix[0] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
matrix[1] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
matrix[2] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
matrix[3] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
matrix[4] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
matrix[5] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
matrix[6] = matrix[0] * multiplier[0] + matrix[1] * multiplier[3] + matrix[2] * multiplier[6];
matrix[7] = matrix[3] * multiplier[1] + matrix[4] * multiplier[4] + matrix[5] * multiplier[7];
matrix[8] = matrix[6] * multiplier[2] + matrix[7] * multiplier[5] + matrix[8] * multiplier[8];
return *this;
}
For some reason I get [18][18][18][108][228][18][108][708][1638]
when they all should be 18. I tried messing around with brackets, but nothing seems to work.
you are modifying matrix as you use it for calculation.
try something like this:
threeby3matrix& operator*=(threeby3matrix& multiplier)
{
std::swap(*this, operator*(multiplier));
return *this;
}

OpenGl rotate custom implementation

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.

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