Fabio Bobrow / Cubli

Cubli library

Matrix/Matrix.cpp@2:dc7840a67f77, 2019-02-25 (annotated)

Committer:
fbob
Date:
Mon Feb 25 16:39:52 2019 +0000
Revision:
2:dc7840a67f77
Parent:
1:085840a3d767 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:431ee55036ca 1 #include "Matrix.h"
fbob 0:431ee55036ca 2
fbob 2:dc7840a67f77 3 Matrix::Matrix()
fbob 0:431ee55036ca 4 {
fbob 2:dc7840a67f77 5 // Set matrix size
fbob 2:dc7840a67f77 6 rows = 1;
fbob 2:dc7840a67f77 7 cols = 1;
fbob 1:085840a3d767 8 // Allocate memory
fbob 2:dc7840a67f77 9 allocate_memmory();
fbob 2:dc7840a67f77 10 // Initialize data
fbob 2:dc7840a67f77 11 data[0][0] = 0.0;
fbob 0:431ee55036ca 12 }
fbob 0:431ee55036ca 13
fbob 2:dc7840a67f77 14 Matrix::Matrix(int r, int c)
fbob 0:431ee55036ca 15 {
fbob 2:dc7840a67f77 16 // Set matrix size
fbob 2:dc7840a67f77 17 rows = r;
fbob 2:dc7840a67f77 18 cols = c;
fbob 1:085840a3d767 19 // Allocate memory
fbob 2:dc7840a67f77 20 allocate_memmory();
fbob 2:dc7840a67f77 21 // Initialize data
fbob 2:dc7840a67f77 22 for (int i = 0; i < rows; i++) {
fbob 2:dc7840a67f77 23 for (int j = 0; j < cols; j++) {
fbob 2:dc7840a67f77 24 data[i][j] = 0.0;
fbob 1:085840a3d767 25 }
fbob 1:085840a3d767 26 }
fbob 0:431ee55036ca 27 }
fbob 0:431ee55036ca 28
fbob 0:431ee55036ca 29 Matrix::~Matrix()
fbob 0:431ee55036ca 30 {
fbob 0:431ee55036ca 31 // Free memory
fbob 2:dc7840a67f77 32 deallocate_memmory();
fbob 0:431ee55036ca 33 }
fbob 0:431ee55036ca 34
fbob 1:085840a3d767 35 Matrix& Matrix::operator=(const Matrix& m)
fbob 1:085840a3d767 36 {
fbob 2:dc7840a67f77 37 // Re-allocate memmory (in case size is different)
fbob 2:dc7840a67f77 38 if (rows != m.rows || cols != m.cols) {
fbob 2:dc7840a67f77 39 deallocate_memmory();
fbob 2:dc7840a67f77 40 rows = m.rows;
fbob 2:dc7840a67f77 41 cols = m.cols;
fbob 2:dc7840a67f77 42 allocate_memmory();
fbob 2:dc7840a67f77 43 }
fbob 2:dc7840a67f77 44 // Copy data
fbob 2:dc7840a67f77 45 for (int i = 0; i < rows; i++) {
fbob 2:dc7840a67f77 46 for (int j = 0; j < cols; j++) {
fbob 2:dc7840a67f77 47 data[i][j] = m.data[i][j];
fbob 1:085840a3d767 48 }
fbob 1:085840a3d767 49 }
fbob 1:085840a3d767 50 return *this;
fbob 1:085840a3d767 51 }
fbob 1:085840a3d767 52
fbob 2:dc7840a67f77 53 float& Matrix::operator()(int r, int c)
fbob 0:431ee55036ca 54 {
fbob 1:085840a3d767 55 // Return cell data
fbob 2:dc7840a67f77 56 return data[r-1][c-1];
fbob 2:dc7840a67f77 57 }
fbob 2:dc7840a67f77 58
fbob 2:dc7840a67f77 59 void Matrix::allocate_memmory()
fbob 2:dc7840a67f77 60 {
fbob 2:dc7840a67f77 61 data = (float **)malloc(rows * sizeof(float *));
fbob 2:dc7840a67f77 62 for (int i = 0; i < rows; i++) {
fbob 2:dc7840a67f77 63 data[i] = (float *)malloc(cols * sizeof(float));
fbob 2:dc7840a67f77 64 }
fbob 2:dc7840a67f77 65 }
fbob 2:dc7840a67f77 66
fbob 2:dc7840a67f77 67 void Matrix::deallocate_memmory()
fbob 2:dc7840a67f77 68 {
fbob 2:dc7840a67f77 69 for (int i = 0; i < rows; i++) {
fbob 2:dc7840a67f77 70 free(data[i]);
fbob 2:dc7840a67f77 71 }
fbob 2:dc7840a67f77 72 free(data);
fbob 0:431ee55036ca 73 }
fbob 0:431ee55036ca 74
fbob 0:431ee55036ca 75 Matrix operator+(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 76 {
fbob 2:dc7840a67f77 77 // Auxiliary matrix where C = A+B
fbob 2:dc7840a67f77 78 Matrix C(A.rows,A.cols);
fbob 2:dc7840a67f77 79 for (int i = 0; i < C.rows; i++) {
fbob 2:dc7840a67f77 80 for (int j = 0; j < C.cols; j++) {
fbob 2:dc7840a67f77 81 C.data[i][j] = A.data[i][j]+B.data[i][j];
fbob 2:dc7840a67f77 82 }
fbob 0:431ee55036ca 83 }
fbob 0:431ee55036ca 84 return C;
fbob 0:431ee55036ca 85 }
fbob 0:431ee55036ca 86
fbob 0:431ee55036ca 87 Matrix operator-(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 88 {
fbob 0:431ee55036ca 89 // Auxiliary matrix where C = A-B
fbob 2:dc7840a67f77 90 Matrix C(A.rows,A.cols);
fbob 2:dc7840a67f77 91 for (int i = 0; i < C.rows; i++) {
fbob 2:dc7840a67f77 92 for (int j = 0; j < C.cols; j++) {
fbob 2:dc7840a67f77 93 C.data[i][j] = A.data[i][j]-B.data[i][j];
fbob 2:dc7840a67f77 94 }
fbob 2:dc7840a67f77 95 }
fbob 2:dc7840a67f77 96 return C;
fbob 2:dc7840a67f77 97 }
fbob 2:dc7840a67f77 98
fbob 2:dc7840a67f77 99 Matrix operator-(const Matrix& A)
fbob 2:dc7840a67f77 100 {
fbob 2:dc7840a67f77 101 // Auxiliary matrix where C = -A
fbob 2:dc7840a67f77 102 Matrix C(A.rows,A.cols);
fbob 2:dc7840a67f77 103 for (int i = 0; i < C.rows; i++) {
fbob 2:dc7840a67f77 104 for (int j = 0; j < C.cols; j++) {
fbob 2:dc7840a67f77 105 C.data[i][j] = -A.data[i][j];
fbob 1:085840a3d767 106 }
fbob 0:431ee55036ca 107 }
fbob 0:431ee55036ca 108 return C;
fbob 0:431ee55036ca 109 }
fbob 0:431ee55036ca 110
fbob 0:431ee55036ca 111 Matrix operator*(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 112 {
fbob 0:431ee55036ca 113 // Auxiliary matrix where C = A*B
fbob 2:dc7840a67f77 114 Matrix C(A.rows,B.cols);
fbob 2:dc7840a67f77 115 for (int i = 0; i < A.rows; i++) {
fbob 2:dc7840a67f77 116 for (int j = 0; j < B.cols; j++) {
fbob 2:dc7840a67f77 117 for (int k = 0; k < A.cols; k++) {
fbob 2:dc7840a67f77 118 C.data[i][j] += A.data[i][k]*B.data[k][j];
fbob 1:085840a3d767 119 }
fbob 0:431ee55036ca 120 }
fbob 0:431ee55036ca 121 }
fbob 0:431ee55036ca 122 return C;
fbob 0:431ee55036ca 123 }
fbob 0:431ee55036ca 124
fbob 2:dc7840a67f77 125 Matrix operator*(float k, const Matrix& A)
fbob 0:431ee55036ca 126 {
fbob 2:dc7840a67f77 127 // Auxiliary matrix where B = k*A
fbob 2:dc7840a67f77 128 Matrix B(A.rows,A.cols);
fbob 2:dc7840a67f77 129 for (int i = 0; i < B.rows; i++) {
fbob 2:dc7840a67f77 130 for (int j = 0; j < B.cols; j++) {
fbob 2:dc7840a67f77 131 B.data[i][j] = k*A.data[i][j];
fbob 1:085840a3d767 132 }
fbob 1:085840a3d767 133 }
fbob 2:dc7840a67f77 134 return B;
fbob 2:dc7840a67f77 135 }
fbob 2:dc7840a67f77 136
fbob 2:dc7840a67f77 137 Matrix operator*(const Matrix& A, float k)
fbob 2:dc7840a67f77 138 {
fbob 2:dc7840a67f77 139 // Auxiliary matrix where B = A*k
fbob 2:dc7840a67f77 140 Matrix B(A.rows,A.cols);
fbob 2:dc7840a67f77 141 for (int i = 0; i < B.rows; i++) {
fbob 2:dc7840a67f77 142 for (int j = 0; j < B.cols; j++) {
fbob 2:dc7840a67f77 143 B.data[i][j] = A.data[i][j]*k;
fbob 2:dc7840a67f77 144 }
fbob 2:dc7840a67f77 145 }
fbob 2:dc7840a67f77 146 return B;
fbob 1:085840a3d767 147 }
fbob 1:085840a3d767 148
fbob 2:dc7840a67f77 149 Matrix operator/(const Matrix& A, float k)
fbob 1:085840a3d767 150 {
fbob 2:dc7840a67f77 151 // Auxiliary matrix where B = A/k
fbob 2:dc7840a67f77 152 Matrix B(A.rows,A.cols);
fbob 2:dc7840a67f77 153 for (int i = 0; i < B.rows; i++) {
fbob 2:dc7840a67f77 154 for (int j = 0; j < B.cols; j++) {
fbob 2:dc7840a67f77 155 B.data[i][j] = A.data[i][j]/k;
fbob 1:085840a3d767 156 }
fbob 1:085840a3d767 157 }
fbob 2:dc7840a67f77 158 return B;
fbob 1:085840a3d767 159 }
fbob 1:085840a3d767 160
fbob 2:dc7840a67f77 161 Matrix eye(int r, int c)
fbob 1:085840a3d767 162 {
fbob 2:dc7840a67f77 163 if (c == 0) {
fbob 2:dc7840a67f77 164 c = r;
fbob 2:dc7840a67f77 165 }
fbob 2:dc7840a67f77 166 Matrix m(r, c);
fbob 2:dc7840a67f77 167 for (int i = 0; i < m.rows; i++) {
fbob 2:dc7840a67f77 168 for (int j = 0; j < m.cols; j++) {
fbob 2:dc7840a67f77 169 if (i == j) {
fbob 2:dc7840a67f77 170 m.data[i][j] = 1.0;
fbob 2:dc7840a67f77 171 }
fbob 1:085840a3d767 172 }
fbob 0:431ee55036ca 173 }
fbob 2:dc7840a67f77 174 return m;
fbob 2:dc7840a67f77 175 }
fbob 2:dc7840a67f77 176
fbob 2:dc7840a67f77 177 Matrix zeros(int r, int c)
fbob 2:dc7840a67f77 178 {
fbob 2:dc7840a67f77 179 if (c == 0) {
fbob 2:dc7840a67f77 180 c = r;
fbob 2:dc7840a67f77 181 }
fbob 2:dc7840a67f77 182 Matrix m(r, c);
fbob 2:dc7840a67f77 183 return m;
fbob 0:431ee55036ca 184 }
fbob 0:431ee55036ca 185
fbob 0:431ee55036ca 186 Matrix transpose(const Matrix& A)
fbob 0:431ee55036ca 187 {
fbob 2:dc7840a67f77 188 // Auxiliary matrix where B = A'
fbob 2:dc7840a67f77 189 Matrix B(A.cols, A.rows);
fbob 2:dc7840a67f77 190 for (int i = 0; i < B.rows; i++) {
fbob 2:dc7840a67f77 191 for (int j = 0; j < B.cols; j++) {
fbob 2:dc7840a67f77 192 B.data[i][j] = A.data[j][i];
fbob 0:431ee55036ca 193 }
fbob 0:431ee55036ca 194 }
fbob 2:dc7840a67f77 195 return B;
fbob 0:431ee55036ca 196 }
fbob 0:431ee55036ca 197
fbob 0:431ee55036ca 198 Matrix inverse(const Matrix& A)
fbob 0:431ee55036ca 199 {
fbob 1:085840a3d767 200 // Apply A = LDL' factorization where L is a lower triangular matrix and D
fbob 0:431ee55036ca 201 // is a block diagonal matrix
fbob 2:dc7840a67f77 202 Matrix L(A.rows, A.cols);
fbob 2:dc7840a67f77 203 Matrix D(A.rows, A.cols);
fbob 0:431ee55036ca 204 float L_sum;
fbob 0:431ee55036ca 205 float D_sum;
fbob 2:dc7840a67f77 206 for (int i = 0; i < A.rows; i++) {
fbob 2:dc7840a67f77 207 for (int j = 0; j < A.rows; j++) {
fbob 1:085840a3d767 208 if (i >= j) {
fbob 1:085840a3d767 209 if (i == j) {
fbob 2:dc7840a67f77 210 L.data[i][j] = 1.0;
fbob 0:431ee55036ca 211 D_sum = 0.0;
fbob 2:dc7840a67f77 212 for (int k = 0; k <= (i-1); k++) {
fbob 2:dc7840a67f77 213 D_sum += L.data[i][k]*L.data[i][k]*D.data[k][k];
fbob 0:431ee55036ca 214 }
fbob 2:dc7840a67f77 215 D.data[i][j] = A.data[i][j] - D_sum;
fbob 1:085840a3d767 216 } else {
fbob 0:431ee55036ca 217 L_sum = 0.0;
fbob 2:dc7840a67f77 218 for (int k = 0; k <= (j-1); k++) {
fbob 2:dc7840a67f77 219 L_sum += L.data[i][k]*L.data[j][k]*D.data[k][k];
fbob 0:431ee55036ca 220 }
fbob 2:dc7840a67f77 221 L.data[i][j] = (1.0/D.data[j][j])*(A.data[i][j]-L_sum);
fbob 0:431ee55036ca 222 }
fbob 0:431ee55036ca 223 }
fbob 0:431ee55036ca 224 }
fbob 0:431ee55036ca 225 }
fbob 1:085840a3d767 226 // Compute the inverse of L and D matrices
fbob 2:dc7840a67f77 227 Matrix L_inv(A.rows, A.cols);
fbob 2:dc7840a67f77 228 Matrix D_inv(A.rows, A.cols);
fbob 0:431ee55036ca 229 float L_inv_sum;
fbob 2:dc7840a67f77 230 for (int i = 0; i < A.rows; i++) {
fbob 2:dc7840a67f77 231 for (int j = 0; j < A.rows; j++) {
fbob 1:085840a3d767 232 if (i >= j) {
fbob 1:085840a3d767 233 if (i == j) {
fbob 2:dc7840a67f77 234 L_inv.data[i][j] = 1.0/L.data[i][j];
fbob 2:dc7840a67f77 235 D_inv.data[i][j] = 1.0/D.data[i][j];
fbob 1:085840a3d767 236 } else {
fbob 0:431ee55036ca 237 L_inv_sum = 0.0;
fbob 1:085840a3d767 238 for (int k = j; k <= (i-1); k++) {
fbob 2:dc7840a67f77 239 L_inv_sum += L.data[i][k]*L_inv.data[k][j];
fbob 0:431ee55036ca 240 }
fbob 2:dc7840a67f77 241 L_inv.data[i][j] = -L_inv_sum;
fbob 0:431ee55036ca 242 }
fbob 0:431ee55036ca 243 }
fbob 0:431ee55036ca 244 }
fbob 0:431ee55036ca 245 }
fbob 1:085840a3d767 246 // Compute the inverse of A matrix in terms of the inverse of L and D
fbob 1:085840a3d767 247 // matrices
fbob 0:431ee55036ca 248 return transpose(L_inv)*D_inv*L_inv;
fbob 0:431ee55036ca 249 }
fbob 1:085840a3d767 250
fbob 2:dc7840a67f77 251 float trace(const Matrix& A)
fbob 2:dc7840a67f77 252 {
fbob 2:dc7840a67f77 253 float t = 0.0;
fbob 2:dc7840a67f77 254 for (int i = 0; i < A.rows; i++) {
fbob 2:dc7840a67f77 255 t += A.data[i][i];
fbob 2:dc7840a67f77 256 }
fbob 2:dc7840a67f77 257 return t;
fbob 2:dc7840a67f77 258 }
fbob 2:dc7840a67f77 259
fbob 2:dc7840a67f77 260 Matrix cross(const Matrix& u, const Matrix& v)
fbob 2:dc7840a67f77 261 {
fbob 2:dc7840a67f77 262 // Auxiliary matrix where w = uXv
fbob 2:dc7840a67f77 263 Matrix w(u.rows,u.cols);
fbob 2:dc7840a67f77 264 w.data[0][0] = u.data[1][0]*v.data[2][0]-u.data[2][0]*v.data[1][0];
fbob 2:dc7840a67f77 265 w.data[1][0] = u.data[2][0]*v.data[0][0]-u.data[0][0]*v.data[2][0];
fbob 2:dc7840a67f77 266 w.data[2][0] = u.data[0][0]*v.data[1][0]-u.data[1][0]*v.data[0][0];
fbob 2:dc7840a67f77 267 return w;
fbob 2:dc7840a67f77 268 }
fbob 2:dc7840a67f77 269
fbob 1:085840a3d767 270 float norm(const Matrix& A)
fbob 1:085840a3d767 271 {
fbob 2:dc7840a67f77 272 float n = 0.0;
fbob 2:dc7840a67f77 273 for (int i = 0; i < A.rows; i++) {
fbob 2:dc7840a67f77 274 n += A.data[i][0]*A.data[i][0];
fbob 2:dc7840a67f77 275 }
fbob 1:085840a3d767 276 return sqrt(n);
fbob 1:085840a3d767 277 }
fbob 2:dc7840a67f77 278
fbob 2:dc7840a67f77 279 Matrix dcm2quat(const Matrix& R)
fbob 2:dc7840a67f77 280 {
fbob 2:dc7840a67f77 281 Matrix q(4, 1);
fbob 2:dc7840a67f77 282 float tr = trace(R);
fbob 2:dc7840a67f77 283 if (tr > 0.0) {
fbob 2:dc7840a67f77 284 float sqtrp1 = sqrt( tr + 1.0);
fbob 2:dc7840a67f77 285 q.data[0][0] = 0.5*sqtrp1;
fbob 2:dc7840a67f77 286 q.data[1][0] = (R.data[1][2] - R.data[2][1])/(2.0*sqtrp1);
fbob 2:dc7840a67f77 287 q.data[2][0] = (R.data[2][0] - R.data[0][2])/(2.0*sqtrp1);
fbob 2:dc7840a67f77 288 q.data[3][0] = (R.data[0][1] - R.data[1][0])/(2.0*sqtrp1);
fbob 2:dc7840a67f77 289 } else {
fbob 2:dc7840a67f77 290 if ((R.data[1][1] > R.data[0][0]) && (R.data[1][1] > R.data[2][2])) {
fbob 2:dc7840a67f77 291 float sqdip1 = sqrt(R.data[1][1] - R.data[0][0] - R.data[2][2] + 1.0 );
fbob 2:dc7840a67f77 292 q.data[2][0] = 0.5*sqdip1;
fbob 2:dc7840a67f77 293 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 294 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 295 }
fbob 2:dc7840a67f77 296 q.data[0][0] = (R.data[2][0] - R.data[0][2])*sqdip1;
fbob 2:dc7840a67f77 297 q.data[1][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
fbob 2:dc7840a67f77 298 q.data[3][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
fbob 2:dc7840a67f77 299 } else if (R.data[2][2] > R.data[0][0]) {
fbob 2:dc7840a67f77 300 float sqdip1 = sqrt(R.data[2][2] - R.data[0][0] - R.data[1][1] + 1.0 );
fbob 2:dc7840a67f77 301 q.data[3][0] = 0.5*sqdip1;
fbob 2:dc7840a67f77 302 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 303 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 304 }
fbob 2:dc7840a67f77 305 q.data[0][0] = (R.data[0][1] - R.data[1][0])*sqdip1;
fbob 2:dc7840a67f77 306 q.data[1][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
fbob 2:dc7840a67f77 307 q.data[2][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
fbob 2:dc7840a67f77 308 } else {
fbob 2:dc7840a67f77 309 float sqdip1 = sqrt(R.data[0][0] - R.data[1][1] - R.data[2][2] + 1.0 );
fbob 2:dc7840a67f77 310 q.data[1][0] = 0.5*sqdip1;
fbob 2:dc7840a67f77 311 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 312 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 313 }
fbob 2:dc7840a67f77 314 q.data[0][0] = (R.data[1][2] - R.data[2][1])*sqdip1;
fbob 2:dc7840a67f77 315 q.data[2][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
fbob 2:dc7840a67f77 316 q.data[3][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
fbob 2:dc7840a67f77 317 }
fbob 2:dc7840a67f77 318 }
fbob 2:dc7840a67f77 319 return q;
fbob 2:dc7840a67f77 320 }