Cubli library
Matrix/Matrix.cpp@1:085840a3d767, 2019-02-13 (annotated)
- Committer:
- fbob
- Date:
- Wed Feb 13 17:06:28 2019 +0000
- Revision:
- 1:085840a3d767
- Parent:
- 0:431ee55036ca
- Child:
- 2:dc7840a67f77
Update
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 | 1:085840a3d767 | 5 | // Allocate memory |
fbob | 1:085840a3d767 | 6 | _data = (float **)malloc(sizeof(float *)); |
fbob | 1:085840a3d767 | 7 | _data[0] = (float *)malloc(sizeof(float)); |
fbob | 1:085840a3d767 | 8 | // Initialize values |
fbob | 1:085840a3d767 | 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 | 1:085840a3d767 | 15 | // Allocate memory |
fbob | 1:085840a3d767 | 16 | _data = (float **)malloc(_rows * sizeof(float *)); // = new float*[_rows]; |
fbob | 1:085840a3d767 | 17 | for (int i = 1; i <= _rows; i++) { |
fbob | 1:085840a3d767 | 18 | _data[i-1] = (float *)malloc(_cols * sizeof(float)); // = new float[_cols]; |
fbob | 0:431ee55036ca | 19 | } |
fbob | 1:085840a3d767 | 20 | // Initialize values |
fbob | 1:085840a3d767 | 21 | for (int i = 1; i <= _rows; i++) { |
fbob | 1:085840a3d767 | 22 | for (int j = 1; j <= _cols; j++) { |
fbob | 1:085840a3d767 | 23 | _data[i-1][j-1] = 0.0; |
fbob | 1:085840a3d767 | 24 | } |
fbob | 1:085840a3d767 | 25 | } |
fbob | 0:431ee55036ca | 26 | } |
fbob | 0:431ee55036ca | 27 | |
fbob | 0:431ee55036ca | 28 | Matrix::~Matrix() |
fbob | 0:431ee55036ca | 29 | { |
fbob | 0:431ee55036ca | 30 | // Free memory |
fbob | 1:085840a3d767 | 31 | for (int i = 1; i <= _rows; i++) { |
fbob | 1:085840a3d767 | 32 | free(_data[i-1]); |
fbob | 0:431ee55036ca | 33 | } |
fbob | 0:431ee55036ca | 34 | free(_data); |
fbob | 0:431ee55036ca | 35 | } |
fbob | 0:431ee55036ca | 36 | |
fbob | 1:085840a3d767 | 37 | Matrix& Matrix::operator=(const Matrix& m) |
fbob | 1:085840a3d767 | 38 | { |
fbob | 1:085840a3d767 | 39 | for (int i = 1; i <= _rows; i++) { |
fbob | 1:085840a3d767 | 40 | for (int j = 1; j <= _cols; j++) { |
fbob | 1:085840a3d767 | 41 | _data[i-1][j-1] = m._data[i-1][j-1]; |
fbob | 1:085840a3d767 | 42 | } |
fbob | 1:085840a3d767 | 43 | } |
fbob | 1:085840a3d767 | 44 | return *this; |
fbob | 1:085840a3d767 | 45 | } |
fbob | 1:085840a3d767 | 46 | |
fbob | 0:431ee55036ca | 47 | float& Matrix::operator()(int row, int col) |
fbob | 0:431ee55036ca | 48 | { |
fbob | 1:085840a3d767 | 49 | // Return cell data |
fbob | 1:085840a3d767 | 50 | return _data[row-1][col-1]; |
fbob | 0:431ee55036ca | 51 | } |
fbob | 0:431ee55036ca | 52 | |
fbob | 0:431ee55036ca | 53 | Matrix operator+(const Matrix& A, const Matrix& B) |
fbob | 0:431ee55036ca | 54 | { |
fbob | 1:085840a3d767 | 55 | /*// Auxiliary matrix where C = A+B |
fbob | 1:085840a3d767 | 56 | Matrix C(A._rows,A._cols); |
fbob | 1:085840a3d767 | 57 | for (int i = 1; i <= A._rows; i++) |
fbob | 1:085840a3d767 | 58 | { |
fbob | 1:085840a3d767 | 59 | for (int j = 1; j <= A._cols; j++) |
fbob | 1:085840a3d767 | 60 | { |
fbob | 1:085840a3d767 | 61 | C._data[i-1][j-1] += A._data[i-1][j-1]+B._data[i-1][j-1]; |
fbob | 1:085840a3d767 | 62 | } |
fbob | 1:085840a3d767 | 63 | } |
fbob | 1:085840a3d767 | 64 | return C; |
fbob | 1:085840a3d767 | 65 | */ |
fbob | 1:085840a3d767 | 66 | |
fbob | 1:085840a3d767 | 67 | /*Matrix temp(m1); |
fbob | 1:085840a3d767 | 68 | return (temp += m2); |
fbob | 1:085840a3d767 | 69 | */ |
fbob | 1:085840a3d767 | 70 | |
fbob | 1:085840a3d767 | 71 | //Matrix A(m1); |
fbob | 1:085840a3d767 | 72 | //Matrix B(m2); |
fbob | 0:431ee55036ca | 73 | Matrix C(A._rows,A._cols); |
fbob | 0:431ee55036ca | 74 | for (int i = 1; i <= A._rows; i++) |
fbob | 0:431ee55036ca | 75 | { |
fbob | 0:431ee55036ca | 76 | for (int j = 1; j <= A._cols; j++) |
fbob | 0:431ee55036ca | 77 | { |
fbob | 0:431ee55036ca | 78 | C._data[i-1][j-1] += A._data[i-1][j-1]+B._data[i-1][j-1]; |
fbob | 0:431ee55036ca | 79 | } |
fbob | 0:431ee55036ca | 80 | } |
fbob | 0:431ee55036ca | 81 | return C; |
fbob | 0:431ee55036ca | 82 | } |
fbob | 0:431ee55036ca | 83 | |
fbob | 0:431ee55036ca | 84 | Matrix operator-(const Matrix& A, const Matrix& B) |
fbob | 0:431ee55036ca | 85 | { |
fbob | 0:431ee55036ca | 86 | // Auxiliary matrix where C = A-B |
fbob | 0:431ee55036ca | 87 | Matrix C(A._rows,A._cols); |
fbob | 1:085840a3d767 | 88 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 89 | for (int j = 1; j <= A._cols; j++) { |
fbob | 1:085840a3d767 | 90 | C._data[i-1][j-1] += A._data[i-1][j-1]-B._data[i-1][j-1]; |
fbob | 1:085840a3d767 | 91 | } |
fbob | 0:431ee55036ca | 92 | } |
fbob | 0:431ee55036ca | 93 | return C; |
fbob | 0:431ee55036ca | 94 | } |
fbob | 0:431ee55036ca | 95 | |
fbob | 0:431ee55036ca | 96 | Matrix operator*(const Matrix& A, const Matrix& B) |
fbob | 0:431ee55036ca | 97 | { |
fbob | 0:431ee55036ca | 98 | // Auxiliary matrix where C = A*B |
fbob | 0:431ee55036ca | 99 | Matrix C(A._rows,B._cols); |
fbob | 1:085840a3d767 | 100 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 101 | for (int j = 1; j <= B._cols; j++) { |
fbob | 1:085840a3d767 | 102 | for (int k = 1; k <= A._cols; k++) { |
fbob | 1:085840a3d767 | 103 | C._data[i-1][j-1] += A._data[i-1][k-1]*B._data[k-1][j-1]; |
fbob | 1:085840a3d767 | 104 | } |
fbob | 0:431ee55036ca | 105 | } |
fbob | 0:431ee55036ca | 106 | } |
fbob | 0:431ee55036ca | 107 | return C; |
fbob | 0:431ee55036ca | 108 | } |
fbob | 0:431ee55036ca | 109 | |
fbob | 0:431ee55036ca | 110 | Matrix operator*(float scalar, const Matrix& A) |
fbob | 0:431ee55036ca | 111 | { |
fbob | 0:431ee55036ca | 112 | // Auxiliary matrix where C = scalar*A |
fbob | 0:431ee55036ca | 113 | Matrix C(A._rows,A._cols); |
fbob | 1:085840a3d767 | 114 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 115 | for (int j = 1; j <= A._cols; j++) { |
fbob | 1:085840a3d767 | 116 | C._data[i-1][j-1] += scalar*A._data[i-1][j-1]; |
fbob | 1:085840a3d767 | 117 | } |
fbob | 1:085840a3d767 | 118 | } |
fbob | 1:085840a3d767 | 119 | return C; |
fbob | 1:085840a3d767 | 120 | } |
fbob | 1:085840a3d767 | 121 | |
fbob | 1:085840a3d767 | 122 | Matrix operator*(const Matrix& A, float scalar) |
fbob | 1:085840a3d767 | 123 | { |
fbob | 1:085840a3d767 | 124 | // Auxiliary matrix where C = scalar*A |
fbob | 1:085840a3d767 | 125 | Matrix C(A._rows,A._cols); |
fbob | 1:085840a3d767 | 126 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 127 | for (int j = 1; j <= A._cols; j++) { |
fbob | 1:085840a3d767 | 128 | C._data[i-1][j-1] += A._data[i-1][j-1]*scalar; |
fbob | 1:085840a3d767 | 129 | } |
fbob | 1:085840a3d767 | 130 | } |
fbob | 1:085840a3d767 | 131 | return C; |
fbob | 1:085840a3d767 | 132 | } |
fbob | 1:085840a3d767 | 133 | |
fbob | 1:085840a3d767 | 134 | Matrix operator/(const Matrix& A, float scalar) |
fbob | 1:085840a3d767 | 135 | { |
fbob | 1:085840a3d767 | 136 | // Auxiliary matrix where C = scalar*A |
fbob | 1:085840a3d767 | 137 | Matrix C(A._rows,A._cols); |
fbob | 1:085840a3d767 | 138 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 139 | for (int j = 1; j <= A._cols; j++) { |
fbob | 1:085840a3d767 | 140 | C._data[i-1][j-1] += A._data[i-1][j-1]/scalar; |
fbob | 1:085840a3d767 | 141 | } |
fbob | 0:431ee55036ca | 142 | } |
fbob | 0:431ee55036ca | 143 | return C; |
fbob | 0:431ee55036ca | 144 | } |
fbob | 0:431ee55036ca | 145 | |
fbob | 0:431ee55036ca | 146 | Matrix transpose(const Matrix& A) |
fbob | 0:431ee55036ca | 147 | { |
fbob | 0:431ee55036ca | 148 | // Auxiliary matrix where C = A' |
fbob | 0:431ee55036ca | 149 | Matrix C(A._cols, A._rows); |
fbob | 1:085840a3d767 | 150 | for (int i = 1; i <= C._rows; i++) { |
fbob | 1:085840a3d767 | 151 | for (int j = 1; j <= C._cols; j++) { |
fbob | 0:431ee55036ca | 152 | C._data[i-1][j-1] = A._data[j-1][i-1]; |
fbob | 0:431ee55036ca | 153 | } |
fbob | 0:431ee55036ca | 154 | } |
fbob | 0:431ee55036ca | 155 | return C; |
fbob | 0:431ee55036ca | 156 | } |
fbob | 0:431ee55036ca | 157 | |
fbob | 0:431ee55036ca | 158 | Matrix inverse(const Matrix& A) |
fbob | 0:431ee55036ca | 159 | { |
fbob | 1:085840a3d767 | 160 | // Apply A = LDL' factorization where L is a lower triangular matrix and D |
fbob | 0:431ee55036ca | 161 | // is a block diagonal matrix |
fbob | 0:431ee55036ca | 162 | Matrix L(A._cols, A._rows); |
fbob | 0:431ee55036ca | 163 | Matrix D(A._cols, A._rows); |
fbob | 0:431ee55036ca | 164 | float L_sum; |
fbob | 0:431ee55036ca | 165 | float D_sum; |
fbob | 1:085840a3d767 | 166 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 167 | for (int j = 1; j <= A._rows; j++) { |
fbob | 1:085840a3d767 | 168 | if (i >= j) { |
fbob | 1:085840a3d767 | 169 | if (i == j) { |
fbob | 0:431ee55036ca | 170 | L._data[i-1][j-1] = 1.0; |
fbob | 0:431ee55036ca | 171 | D_sum = 0.0; |
fbob | 1:085840a3d767 | 172 | for (int k = 1; k <= (i-1); k++) { |
fbob | 0:431ee55036ca | 173 | D_sum += L._data[i-1][k-1]*L._data[i-1][k-1]*D._data[k-1][k-1]; |
fbob | 0:431ee55036ca | 174 | } |
fbob | 0:431ee55036ca | 175 | D._data[i-1][j-1] = A._data[i-1][j-1] - D_sum; |
fbob | 1:085840a3d767 | 176 | } else { |
fbob | 0:431ee55036ca | 177 | L_sum = 0.0; |
fbob | 1:085840a3d767 | 178 | for (int k = 1; k <= (j-1); k++) { |
fbob | 0:431ee55036ca | 179 | L_sum += L._data[i-1][k-1]*L._data[j-1][k-1]*D._data[k-1][k-1]; |
fbob | 0:431ee55036ca | 180 | } |
fbob | 0:431ee55036ca | 181 | 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 | 182 | } |
fbob | 0:431ee55036ca | 183 | } |
fbob | 0:431ee55036ca | 184 | } |
fbob | 0:431ee55036ca | 185 | } |
fbob | 1:085840a3d767 | 186 | // Compute the inverse of L and D matrices |
fbob | 0:431ee55036ca | 187 | Matrix L_inv(A._cols, A._rows); |
fbob | 0:431ee55036ca | 188 | Matrix D_inv(A._cols, A._rows); |
fbob | 0:431ee55036ca | 189 | float L_inv_sum; |
fbob | 1:085840a3d767 | 190 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 191 | for (int j = 1; j <= A._rows; j++) { |
fbob | 1:085840a3d767 | 192 | if (i >= j) { |
fbob | 1:085840a3d767 | 193 | if (i == j) { |
fbob | 0:431ee55036ca | 194 | L_inv._data[i-1][j-1] = 1.0/L._data[i-1][j-1]; |
fbob | 0:431ee55036ca | 195 | D_inv._data[i-1][j-1] = 1.0/D._data[i-1][j-1]; |
fbob | 1:085840a3d767 | 196 | } else { |
fbob | 0:431ee55036ca | 197 | L_inv_sum = 0.0; |
fbob | 1:085840a3d767 | 198 | for (int k = j; k <= (i-1); k++) { |
fbob | 0:431ee55036ca | 199 | L_inv_sum += L._data[i-1][k-1]*L_inv._data[k-1][j-1]; |
fbob | 0:431ee55036ca | 200 | } |
fbob | 0:431ee55036ca | 201 | L_inv._data[i-1][j-1] = -L_inv_sum; |
fbob | 0:431ee55036ca | 202 | } |
fbob | 0:431ee55036ca | 203 | } |
fbob | 0:431ee55036ca | 204 | } |
fbob | 0:431ee55036ca | 205 | } |
fbob | 1:085840a3d767 | 206 | // Compute the inverse of A matrix in terms of the inverse of L and D |
fbob | 1:085840a3d767 | 207 | // matrices |
fbob | 0:431ee55036ca | 208 | return transpose(L_inv)*D_inv*L_inv; |
fbob | 0:431ee55036ca | 209 | } |
fbob | 1:085840a3d767 | 210 | |
fbob | 1:085840a3d767 | 211 | float norm(const Matrix& A) |
fbob | 1:085840a3d767 | 212 | { |
fbob | 1:085840a3d767 | 213 | // Auxiliary matrix where C = A' |
fbob | 1:085840a3d767 | 214 | float n; |
fbob | 1:085840a3d767 | 215 | n = 0.0; |
fbob | 1:085840a3d767 | 216 | for (int i = 1; i <= A._rows; i++) { |
fbob | 1:085840a3d767 | 217 | n += A._data[i-1][0]*A._data[i-1][0]; |
fbob | 1:085840a3d767 | 218 | } |
fbob | 1:085840a3d767 | 219 | return sqrt(n); |
fbob | 1:085840a3d767 | 220 | } |