35 #ifndef LVR2_MATRIX_H_
36 #define LVR2_MATRIX_H_
46 #define _USE_MATH_DEFINES
49 #include <Eigen/Dense>
52 #define M_PI 3.141592654
63 template<
typename BaseVecT>
74 for(
int i = 0; i < 16; i++) m[i] = 0;
75 m[0] = m[5] = m[10] = m[15] = 1;
85 for(
int i = 0; i < 16; i++) m[i] = matrix[i];
94 for(
int i = 0; i < 16; i++) m[i] = other[i];
105 if(fabs(angle) < 0.0001){
107 bool invert_z = axis.z < 0;
110 float pitch = atan2(axis.z, axis.x) - M_PI_2;
111 if(pitch < 0.0f) pitch += 2.0f *
M_PI;
113 if(axis.x == 0.0f && axis.z == 0.0) pitch = 0.0f;
116 axis.x = axis.x * cos(pitch) + axis.z * sin(pitch);
117 axis.z = -axis.x * sin(pitch) + axis.z * cos(pitch);
120 float yaw = atan2(axis.y, axis.z);
121 if(yaw < 0) yaw += 2 *
M_PI;
125 if(invert_z) yaw = -yaw;
127 cout <<
"YAW: " << yaw <<
" PITCH: " << pitch << endl;
129 if(fabs(yaw) > 0.0001){
130 m2 =
Matrix4(T(1.0, 0.0, 0.0), yaw);
134 if(fabs(pitch) > 0.0001){
135 m1 =
Matrix4(T(0.0, 1.0, 0.0), pitch);
139 for(
int i = 0; i < 16; i++) m[i] = m3[i];
142 float c = cos(angle);
143 float s = sin(angle);
150 m[ 0] = c + a.x * a.x * t;
151 m[ 5] = c + a.y * a.y * t;
152 m[10] = c + a.z * a.z * t;
154 tmp1 = a.x * a.y * t;
159 tmp1 = a.x * a.z * t;
164 tmp1 = a.y * a.z * t;
169 m[ 3] = m[ 7] = m[11] = 0.0;
170 m[12] = m[13] = m[14] = 0.0;
178 float sx = sin(angles[0]);
179 float cx = cos(angles[0]);
180 float sy = sin(angles[1]);
181 float cy = cos(angles[1]);
182 float sz = sin(angles[2]);
183 float cz = cos(angles[2]);
186 m[1] = sx*sy*cz + cx*sz;
187 m[2] = -cx*sy*cz + sx*sz;
190 m[5] = -sx*sy*sz + cx*cz;
191 m[6] = cx*sy*sz + sx*cz;
270 for(
int i = 0; i < 16; i++){
271 new_matrix[i] = m[i] * scale;
284 new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
285 new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
286 new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
287 new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
288 new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
289 new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
290 new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
291 new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
292 new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
293 new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
294 new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
295 new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
296 new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
297 new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
298 new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
299 new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
311 for(
int i = 0; i < 16; i++)
313 new_matrix[i] = m[i] + other[i];
326 return *
this + other;
344 new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
345 new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
346 new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
347 new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
348 new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
349 new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
350 new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
351 new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
352 new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
353 new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
354 new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
355 new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
356 new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
357 new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
358 new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
359 new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
369 using ValType =
typename T::CoordType;
370 ValType x = m[ 0] * v.x + m[ 4] * v.y + m[8 ] * v.z;
371 ValType y = m[ 1] * v.x + m[ 5] * v.y + m[9 ] * v.z;
372 ValType z = m[ 2] * v.x + m[ 6] * v.y + m[10] * v.z;
387 T x = m[ 0] * v.x + m[ 4] * v.y + m[8 ] * v.z;
388 T y = m[ 1] * v.x + m[ 5] * v.y + m[9 ] * v.z;
389 T z = m[ 2] * v.x + m[ 6] * v.y + m[10] * v.z;
425 for(
int i = 0; i < 16; i++) m[i] = m_tmp[i];
438 pose[4] = asin(m[8]);
440 pose[4] = (float)
M_PI - asin(m[8]);
444 float C = cos( pose[4] );
445 if ( fabs( C ) > 0.005 ) {
448 pose[3] = atan2( _trY, _trX );
451 pose[5] = atan2( _trY, _trX );
456 pose[5] = atan2( _trY, _trX );
473 for(
int i = 0; i < 16; i++){
475 cout <<
"Warning: Matrix::loadFromFile: File not found or corrupted: " <<
filename << endl;
488 *
this = *
this * scale;
497 *
this = *
this * other;
506 *
this = *
this * other;
518 for(
int i = 0; i < 16; i++)
527 std::vector<ValueType> tmp(16);
528 for(
int i = 0; i < 16; i++)
538 ValueType at(
const int i)
const;
566 for ( n = 0; n < 4; n++, i *= -1.0 ) {
567 submat( Msub3, 0, n );
569 result += m[n] * det * i;
578 if ( fabs( mdet ) < 0.00000000000005 ) {
579 cout <<
"Error matrix inverting! " << mdet << endl;
584 for ( i = 0; i < 4; i++ ) {
585 for ( j = 0; j < 4; j++ ) {
586 sign = 1 - ( (i +j) % 2 ) * 2;
587 submat( mtemp, i, j );
588 Mout[i+j*4] = ( det3( mtemp ) * sign ) / mdet;
605 for( di = 0; di < 3; di ++ ) {
606 for( dj = 0; dj < 3; dj ++ ) {
608 si = di + ( ( di >= i ) ? 1 : 0 );
609 sj = dj + ( ( dj >= j ) ? 1 : 0 );
611 submat[di * 3 + dj] = m[si * 4 + sj];
625 det = (double)( M[0] * ( M[4]*M[8] - M[7]*M[5] )
626 - M[1] * ( M[3]*M[8] - M[6]*M[5] )
627 + M[2] * ( M[3]*M[7] - M[6]*M[4] ));
639 os <<
"Matrix:" << endl;
641 for(
int i = 0; i < 16; i++){
642 os << setprecision(4) << matrix[i] <<
" ";
643 if(i % 4 == 3) os <<
" " << endl;