Matrix.cpp

00001 #include "Matrix.h"
00002 
00003 Matrix::Matrix()
00004 {
00005     // Set matrix size
00006     rows = 1;
00007     cols = 1;
00008     // Allocate memory
00009     allocate_memmory();
00010     // Initialize data
00011     data[0][0] = 0.0;
00012 }
00013 
00014 Matrix::Matrix(int r, int c)
00015 {
00016     // Set matrix size
00017     rows = r;
00018     cols = c;
00019     // Allocate memory
00020     allocate_memmory();
00021     // Initialize data
00022     for (int i = 0; i < rows; i++) {
00023         for (int j = 0; j < cols; j++) {
00024             data[i][j] = 0.0;
00025         }
00026     }
00027 }
00028 
00029 Matrix::~Matrix()
00030 {
00031     // Free memory
00032     deallocate_memmory();
00033 }
00034 
00035 Matrix& Matrix::operator=(const Matrix& m)
00036 {
00037     // Re-allocate memmory (in case size is different)
00038     if (rows != m.rows || cols != m.cols) {
00039         deallocate_memmory();
00040         rows = m.rows;
00041         cols = m.cols;
00042         allocate_memmory();
00043     }
00044     // Copy data
00045     for (int i = 0; i < rows; i++) {
00046         for (int j = 0; j < cols; j++) {
00047             data[i][j] = m.data[i][j];
00048         }
00049     }
00050     return *this;
00051 }
00052 
00053 float& Matrix::operator()(int r, int c)
00054 {
00055     // Return cell data
00056     return data[r-1][c-1];
00057 }
00058 
00059 void Matrix::allocate_memmory()
00060 {
00061     data = (float **)malloc(rows * sizeof(float *));
00062     for (int i = 0; i < rows; i++) {
00063         data[i] = (float *)malloc(cols * sizeof(float));
00064     }
00065 }
00066 
00067 void Matrix::deallocate_memmory()
00068 {
00069     for (int i = 0; i < rows; i++) {
00070         free(data[i]);
00071     }
00072     free(data);
00073 }
00074 
00075 Matrix operator+(const Matrix& A, const Matrix& B)
00076 {
00077     // Auxiliary matrix where C = A+B
00078     Matrix C(A.rows,A.cols);
00079     for (int i = 0; i < C.rows; i++) {
00080         for (int j = 0; j < C.cols; j++) {
00081             C.data[i][j] = A.data[i][j]+B.data[i][j];
00082         }
00083     }
00084     return C;
00085 }
00086 
00087 Matrix operator-(const Matrix& A, const Matrix& B)
00088 {
00089     // Auxiliary matrix where C = A-B
00090     Matrix C(A.rows,A.cols);
00091     for (int i = 0; i < C.rows; i++) {
00092         for (int j = 0; j < C.cols; j++) {
00093             C.data[i][j] = A.data[i][j]-B.data[i][j];
00094         }
00095     }
00096     return C;
00097 }
00098 
00099 Matrix operator-(const Matrix& A)
00100 {
00101     // Auxiliary matrix where C = -A
00102     Matrix C(A.rows,A.cols);
00103     for (int i = 0; i < C.rows; i++) {
00104         for (int j = 0; j < C.cols; j++) {
00105             C.data[i][j] = -A.data[i][j];
00106         }
00107     }
00108     return C;
00109 }
00110 
00111 Matrix operator*(const Matrix& A, const Matrix& B)
00112 {
00113     // Auxiliary matrix where C = A*B
00114     Matrix C(A.rows,B.cols);
00115     for (int i = 0; i < A.rows; i++) {
00116         for (int j = 0; j < B.cols; j++) {
00117             for (int k = 0; k < A.cols; k++) {
00118                 C.data[i][j] += A.data[i][k]*B.data[k][j];
00119             }
00120         }
00121     }
00122     return C;
00123 }
00124 
00125 Matrix operator*(float k, const Matrix& A)
00126 {
00127     // Auxiliary matrix where B = k*A
00128     Matrix B(A.rows,A.cols);
00129     for (int i = 0; i < B.rows; i++) {
00130         for (int j = 0; j < B.cols; j++) {
00131             B.data[i][j] = k*A.data[i][j];
00132         }
00133     }
00134     return B;
00135 }
00136 
00137 Matrix operator*(const Matrix& A, float k)
00138 {
00139     // Auxiliary matrix where B = A*k
00140     Matrix B(A.rows,A.cols);
00141     for (int i = 0; i < B.rows; i++) {
00142         for (int j = 0; j < B.cols; j++) {
00143             B.data[i][j] = A.data[i][j]*k;
00144         }
00145     }
00146     return B;
00147 }
00148 
00149 Matrix operator/(const Matrix& A, float k)
00150 {
00151     // Auxiliary matrix where B = A/k
00152     Matrix B(A.rows,A.cols);
00153     for (int i = 0; i < B.rows; i++) {
00154         for (int j = 0; j < B.cols; j++) {
00155             B.data[i][j] = A.data[i][j]/k;
00156         }
00157     }
00158     return B;
00159 }
00160 
00161 Matrix eye(int r, int c)
00162 {
00163     if (c == 0) {
00164         c = r;
00165     }
00166     Matrix m(r, c);
00167     for (int i = 0; i < m.rows; i++) {
00168         for (int j = 0; j < m.cols; j++) {
00169             if (i == j) {
00170                 m.data[i][j] = 1.0;
00171             }
00172         }
00173     }
00174     return m;
00175 }
00176 
00177 Matrix zeros(int r, int c)
00178 {
00179     if (c == 0) {
00180         c = r;
00181     }
00182     Matrix m(r, c);
00183     return m;
00184 }
00185 
00186 Matrix transpose(const Matrix& A)
00187 {
00188     // Auxiliary matrix where B = A'
00189     Matrix B(A.cols, A.rows);
00190     for (int i = 0; i < B.rows; i++) {
00191         for (int j = 0; j < B.cols; j++) {
00192             B.data[i][j] = A.data[j][i];
00193         }
00194     }
00195     return B;
00196 }
00197 
00198 Matrix inverse(const Matrix& A)
00199 {
00200     // Apply A = LDL' factorization where L is a lower triangular matrix and D
00201     // is a block diagonal matrix
00202     Matrix L(A.rows, A.cols);
00203     Matrix D(A.rows, A.cols);
00204     float L_sum;
00205     float D_sum;
00206     for (int i = 0; i < A.rows; i++) {
00207         for (int j = 0; j < A.rows; j++) {
00208             if (i >= j) {
00209                 if (i == j) {
00210                     L.data[i][j] = 1.0;
00211                     D_sum = 0.0;
00212                     for (int k = 0; k <= (i-1); k++) {
00213                         D_sum += L.data[i][k]*L.data[i][k]*D.data[k][k];
00214                     }
00215                     D.data[i][j] = A.data[i][j] - D_sum;
00216                 } else {
00217                     L_sum = 0.0;
00218                     for (int k = 0; k <= (j-1); k++) {
00219                         L_sum += L.data[i][k]*L.data[j][k]*D.data[k][k];
00220                     }
00221                     L.data[i][j] = (1.0/D.data[j][j])*(A.data[i][j]-L_sum);
00222                 }
00223             }
00224         }
00225     }
00226     // Compute the inverse of L and D matrices
00227     Matrix L_inv(A.rows, A.cols);
00228     Matrix D_inv(A.rows, A.cols);
00229     float L_inv_sum;
00230     for (int i = 0; i < A.rows; i++) {
00231         for (int j = 0; j < A.rows; j++) {
00232             if (i >= j) {
00233                 if (i == j) {
00234                     L_inv.data[i][j] = 1.0/L.data[i][j];
00235                     D_inv.data[i][j] = 1.0/D.data[i][j];
00236                 } else {
00237                     L_inv_sum = 0.0;
00238                     for (int k = j; k <= (i-1); k++) {
00239                         L_inv_sum += L.data[i][k]*L_inv.data[k][j];
00240                     }
00241                     L_inv.data[i][j] = -L_inv_sum;
00242                 }
00243             }
00244         }
00245     }
00246     // Compute the inverse of A matrix in terms of the inverse of L and D
00247     // matrices
00248     return transpose(L_inv)*D_inv*L_inv;
00249 }
00250 
00251 float trace(const Matrix& A)
00252 {
00253     float t = 0.0;
00254     for (int i = 0; i < A.rows; i++) {
00255         t += A.data[i][i];
00256     }
00257     return t;
00258 }
00259 
00260 Matrix cross(const Matrix& u, const Matrix& v)
00261 {
00262     // Auxiliary matrix where w = uXv
00263     Matrix w(u.rows,u.cols);
00264     w.data[0][0] = u.data[1][0]*v.data[2][0]-u.data[2][0]*v.data[1][0];
00265     w.data[1][0] = u.data[2][0]*v.data[0][0]-u.data[0][0]*v.data[2][0];
00266     w.data[2][0] = u.data[0][0]*v.data[1][0]-u.data[1][0]*v.data[0][0];
00267     return w;
00268 }
00269 
00270 float norm(const Matrix& A)
00271 {
00272     float n = 0.0;
00273     for (int i = 0; i < A.rows; i++) {
00274         n += A.data[i][0]*A.data[i][0];
00275     }
00276     return sqrt(n);
00277 }
00278 
00279 Matrix dcm2quat(const Matrix& R)
00280 {
00281     Matrix q(4, 1);
00282     float tr = trace(R);
00283     if (tr > 0.0) {
00284         float sqtrp1 = sqrt( tr + 1.0);
00285         q.data[0][0] = 0.5*sqtrp1;
00286         q.data[1][0] = (R.data[1][2] - R.data[2][1])/(2.0*sqtrp1);
00287         q.data[2][0] = (R.data[2][0] - R.data[0][2])/(2.0*sqtrp1);
00288         q.data[3][0] = (R.data[0][1] - R.data[1][0])/(2.0*sqtrp1);
00289     } else {
00290         if ((R.data[1][1] > R.data[0][0]) && (R.data[1][1] > R.data[2][2])) {
00291             float sqdip1 = sqrt(R.data[1][1] - R.data[0][0] - R.data[2][2] + 1.0 );
00292             q.data[2][0] = 0.5*sqdip1;
00293             if ( sqdip1 != 0 ) {
00294                 sqdip1 = 0.5/sqdip1;
00295             }
00296             q.data[0][0] = (R.data[2][0] - R.data[0][2])*sqdip1;
00297             q.data[1][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
00298             q.data[3][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
00299         } else if (R.data[2][2] > R.data[0][0]) {
00300             float sqdip1 = sqrt(R.data[2][2] - R.data[0][0] - R.data[1][1] + 1.0 );
00301             q.data[3][0] = 0.5*sqdip1;
00302             if ( sqdip1 != 0 ) {
00303                 sqdip1 = 0.5/sqdip1;
00304             }
00305             q.data[0][0] = (R.data[0][1] - R.data[1][0])*sqdip1;
00306             q.data[1][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
00307             q.data[2][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
00308         } else {
00309             float sqdip1 = sqrt(R.data[0][0] - R.data[1][1] - R.data[2][2] + 1.0 );
00310             q.data[1][0] = 0.5*sqdip1;
00311             if ( sqdip1 != 0 ) {
00312                 sqdip1 = 0.5/sqdip1;
00313             }
00314             q.data[0][0] = (R.data[1][2] - R.data[2][1])*sqdip1;
00315             q.data[2][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
00316             q.data[3][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
00317         }
00318     }
00319     return q;
00320 }