Fabio Bobrow / Cubli

Cubli library

Matrix/Matrix.cpp@0:431ee55036ca, 2019-02-13 (annotated)

Committer:
fbob
Date:
Wed Feb 13 13:07:37 2019 +0000
Revision:
0:431ee55036ca
Child:
1:085840a3d767
Cubli library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:431ee55036ca 1 #include "Matrix.h"
fbob 0:431ee55036ca 2
fbob 0:431ee55036ca 3 Matrix::Matrix() : _rows(1), _cols(1)
fbob 0:431ee55036ca 4 {
fbob 0:431ee55036ca 5 // Allocate memory
fbob 0:431ee55036ca 6 _data = (float **)malloc(sizeof(float *));
fbob 0:431ee55036ca 7 _data[0] = (float *)malloc(sizeof(float));
fbob 0:431ee55036ca 8 // Initialize values
fbob 0:431ee55036ca 9 _data[0][0] = 0.0;
fbob 0:431ee55036ca 10 }
fbob 0:431ee55036ca 11
fbob 0:431ee55036ca 12 Matrix::Matrix(int rows, int cols) : _rows(rows), _cols(cols)
fbob 0:431ee55036ca 13 {
fbob 0:431ee55036ca 14
fbob 0:431ee55036ca 15 // Allocate memory
fbob 0:431ee55036ca 16 _data = (float **)malloc(_rows * sizeof(float *)); // = new float*[_rows];
fbob 0:431ee55036ca 17 for (int i = 1; i <= _rows; i++)
fbob 0:431ee55036ca 18 {
fbob 0:431ee55036ca 19 _data[i-1] = (float *)malloc(_cols * sizeof(float)); // = new float[_cols];
fbob 0:431ee55036ca 20 }
fbob 0:431ee55036ca 21 // Initialize values
fbob 0:431ee55036ca 22 for (int i = 1; i <= _rows; i++)
fbob 0:431ee55036ca 23 {
fbob 0:431ee55036ca 24 for (int j = 1; j <= _cols; j++)
fbob 0:431ee55036ca 25 {
fbob 0:431ee55036ca 26 _data[i-1][j-1] = 0.0;
fbob 0:431ee55036ca 27 }
fbob 0:431ee55036ca 28 }
fbob 0:431ee55036ca 29 }
fbob 0:431ee55036ca 30
fbob 0:431ee55036ca 31 Matrix::~Matrix()
fbob 0:431ee55036ca 32 {
fbob 0:431ee55036ca 33 // Free memory
fbob 0:431ee55036ca 34 for (int i = 0; i < _rows; ++i) {
fbob 0:431ee55036ca 35 free(_data[i]);
fbob 0:431ee55036ca 36 }
fbob 0:431ee55036ca 37 free(_data);
fbob 0:431ee55036ca 38 }
fbob 0:431ee55036ca 39
fbob 0:431ee55036ca 40 float& Matrix::operator()(int row, int col)
fbob 0:431ee55036ca 41 {
fbob 0:431ee55036ca 42 // Return cell data
fbob 0:431ee55036ca 43 return _data[row-1][col-1];
fbob 0:431ee55036ca 44 }
fbob 0:431ee55036ca 45
fbob 0:431ee55036ca 46 Matrix operator+(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 47 {
fbob 0:431ee55036ca 48 // Auxiliary matrix where C = A+B
fbob 0:431ee55036ca 49 Matrix C(A._rows,A._cols);
fbob 0:431ee55036ca 50 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 51 {
fbob 0:431ee55036ca 52 for (int j = 1; j <= A._cols; j++)
fbob 0:431ee55036ca 53 {
fbob 0:431ee55036ca 54 C._data[i-1][j-1] += A._data[i-1][j-1]+B._data[i-1][j-1];
fbob 0:431ee55036ca 55 }
fbob 0:431ee55036ca 56 }
fbob 0:431ee55036ca 57 return C;
fbob 0:431ee55036ca 58 }
fbob 0:431ee55036ca 59
fbob 0:431ee55036ca 60 Matrix operator-(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 61 {
fbob 0:431ee55036ca 62 // Auxiliary matrix where C = A-B
fbob 0:431ee55036ca 63 Matrix C(A._rows,A._cols);
fbob 0:431ee55036ca 64 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 65 {
fbob 0:431ee55036ca 66 for (int j = 1; j <= A._cols; j++)
fbob 0:431ee55036ca 67 {
fbob 0:431ee55036ca 68 C._data[i-1][j-1] += A._data[i-1][j-1]-B._data[i-1][j-1];
fbob 0:431ee55036ca 69 }
fbob 0:431ee55036ca 70 }
fbob 0:431ee55036ca 71 return C;
fbob 0:431ee55036ca 72 }
fbob 0:431ee55036ca 73
fbob 0:431ee55036ca 74 Matrix operator*(const Matrix& A, const Matrix& B)
fbob 0:431ee55036ca 75 {
fbob 0:431ee55036ca 76 // Auxiliary matrix where C = A*B
fbob 0:431ee55036ca 77 Matrix C(A._rows,B._cols);
fbob 0:431ee55036ca 78 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 79 {
fbob 0:431ee55036ca 80 for (int j = 1; j <= B._cols; j++)
fbob 0:431ee55036ca 81 {
fbob 0:431ee55036ca 82 for (int k = 1; k <= A._cols; k++)
fbob 0:431ee55036ca 83 {
fbob 0:431ee55036ca 84 C._data[i-1][j-1] += A._data[i-1][k-1]*B._data[k-1][j-1];
fbob 0:431ee55036ca 85 }
fbob 0:431ee55036ca 86 }
fbob 0:431ee55036ca 87 }
fbob 0:431ee55036ca 88 return C;
fbob 0:431ee55036ca 89 }
fbob 0:431ee55036ca 90
fbob 0:431ee55036ca 91 Matrix operator*(float scalar, const Matrix& A)
fbob 0:431ee55036ca 92 {
fbob 0:431ee55036ca 93 // Auxiliary matrix where C = scalar*A
fbob 0:431ee55036ca 94 Matrix C(A._rows,A._cols);
fbob 0:431ee55036ca 95 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 96 {
fbob 0:431ee55036ca 97 for (int j = 1; j <= A._cols; j++)
fbob 0:431ee55036ca 98 {
fbob 0:431ee55036ca 99 C._data[i-1][j-1] += scalar*A._data[i-1][j-1];
fbob 0:431ee55036ca 100 }
fbob 0:431ee55036ca 101 }
fbob 0:431ee55036ca 102 return C;
fbob 0:431ee55036ca 103 }
fbob 0:431ee55036ca 104
fbob 0:431ee55036ca 105 Matrix transpose(const Matrix& A)
fbob 0:431ee55036ca 106 {
fbob 0:431ee55036ca 107 // Auxiliary matrix where C = A'
fbob 0:431ee55036ca 108 Matrix C(A._cols, A._rows);
fbob 0:431ee55036ca 109 for (int i = 1; i <= C._rows; i++)
fbob 0:431ee55036ca 110 {
fbob 0:431ee55036ca 111 for (int j = 1; j <= C._cols; j++)
fbob 0:431ee55036ca 112 {
fbob 0:431ee55036ca 113 C._data[i-1][j-1] = A._data[j-1][i-1];
fbob 0:431ee55036ca 114 }
fbob 0:431ee55036ca 115 }
fbob 0:431ee55036ca 116 return C;
fbob 0:431ee55036ca 117 }
fbob 0:431ee55036ca 118
fbob 0:431ee55036ca 119 Matrix inverse(const Matrix& A)
fbob 0:431ee55036ca 120 {
fbob 0:431ee55036ca 121 // Apply A = LDL' factorization where L is a lower triangular matrix and D
fbob 0:431ee55036ca 122 // is a block diagonal matrix
fbob 0:431ee55036ca 123 Matrix L(A._cols, A._rows);
fbob 0:431ee55036ca 124 Matrix D(A._cols, A._rows);
fbob 0:431ee55036ca 125 float L_sum;
fbob 0:431ee55036ca 126 float D_sum;
fbob 0:431ee55036ca 127 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 128 {
fbob 0:431ee55036ca 129 for (int j = 1; j <= A._rows; j++)
fbob 0:431ee55036ca 130 {
fbob 0:431ee55036ca 131 if (i >= j)
fbob 0:431ee55036ca 132 {
fbob 0:431ee55036ca 133 if (i == j)
fbob 0:431ee55036ca 134 {
fbob 0:431ee55036ca 135 L._data[i-1][j-1] = 1.0;
fbob 0:431ee55036ca 136 D_sum = 0.0;
fbob 0:431ee55036ca 137 for (int k = 1; k <= (i-1); k++)
fbob 0:431ee55036ca 138 {
fbob 0:431ee55036ca 139 D_sum += L._data[i-1][k-1]*L._data[i-1][k-1]*D._data[k-1][k-1];
fbob 0:431ee55036ca 140 }
fbob 0:431ee55036ca 141 D._data[i-1][j-1] = A._data[i-1][j-1] - D_sum;
fbob 0:431ee55036ca 142 }
fbob 0:431ee55036ca 143 else
fbob 0:431ee55036ca 144 {
fbob 0:431ee55036ca 145 L_sum = 0.0;
fbob 0:431ee55036ca 146 for (int k = 1; k <= (j-1); k++)
fbob 0:431ee55036ca 147 {
fbob 0:431ee55036ca 148 L_sum += L._data[i-1][k-1]*L._data[j-1][k-1]*D._data[k-1][k-1];
fbob 0:431ee55036ca 149 }
fbob 0:431ee55036ca 150 L._data[i-1][j-1] = (1.0/D._data[j-1][j-1])*(A._data[i-1][j-1]-L_sum);
fbob 0:431ee55036ca 151 }
fbob 0:431ee55036ca 152 }
fbob 0:431ee55036ca 153 }
fbob 0:431ee55036ca 154 }
fbob 0:431ee55036ca 155 // Compute the inverse of L and D matrices
fbob 0:431ee55036ca 156 Matrix L_inv(A._cols, A._rows);
fbob 0:431ee55036ca 157 Matrix D_inv(A._cols, A._rows);
fbob 0:431ee55036ca 158 float L_inv_sum;
fbob 0:431ee55036ca 159 for (int i = 1; i <= A._rows; i++)
fbob 0:431ee55036ca 160 {
fbob 0:431ee55036ca 161 for (int j = 1; j <= A._rows; j++)
fbob 0:431ee55036ca 162 {
fbob 0:431ee55036ca 163 if (i >= j)
fbob 0:431ee55036ca 164 {
fbob 0:431ee55036ca 165 if (i == j)
fbob 0:431ee55036ca 166 {
fbob 0:431ee55036ca 167 L_inv._data[i-1][j-1] = 1.0/L._data[i-1][j-1];
fbob 0:431ee55036ca 168 D_inv._data[i-1][j-1] = 1.0/D._data[i-1][j-1];
fbob 0:431ee55036ca 169 }
fbob 0:431ee55036ca 170 else
fbob 0:431ee55036ca 171 {
fbob 0:431ee55036ca 172 L_inv_sum = 0.0;
fbob 0:431ee55036ca 173 for (int k = j; k <= (i-1); k++)
fbob 0:431ee55036ca 174 {
fbob 0:431ee55036ca 175 L_inv_sum += L._data[i-1][k-1]*L_inv._data[k-1][j-1];
fbob 0:431ee55036ca 176 }
fbob 0:431ee55036ca 177 L_inv._data[i-1][j-1] = -L_inv_sum;
fbob 0:431ee55036ca 178 }
fbob 0:431ee55036ca 179 }
fbob 0:431ee55036ca 180 }
fbob 0:431ee55036ca 181 }
fbob 0:431ee55036ca 182 // Compute the inverse of A matrix in terms of the inverse of L and D
fbob 0:431ee55036ca 183 // matrices
fbob 0:431ee55036ca 184 return transpose(L_inv)*D_inv*L_inv;
fbob 0:431ee55036ca 185 }