Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #ifndef MATRIX_H_
00009 #define MATRIX_H_
00010
00011 #include <iostream>
00012 #include <fstream>
00013 #include <iomanip>
00014
00015 #include "Vertex.hpp"
00016 #include "Normal.hpp"
00017
00018 #define _USE_MATH_DEFINES
00019 #include <cmath>
00020
00021 using namespace std;
00022
00023 namespace lssr{
00024
00029 template<typename ValueType>
00030 class Matrix4 {
00031 public:
00032
00036 Matrix4()
00037 {
00038 for(int i = 0; i < 16; i++) m[i] = 0;
00039 m[0] = m[5] = m[10] = m[15] = 1;
00040 }
00041
00046 template<typename T>
00047 Matrix4(T* matrix)
00048 {
00049 for(int i = 0; i < 16; i++) m[i] = matrix[i];
00050 }
00051
00055 template<typename T>
00056 Matrix4(const Matrix4<T>& other)
00057 {
00058 for(int i = 0; i < 16; i++) m[i] = other[i];
00059 }
00060
00065 template<typename T>
00066 Matrix4(Vertex<T> axis, ValueType angle)
00067 {
00068
00069 if(fabs(angle) < 0.0001){
00070
00071 bool invert_z = axis.z < 0;
00072
00073
00074 float pitch = atan2(axis.z, axis.x) - M_PI_2;
00075 if(pitch < 0.0f) pitch += 2.0f * M_PI;
00076
00077 if(axis.x == 0.0f && axis.z == 0.0) pitch = 0.0f;
00078
00079
00080 axis.x = axis.x * cos(pitch) + axis.z * sin(pitch);
00081 axis.z = -axis.x * sin(pitch) + axis.z * cos(pitch);
00082
00083
00084 float yaw = atan2(axis.y, axis.z);
00085 if(yaw < 0) yaw += 2 * M_PI;
00086
00087 Matrix4 m1, m2, m3;
00088
00089 if(invert_z) yaw = -yaw;
00090
00091 cout << "YAW: " << yaw << " PITCH: " << pitch << endl;
00092
00093 if(fabs(yaw) > 0.0001){
00094 m2 = Matrix4(Vertex<T>(1.0, 0.0, 0.0), yaw);
00095 m3 = m3 * m2;
00096 }
00097
00098 if(fabs(pitch) > 0.0001){
00099 m1 = Matrix4(Vertex<T>(0.0, 1.0, 0.0), pitch);
00100 m3 = m3 * m1;
00101 }
00102
00103 for(int i = 0; i < 16; i++) m[i] = m3[i];
00104
00105 } else {
00106 float c = cos(angle);
00107 float s = sin(angle);
00108 float t = 1.0f - c;
00109 float tmp1, tmp2;
00110
00111
00112 Normal<T> a(axis);
00113
00114 m[ 0] = c + a.x * a.x * t;
00115 m[ 5] = c + a.y * a.y * t;
00116 m[10] = c + a.z * a.z * t;
00117
00118 tmp1 = a.x * a.y * t;
00119 tmp2 = a.z * s;
00120 m[ 4] = tmp1 + tmp2;
00121 m[ 1] = tmp1 - tmp2;
00122
00123 tmp1 = a.x * a.z * t;
00124 tmp2 = a.y * s;
00125 m[ 8] = tmp1 - tmp2;
00126 m[ 2] = tmp1 + tmp2;
00127
00128 tmp1 = a.y * a.z * t;
00129 tmp2 = a.x * s;
00130 m[ 9] = tmp1 + tmp2;
00131 m[ 6] = tmp1 - tmp2;
00132
00133 m[ 3] = m[ 7] = m[11] = 0.0;
00134 m[12] = m[13] = m[14] = 0.0;
00135 m[15] = 1.0;
00136 }
00137 }
00138
00139 template<typename T>
00140 Matrix4(const Vertex<T> &position, const Vertex<T> &angles)
00141 {
00142 float sx = sin(angles[0]);
00143 float cx = cos(angles[0]);
00144 float sy = sin(angles[1]);
00145 float cy = cos(angles[1]);
00146 float sz = sin(angles[2]);
00147 float cz = cos(angles[2]);
00148
00149 m[0] = cy*cz;
00150 m[1] = sx*sy*cz + cx*sz;
00151 m[2] = -cx*sy*cz + sx*sz;
00152 m[3] = 0.0;
00153 m[4] = -cy*sz;
00154 m[5] = -sx*sy*sz + cx*cz;
00155 m[6] = cx*sy*sz + sx*cz;
00156 m[7] = 0.0;
00157 m[8] = sy;
00158 m[9] = -sx*cy;
00159 m[10] = cx*cy;
00160
00161 m[11] = 0.0;
00162
00163 m[12] = position[0];
00164 m[13] = position[1];
00165 m[14] = position[2];
00166 m[15] = 1;
00167 }
00168
00169 Matrix4(string filename);
00170
00171 virtual ~Matrix4()
00172 {
00173
00174 }
00175
00179 template<typename T>
00180 Matrix4 operator*(const T &scale) const
00181 {
00182 ValueType new_matrix[16];
00183 for(int i = 0; i < 16; i++){
00184 new_matrix[i] = m[i] * scale;
00185 }
00186 return Matrix4<ValueType>(new_matrix);
00187 }
00188
00193 template<typename T>
00194 Matrix4 operator*(const Matrix4<T> &other) const
00195 {
00196 ValueType new_matrix[16];
00197 new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
00198 new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
00199 new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
00200 new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
00201 new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
00202 new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
00203 new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
00204 new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
00205 new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
00206 new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
00207 new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
00208 new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
00209 new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
00210 new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
00211 new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
00212 new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
00213 return Matrix4<ValueType>(new_matrix);
00214 }
00215
00222 template<typename T>
00223 Matrix4 operator*(const T* &other) const
00224 {
00225 ValueType new_matrix[16];
00226 new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
00227 new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
00228 new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
00229 new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
00230 new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
00231 new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
00232 new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
00233 new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
00234 new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
00235 new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
00236 new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
00237 new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
00238 new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
00239 new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
00240 new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
00241 new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
00242 return Matrix4<ValueType>(new_matrix);
00243 }
00244
00248 template<typename T>
00249 Vertex<T> operator*(const Vertex<T> &v) const
00250 {
00251 T x = m[ 0] * v.x + m[ 4] * v.y + m[8 ] * v.z;
00252 T y = m[ 1] * v.x + m[ 5] * v.y + m[9 ] * v.z;
00253 T z = m[ 2] * v.x + m[ 6] * v.y + m[10] * v.z;
00254
00255 x = x + m[12];
00256 y = y + m[13];
00257 z = z + m[14];
00258
00259 return Vertex<T>(x, y, z);
00260 }
00261
00269 void set(int i, ValueType value){m[i] = value;};
00270
00274 void transpose()
00275 {
00276 ValueType m_tmp[16];
00277 m_tmp[0] = m[0];
00278 m_tmp[4] = m[1];
00279 m_tmp[8] = m[2];
00280 m_tmp[12] = m[3];
00281 m_tmp[1] = m[4];
00282 m_tmp[5] = m[5];
00283 m_tmp[9] = m[6];
00284 m_tmp[13] = m[7];
00285 m_tmp[2] = m[8];
00286 m_tmp[6] = m[9];
00287 m_tmp[10] = m[10];
00288 m_tmp[14] = m[11];
00289 m_tmp[3] = m[12];
00290 m_tmp[7] = m[13];
00291 m_tmp[11] = m[14];
00292 m_tmp[15] = m[15];
00293 for(int i = 0; i < 16; i++) m[i] = m_tmp[i];
00294 }
00295
00301 void toPostionAngle(ValueType pose[6])
00302 {
00303 if(pose != 0){
00304 float _trX, _trY;
00305 if(m[0] > 0.0) {
00306 pose[4] = asin(m[8]);
00307 } else {
00308 pose[4] = M_PI - asin(m[8]);
00309 }
00310
00311
00312 float C = cos( pose[4] );
00313 if ( fabs( C ) > 0.005 ) {
00314 _trX = m[10] / C;
00315 _trY = -m[9] / C;
00316 pose[3] = atan2( _trY, _trX );
00317 _trX = m[0] / C;
00318 _trY = -m[4] / C;
00319 pose[5] = atan2( _trY, _trX );
00320 } else {
00321 pose[3] = 0.0;
00322 _trX = m[5];
00323 _trY = m[1];
00324 pose[5] = atan2( _trY, _trX );
00325 }
00326
00327 pose[0] = m[12];
00328 pose[1] = m[13];
00329 pose[2] = m[14];
00330 }
00331 }
00332
00336 void loadFromFile(string filename)
00337 {
00338 ifstream in(filename.c_str());
00339 for(int i = 0; i < 16; i++){
00340 if(!in.good()){
00341 cout << "Warning: Matrix::loadFromFile: File not found or corrupted." << endl;
00342 return;
00343 }
00344 in >> m[i];
00345 }
00346 }
00347
00351 template<typename T>
00352 void operator*=(const T scale)
00353 {
00354 *this = *this * scale;
00355 }
00356
00360 template<typename T>
00361 void operator*=(const Matrix4<T>& other)
00362 {
00363 *this = *this * other;
00364 }
00365
00369 template<typename T>
00370 void operator*=(const T* other)
00371 {
00372 *this = *this * other;
00373 }
00374
00379 ValueType* getData(){ return m;};
00380
00384 ValueType at(const int i) const;
00385
00389 ValueType operator[](const int index) const
00390 {
00392 return m[index];
00393 }
00394
00395 private:
00396 ValueType m[16];
00397 };
00398
00402 template<typename T>
00403 inline ostream& operator<<(ostream& os, const Matrix4<T> matrix){
00404 os << "Matrix:" << endl;
00405 os << fixed;
00406 for(int i = 0; i < 16; i++){
00407 os << setprecision(4) << matrix[i] << " ";
00408 if(i % 4 == 3) os << " " << endl;
00409 }
00410 os << endl;
00411 return os;
00412 }
00413
00414 }
00415 #endif