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 );
472 ifstream in(filename.c_str());
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++)
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] ));
638 inline ostream& operator<<(ostream& os, const Matrix4<T> matrix){
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;
Datastructures for holding loaded data.
Matrix4< BaseVecT > operator*(const ValueType &scale) const
Scales the matrix elemnts by the given factor.
A 4x4 matrix class implementation for use with the provided vertex types.
Matrix4(const Matrix4< T > &other)
Copy constructor.
Matrix4< BaseVecT > inv(bool &success)
Matrix4(T axis, ValueType angle)
Constructs a matrix from given axis and angle. Trys to avoid a gimbal lock.
ValueType * getData()
Returns the internal data array. Unsafe. Will probably removed in one of the next versions...
Matrix4< BaseVecT > operator*(const Matrix4< T > &other) const
Matrix-Matrix multiplication. Returns the new matrix.
ValueType & operator[](const int index)
Writeable index access.
Normal< T > operator*(const Normal< T > &v) const
Multiplication of Matrix and Vertex types.
Matrix4< BaseVecT > operator+(const Matrix4< T > &other) const
Matrix addition operator. Returns a new matrix.
ValueType det()
Returns the matrix's determinant.
A vector guaranteed to be normalized (length = 1).
void transpose()
Transposes the current matrix.
T operator*(const T &v) const
Multiplication of Matrix and Vertex types.
void submat(ValueType *submat, int i, int j)
Returns a sub matrix without row i and column j.
ValueType det3(const ValueType *M)
Calculates the determinant of a 3x3 matrix.
Matrix4(T *matrix)
Initializes a matrix wit the given data array. Ensure that the array has exactly 16 fields...
Eigen::Matrix4d toEigenMatrix()
boost::shared_array< float > floatArr
void operator*=(const T *other)
Matrix-Matrix multiplication (array based). See operator*}.
std::vector< ValueType > getVector()
Matrix4()
Default constructor. Initializes a identity matrix.
void operator*=(const Matrix4< T > &other)
Matrix-Matrix multiplication with self assigment.
Matrix4(const T &position, const T &angles)
Eigen::Matrix4d Matrix4d
Eigen 4x4 matrix, double precision.
Matrix4 & operator=(const Eigen::Matrix4d &mat)
ValueType operator[](const int index) const
Indexed element (reading) access.
void operator*=(const T scale)
Matrix scaling with self assignment.
typename BaseVector< float > ::CoordType ValueType
void toPostionAngle(ValueType pose[6])
Computes an Euler representation (x, y, z) plus three rotation values in rad. Rotations are with resp...
void loadFromFile(string filename)
Loads matrix values from a given file.
Matrix4< BaseVecT > operator+=(const Matrix4< T > &other)
Matrix addition operator.
Matrix4< BaseVecT > operator*(const T *&other) const
Matrix-Matrix multiplication (array based). Mainly implemented for compatibility with other math libs...