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?
User | Revision | Line number | New 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 | } |