Fabio Bobrow / Cubli

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?

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 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 }