Matrix4.hpp
Go to the documentation of this file.
1 
28 /*
29  * Matrix.hpp
30  *
31  * @date 26.08.2008
32  * @author Thomas Wiemann (twiemann@uos.de)
33  */
34 
35 #ifndef LVR2_MATRIX_H_
36 #define LVR2_MATRIX_H_
37 
38 #include <iostream>
39 #include <fstream>
40 #include <iomanip>
41 #include <vector>
42 
43 #include "lvr2/geometry/Normal.hpp"
44 #include "lvr2/io/DataStruct.hpp"
45 
46 #define _USE_MATH_DEFINES
47 #include <cmath>
48 
49 #include <Eigen/Dense>
50 
51 #ifndef M_PI
52 #define M_PI 3.141592654
53 #endif
54 using namespace std;
55 
56 namespace lvr2
57 {
58 
63 template<typename BaseVecT>
64 class Matrix4 {
65 public:
66 
67  using ValueType = typename BaseVecT::CoordType;
68 
73  {
74  for(int i = 0; i < 16; i++) m[i] = 0;
75  m[0] = m[5] = m[10] = m[15] = 1;
76  }
77 
82  template<typename T>
83  Matrix4(T* matrix)
84  {
85  for(int i = 0; i < 16; i++) m[i] = matrix[i];
86  }
87 
91  template<typename T>
92  Matrix4(const Matrix4<T>& other)
93  {
94  for(int i = 0; i < 16; i++) m[i] = other[i];
95  }
96 
101  template<typename T>
102  Matrix4(T axis, ValueType angle)
103  {
104  // Check for gimbal lock
105  if(fabs(angle) < 0.0001){
106 
107  bool invert_z = axis.z < 0;
108 
109  //Angle to yz-plane
110  float pitch = atan2(axis.z, axis.x) - M_PI_2;
111  if(pitch < 0.0f) pitch += 2.0f * M_PI;
112 
113  if(axis.x == 0.0f && axis.z == 0.0) pitch = 0.0f;
114 
115  //Transform axis into yz-plane
116  axis.x = axis.x * cos(pitch) + axis.z * sin(pitch);
117  axis.z = -axis.x * sin(pitch) + axis.z * cos(pitch);
118 
119  //Angle to y-Axis
120  float yaw = atan2(axis.y, axis.z);
121  if(yaw < 0) yaw += 2 * M_PI;
122 
123  Matrix4<BaseVecT> m1, m2, m3;
124 
125  if(invert_z) yaw = -yaw;
126 
127  cout << "YAW: " << yaw << " PITCH: " << pitch << endl;
128 
129  if(fabs(yaw) > 0.0001){
130  m2 = Matrix4(T(1.0, 0.0, 0.0), yaw);
131  m3 = m3 * m2;
132  }
133 
134  if(fabs(pitch) > 0.0001){
135  m1 = Matrix4(T(0.0, 1.0, 0.0), pitch);
136  m3 = m3 * m1;
137  }
138 
139  for(int i = 0; i < 16; i++) m[i] = m3[i];
140 
141  } else {
142  float c = cos(angle);
143  float s = sin(angle);
144  float t = 1.0f - c;
145  float tmp1, tmp2;
146 
147  // Normalize axis
148  Normal<ValueType> a(axis);
149 
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;
153 
154  tmp1 = a.x * a.y * t;
155  tmp2 = a.z * s;
156  m[ 4] = tmp1 + tmp2;
157  m[ 1] = tmp1 - tmp2;
158 
159  tmp1 = a.x * a.z * t;
160  tmp2 = a.y * s;
161  m[ 8] = tmp1 - tmp2;
162  m[ 2] = tmp1 + tmp2;
163 
164  tmp1 = a.y * a.z * t;
165  tmp2 = a.x * s;
166  m[ 9] = tmp1 + tmp2;
167  m[ 6] = tmp1 - tmp2;
168 
169  m[ 3] = m[ 7] = m[11] = 0.0;
170  m[12] = m[13] = m[14] = 0.0;
171  m[15] = 1.0;
172  }
173  }
174 
175  template<typename T>
176  Matrix4(const T &position, const T &angles)
177  {
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]);
184 
185  m[0] = cy*cz;
186  m[1] = sx*sy*cz + cx*sz;
187  m[2] = -cx*sy*cz + sx*sz;
188  m[3] = 0.0;
189  m[4] = -cy*sz;
190  m[5] = -sx*sy*sz + cx*cz;
191  m[6] = cx*sy*sz + sx*cz;
192  m[7] = 0.0;
193  m[8] = sy;
194  m[9] = -sx*cy;
195  m[10] = cx*cy;
196 
197  m[11] = 0.0;
198 
199  m[12] = position[0];
200  m[13] = position[1];
201  m[14] = position[2];
202  m[15] = 1;
203  }
204 
205  Matrix4(string filename);
206 
208  {
209 
210  }
211 
213  {
214  m[0] = mat(0, 0);
215  m[1] = mat(0, 1);
216  m[2] = mat(0, 2);
217  m[3] = mat(0, 3);
218 
219  m[4] = mat(1, 0);
220  m[5] = mat(1, 1);
221  m[6] = mat(1, 2);
222  m[7] = mat(1, 3);
223 
224  m[8] = mat(2, 0);
225  m[9] = mat(2, 1);
226  m[10] = mat(2, 2);
227  m[11] = mat(2, 3);
228 
229  m[12] = mat(3, 0);
230  m[13] = mat(3, 1);
231  m[14] = mat(3, 2);
232  m[15] = mat(3, 3);
233 
234  return *this;
235  }
236 
238  {
239  Eigen::Matrix4d mat;
240 
241  mat(0, 0) = m[0];
242  mat(0, 1) = m[1];
243  mat(0, 2) = m[2];
244  mat(0, 3) = m[3];
245 
246  mat(1, 0) = m[4];
247  mat(1, 1) = m[5];
248  mat(1, 2) = m[6];
249  mat(1, 3) = m[7];
250 
251  mat(2, 0) = m[8];
252  mat(2, 1) = m[9];
253  mat(2, 2) = m[10];
254  mat(2, 3) = m[11];
255 
256  mat(3, 0) = m[12];
257  mat(3, 1) = m[13];
258  mat(3, 2) = m[14];
259  mat(3, 3) = m[15];
260 
261  return mat;
262  }
263 
268  {
269  ValueType new_matrix[16];
270  for(int i = 0; i < 16; i++){
271  new_matrix[i] = m[i] * scale;
272  }
273  return Matrix4<BaseVecT>(new_matrix);
274  }
275 
280  template<typename T>
282  {
283  ValueType new_matrix[16];
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];
300  return Matrix4<BaseVecT>(new_matrix);
301  }
302 
307  template<typename T>
309  {
310  ValueType new_matrix[16];
311  for(int i = 0; i < 16; i++)
312  {
313  new_matrix[i] = m[i] + other[i];
314  }
315  return Matrix4<BaseVecT>(new_matrix);
316  }
317 
321  template<typename T>
323  {
324  //if(other != *this)
325  //{
326  return *this + other;
327  //}
328  //else
329  //{
330  //return *this;
331  //}
332  }
333 
340  template<typename T>
341  Matrix4<BaseVecT> operator*(const T* &other) const
342  {
343  ValueType new_matrix[16];
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];
360  return Matrix4<BaseVecT>(new_matrix);
361  }
362 
366  template<typename T>
367  T operator*(const T &v) const
368  {
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;
373 
374  x = x + m[12];
375  y = y + m[13];
376  z = z + m[14];
377 
378  return T(x, y, z);
379  }
380 
384  template<typename T>
385  Normal<T> operator*(const Normal<T> &v) const
386  {
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;
390 
391  return Normal<T>(x, y, z);
392  }
393 
401  void set(int i, ValueType value){m[i] = value;};
402 
406  void transpose()
407  {
408  ValueType m_tmp[16];
409  m_tmp[0] = m[0];
410  m_tmp[4] = m[1];
411  m_tmp[8] = m[2];
412  m_tmp[12] = m[3];
413  m_tmp[1] = m[4];
414  m_tmp[5] = m[5];
415  m_tmp[9] = m[6];
416  m_tmp[13] = m[7];
417  m_tmp[2] = m[8];
418  m_tmp[6] = m[9];
419  m_tmp[10] = m[10];
420  m_tmp[14] = m[11];
421  m_tmp[3] = m[12];
422  m_tmp[7] = m[13];
423  m_tmp[11] = m[14];
424  m_tmp[15] = m[15];
425  for(int i = 0; i < 16; i++) m[i] = m_tmp[i];
426  }
427 
433  void toPostionAngle(ValueType pose[6])
434  {
435  if(pose != 0){
436  float _trX, _trY;
437  if(m[0] > 0.0) {
438  pose[4] = asin(m[8]);
439  } else {
440  pose[4] = (float)M_PI - asin(m[8]);
441  }
442  // rPosTheta[1] = asin( m[8]); // Calculate Y-axis angle
443 
444  float C = cos( pose[4] );
445  if ( fabs( C ) > 0.005 ) { // Gimball lock?
446  _trX = m[10] / C; // No, so get X-axis angle
447  _trY = -m[9] / C;
448  pose[3] = atan2( _trY, _trX );
449  _trX = m[0] / C; // Get Z-axis angle
450  _trY = -m[4] / C;
451  pose[5] = atan2( _trY, _trX );
452  } else { // Gimball lock has occurred
453  pose[3] = 0.0; // Set X-axis angle to zero
454  _trX = m[5]; //1 // And calculate Z-axis angle
455  _trY = m[1]; //2
456  pose[5] = atan2( _trY, _trX );
457  }
458 
459  // cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;
460 
461  pose[0] = m[12];
462  pose[1] = m[13];
463  pose[2] = m[14];
464  }
465  }
466 
470  void loadFromFile(string filename)
471  {
472  ifstream in(filename.c_str());
473  for(int i = 0; i < 16; i++){
474  if(!in.good()){
475  cout << "Warning: Matrix::loadFromFile: File not found or corrupted: " << filename << endl;
476  return;
477  }
478  in >> m[i];
479  }
480  }
481 
485  template<typename T>
486  void operator*=(const T scale)
487  {
488  *this = *this * scale;
489  }
490 
494  template<typename T>
495  void operator*=(const Matrix4<T>& other)
496  {
497  *this = *this * other;
498  }
499 
503  template<typename T>
504  void operator*=(const T* other)
505  {
506  *this = *this * other;
507  }
508 
513  ValueType* getData(){ return m;}
514 
516  {
517  floatArr a(new float[16]);
518  for(int i = 0; i < 16; i++)
519  {
520  a[i] = m[i];
521  }
522  return a;
523  }
524 
525  std::vector<ValueType> getVector()
526  {
527  std::vector<ValueType> tmp(16);
528  for(int i = 0; i < 16; i++)
529  {
530  tmp.push_back(m[i]);
531  }
532  return tmp;
533  }
534 
538  ValueType at(const int i) const;
539 
543  ValueType operator[](const int index) const
544  {
546  return m[index];
547  }
548 
549 
553  ValueType& operator[](const int index)
554  {
555  return m[index];
556  }
557 
562  {
563  ValueType det, result = 0, i = 1.0;
564  ValueType Msub3[9];
565  int n;
566  for ( n = 0; n < 4; n++, i *= -1.0 ) {
567  submat( Msub3, 0, n );
568  det = det3( Msub3 );
569  result += m[n] * det * i;
570  }
571  return( result );
572  }
573 
574  Matrix4<BaseVecT> inv(bool& success)
575  {
576  Matrix4<BaseVecT> Mout;
577  ValueType mdet = det();
578  if ( fabs( mdet ) < 0.00000000000005 ) {
579  cout << "Error matrix inverting! " << mdet << endl;
580  return Mout;
581  }
582  ValueType mtemp[9];
583  int i, j, sign;
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;
589  }
590  }
591  return Mout;
592  }
593 
594  ValueType m[16];
595 
596 private:
597 
601  void submat(ValueType* submat, int i, int j)
602  {
603  int di, dj, si, sj;
604  // loop through 3x3 submatrix
605  for( di = 0; di < 3; di ++ ) {
606  for( dj = 0; dj < 3; dj ++ ) {
607  // map 3x3 element (destination) to 4x4 element (source)
608  si = di + ( ( di >= i ) ? 1 : 0 );
609  sj = dj + ( ( dj >= j ) ? 1 : 0 );
610  // copy element
611  submat[di * 3 + dj] = m[si * 4 + sj];
612  }
613  }
614  }
615 
623  {
624  ValueType det;
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] ));
628  return ( det );
629  }
630 
631 
632 };
633 
637 template<typename T>
638 inline ostream& operator<<(ostream& os, const Matrix4<T> matrix){
639  os << "Matrix:" << endl;
640  os << fixed;
641  for(int i = 0; i < 16; i++){
642  os << setprecision(4) << matrix[i] << " ";
643  if(i % 4 == 3) os << " " << endl;
644  }
645  os << endl;
646  return os;
647 }
648 
649 } // namespace lvr2
650 
651 #endif /* MATRIX_H_ */
Datastructures for holding loaded data.
Matrix4< BaseVecT > operator*(const ValueType &scale) const
Scales the matrix elemnts by the given factor.
Definition: Matrix4.hpp:267
A 4x4 matrix class implementation for use with the provided vertex types.
Definition: Matrix4.hpp:64
Matrix4(const Matrix4< T > &other)
Copy constructor.
Definition: Matrix4.hpp:92
Matrix4< BaseVecT > inv(bool &success)
Definition: Matrix4.hpp:574
Matrix4(T axis, ValueType angle)
Constructs a matrix from given axis and angle. Trys to avoid a gimbal lock.
Definition: Matrix4.hpp:102
ValueType * getData()
Returns the internal data array. Unsafe. Will probably removed in one of the next versions...
Definition: Matrix4.hpp:513
floatArr toFloatArray()
Definition: Matrix4.hpp:515
Matrix4< BaseVecT > operator*(const Matrix4< T > &other) const
Matrix-Matrix multiplication. Returns the new matrix.
Definition: Matrix4.hpp:281
ValueType & operator[](const int index)
Writeable index access.
Definition: Matrix4.hpp:553
Normal< T > operator*(const Normal< T > &v) const
Multiplication of Matrix and Vertex types.
Definition: Matrix4.hpp:385
Matrix4< BaseVecT > operator+(const Matrix4< T > &other) const
Matrix addition operator. Returns a new matrix.
Definition: Matrix4.hpp:308
ValueType det()
Returns the matrix&#39;s determinant.
Definition: Matrix4.hpp:561
#define M_PI
Definition: Matrix4.hpp:52
A vector guaranteed to be normalized (length = 1).
Definition: BaseVector.hpp:49
void transpose()
Transposes the current matrix.
Definition: Matrix4.hpp:406
T operator*(const T &v) const
Multiplication of Matrix and Vertex types.
Definition: Matrix4.hpp:367
void submat(ValueType *submat, int i, int j)
Returns a sub matrix without row i and column j.
Definition: Matrix4.hpp:601
ValueType det3(const ValueType *M)
Calculates the determinant of a 3x3 matrix.
Definition: Matrix4.hpp:622
Matrix4(T *matrix)
Initializes a matrix wit the given data array. Ensure that the array has exactly 16 fields...
Definition: Matrix4.hpp:83
Eigen::Matrix4d toEigenMatrix()
Definition: Matrix4.hpp:237
boost::shared_array< float > floatArr
Definition: DataStruct.hpp:133
void operator*=(const T *other)
Matrix-Matrix multiplication (array based). See operator*}.
Definition: Matrix4.hpp:504
std::vector< ValueType > getVector()
Definition: Matrix4.hpp:525
Matrix4()
Default constructor. Initializes a identity matrix.
Definition: Matrix4.hpp:72
void operator*=(const Matrix4< T > &other)
Matrix-Matrix multiplication with self assigment.
Definition: Matrix4.hpp:495
Matrix4(const T &position, const T &angles)
Definition: Matrix4.hpp:176
Eigen::Matrix4d Matrix4d
Eigen 4x4 matrix, double precision.
Matrix4 & operator=(const Eigen::Matrix4d &mat)
Definition: Matrix4.hpp:212
ValueType operator[](const int index) const
Indexed element (reading) access.
Definition: Matrix4.hpp:543
void operator*=(const T scale)
Matrix scaling with self assignment.
Definition: Matrix4.hpp:486
typename BaseVector< float > ::CoordType ValueType
Definition: Matrix4.hpp:67
void toPostionAngle(ValueType pose[6])
Computes an Euler representation (x, y, z) plus three rotation values in rad. Rotations are with resp...
Definition: Matrix4.hpp:433
void loadFromFile(string filename)
Loads matrix values from a given file.
Definition: Matrix4.hpp:470
Matrix4< BaseVecT > operator+=(const Matrix4< T > &other)
Matrix addition operator.
Definition: Matrix4.hpp:322
Matrix4< BaseVecT > operator*(const T *&other) const
Matrix-Matrix multiplication (array based). Mainly implemented for compatibility with other math libs...
Definition: Matrix4.hpp:341


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Mon Feb 28 2022 22:46:08