Atsumi Toda / Mbed 2 deprecated Auto_pilot_prototype_2

ドローン用計測制御基板の作り方vol.2で使用したピッチ制御プログラムです。

Dependencies:   mbed MPU6050_alter SDFileSystem

Matrix/Matrix.cpp@0:e647f6de3d26, 2020-03-06 (annotated)

Committer:
Joeatsumi
Date:
Fri Mar 06 15:03:57 2020 +0000
Revision:
0:e647f6de3d26
An software for autopilot(ver2)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:e647f6de3d26 1 #include "myConstants.h"
Joeatsumi 0:e647f6de3d26 2 #include "Matrix.h"
Joeatsumi 0:e647f6de3d26 3
Joeatsumi 0:e647f6de3d26 4
Joeatsumi 0:e647f6de3d26 5
Joeatsumi 0:e647f6de3d26 6 Matrix::Matrix(int row, int col) : row(row), col(col), components(0) {
Joeatsumi 0:e647f6de3d26 7 components = new float[row*col];
Joeatsumi 0:e647f6de3d26 8 if (!components) error("Memory Allocation Error");
Joeatsumi 0:e647f6de3d26 9 for(int i=0; i<row*col; i++) components[i] = 0.0f;
Joeatsumi 0:e647f6de3d26 10 if (row == col) {
Joeatsumi 0:e647f6de3d26 11 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 12 components[i * col + i] = 1.0f;
Joeatsumi 0:e647f6de3d26 13 }
Joeatsumi 0:e647f6de3d26 14 }
Joeatsumi 0:e647f6de3d26 15 }
Joeatsumi 0:e647f6de3d26 16
Joeatsumi 0:e647f6de3d26 17 Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) {
Joeatsumi 0:e647f6de3d26 18 components = new float[row*col];
Joeatsumi 0:e647f6de3d26 19 if (!components) error("Memory Allocation Error");
Joeatsumi 0:e647f6de3d26 20 memcpy(components, comps, sizeof(float)*row*col);
Joeatsumi 0:e647f6de3d26 21 }
Joeatsumi 0:e647f6de3d26 22
Joeatsumi 0:e647f6de3d26 23
Joeatsumi 0:e647f6de3d26 24 Matrix::~Matrix() {
Joeatsumi 0:e647f6de3d26 25 delete[] components;
Joeatsumi 0:e647f6de3d26 26 }
Joeatsumi 0:e647f6de3d26 27
Joeatsumi 0:e647f6de3d26 28 Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) {
Joeatsumi 0:e647f6de3d26 29 components = new float[row*col];
Joeatsumi 0:e647f6de3d26 30 if (!components) error("Memory Allocation Error");
Joeatsumi 0:e647f6de3d26 31 memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
Joeatsumi 0:e647f6de3d26 32 }
Joeatsumi 0:e647f6de3d26 33
Joeatsumi 0:e647f6de3d26 34 Matrix Matrix::operator-() const{
Joeatsumi 0:e647f6de3d26 35 Matrix retMat(*this);
Joeatsumi 0:e647f6de3d26 36
Joeatsumi 0:e647f6de3d26 37 for (int i = 0; i < row * col; i++) {
Joeatsumi 0:e647f6de3d26 38 retMat.components[i] = - this->components[i];
Joeatsumi 0:e647f6de3d26 39 }
Joeatsumi 0:e647f6de3d26 40
Joeatsumi 0:e647f6de3d26 41 return retMat;
Joeatsumi 0:e647f6de3d26 42 }
Joeatsumi 0:e647f6de3d26 43
Joeatsumi 0:e647f6de3d26 44 Matrix& Matrix::operator=(const Matrix& m) {
Joeatsumi 0:e647f6de3d26 45 if (this == &m) return *this;
Joeatsumi 0:e647f6de3d26 46 row = m.row;
Joeatsumi 0:e647f6de3d26 47 col = m.col;
Joeatsumi 0:e647f6de3d26 48 delete[] components;
Joeatsumi 0:e647f6de3d26 49 components = new float[row*col];
Joeatsumi 0:e647f6de3d26 50 if (!components) error("Memory Allocation Error");
Joeatsumi 0:e647f6de3d26 51 memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
Joeatsumi 0:e647f6de3d26 52
Joeatsumi 0:e647f6de3d26 53 return *this;
Joeatsumi 0:e647f6de3d26 54 }
Joeatsumi 0:e647f6de3d26 55
Joeatsumi 0:e647f6de3d26 56 Matrix& Matrix::operator+=(const Matrix& m) {
Joeatsumi 0:e647f6de3d26 57 if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
Joeatsumi 0:e647f6de3d26 58
Joeatsumi 0:e647f6de3d26 59 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 60 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 61 components[i * col + j] += m.components[i * col + j];
Joeatsumi 0:e647f6de3d26 62 }
Joeatsumi 0:e647f6de3d26 63 }
Joeatsumi 0:e647f6de3d26 64
Joeatsumi 0:e647f6de3d26 65 this->CleanUp();
Joeatsumi 0:e647f6de3d26 66
Joeatsumi 0:e647f6de3d26 67 return *this;
Joeatsumi 0:e647f6de3d26 68 }
Joeatsumi 0:e647f6de3d26 69
Joeatsumi 0:e647f6de3d26 70 Matrix& Matrix::operator-=(const Matrix& m) {
Joeatsumi 0:e647f6de3d26 71 if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
Joeatsumi 0:e647f6de3d26 72
Joeatsumi 0:e647f6de3d26 73 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 74 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 75 components[i * col + j] -= m.components[i * col + j];
Joeatsumi 0:e647f6de3d26 76 }
Joeatsumi 0:e647f6de3d26 77 }
Joeatsumi 0:e647f6de3d26 78
Joeatsumi 0:e647f6de3d26 79 this->CleanUp();
Joeatsumi 0:e647f6de3d26 80
Joeatsumi 0:e647f6de3d26 81 return *this;
Joeatsumi 0:e647f6de3d26 82 }
Joeatsumi 0:e647f6de3d26 83 /*
Joeatsumi 0:e647f6de3d26 84 Matrix& Matrix::operator*=(const Matrix& m) {
Joeatsumi 0:e647f6de3d26 85 if (col != m.GetRow()) error("Irregular Dimention");
Joeatsumi 0:e647f6de3d26 86 Matrix temp = Matrix(*this);
Joeatsumi 0:e647f6de3d26 87
Joeatsumi 0:e647f6de3d26 88 col = m.GetCol();
Joeatsumi 0:e647f6de3d26 89 delete[] components;
Joeatsumi 0:e647f6de3d26 90 components = new float[row*col];
Joeatsumi 0:e647f6de3d26 91
Joeatsumi 0:e647f6de3d26 92 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 93 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 94 components[i*col + j] = 0.0f;
Joeatsumi 0:e647f6de3d26 95 for (int k = 0; k < m.GetRow(); k++) {
Joeatsumi 0:e647f6de3d26 96 components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j];
Joeatsumi 0:e647f6de3d26 97 }
Joeatsumi 0:e647f6de3d26 98 }
Joeatsumi 0:e647f6de3d26 99 }
Joeatsumi 0:e647f6de3d26 100
Joeatsumi 0:e647f6de3d26 101 this->CleanUp();
Joeatsumi 0:e647f6de3d26 102
Joeatsumi 0:e647f6de3d26 103 return *this;
Joeatsumi 0:e647f6de3d26 104 }
Joeatsumi 0:e647f6de3d26 105 */
Joeatsumi 0:e647f6de3d26 106
Joeatsumi 0:e647f6de3d26 107 Matrix& Matrix::operator*=(float c) {
Joeatsumi 0:e647f6de3d26 108 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 109 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 110 components[i*col + j] *= c;
Joeatsumi 0:e647f6de3d26 111 }
Joeatsumi 0:e647f6de3d26 112 }
Joeatsumi 0:e647f6de3d26 113
Joeatsumi 0:e647f6de3d26 114 return *this;
Joeatsumi 0:e647f6de3d26 115 }
Joeatsumi 0:e647f6de3d26 116
Joeatsumi 0:e647f6de3d26 117 Matrix& Matrix::operator/=(float c) {
Joeatsumi 0:e647f6de3d26 118 if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
Joeatsumi 0:e647f6de3d26 119 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 120 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 121 components[i*col + j] /= c;
Joeatsumi 0:e647f6de3d26 122 }
Joeatsumi 0:e647f6de3d26 123 }
Joeatsumi 0:e647f6de3d26 124
Joeatsumi 0:e647f6de3d26 125 return *this;
Joeatsumi 0:e647f6de3d26 126 }
Joeatsumi 0:e647f6de3d26 127
Joeatsumi 0:e647f6de3d26 128 void Matrix::SetComp(int rowNo, int colNo, float val) {
Joeatsumi 0:e647f6de3d26 129 if (rowNo > row || colNo > col) error("Index Out of Bounds Error");
Joeatsumi 0:e647f6de3d26 130 components[(rowNo-1)*col + (colNo-1)] = val;
Joeatsumi 0:e647f6de3d26 131 }
Joeatsumi 0:e647f6de3d26 132
Joeatsumi 0:e647f6de3d26 133 void Matrix::SetComps(float* pComps) {
Joeatsumi 0:e647f6de3d26 134 memcpy(components, pComps, sizeof(float) * row * col);
Joeatsumi 0:e647f6de3d26 135 }
Joeatsumi 0:e647f6de3d26 136
Joeatsumi 0:e647f6de3d26 137 float Matrix::Determinant() const{
Joeatsumi 0:e647f6de3d26 138 if (row != col) error("failed to calculate det. : matrix is not square");
Joeatsumi 0:e647f6de3d26 139 int decSign = 0;
Joeatsumi 0:e647f6de3d26 140 float retVal = 1.0f;
Joeatsumi 0:e647f6de3d26 141
Joeatsumi 0:e647f6de3d26 142 // 行列のLU分解
Joeatsumi 0:e647f6de3d26 143 Matrix LU(this->LU_Decompose(&decSign));
Joeatsumi 0:e647f6de3d26 144
Joeatsumi 0:e647f6de3d26 145 for (int i = 0; i < LU.row; i++) {
Joeatsumi 0:e647f6de3d26 146 retVal *= LU.components[i * LU.col + i];
Joeatsumi 0:e647f6de3d26 147 }
Joeatsumi 0:e647f6de3d26 148
Joeatsumi 0:e647f6de3d26 149 return retVal*decSign;
Joeatsumi 0:e647f6de3d26 150 }
Joeatsumi 0:e647f6de3d26 151
Joeatsumi 0:e647f6de3d26 152 float Matrix::det() const {
Joeatsumi 0:e647f6de3d26 153 if (row != col) error("failed to calculate det : matrix is not square");
Joeatsumi 0:e647f6de3d26 154
Joeatsumi 0:e647f6de3d26 155 Matrix temp(*this);
Joeatsumi 0:e647f6de3d26 156 int decSign = 1;
Joeatsumi 0:e647f6de3d26 157
Joeatsumi 0:e647f6de3d26 158 for (int j = 0; j < col - 1; j++) {
Joeatsumi 0:e647f6de3d26 159
Joeatsumi 0:e647f6de3d26 160 // 列内のみで最大の要素を探す
Joeatsumi 0:e647f6de3d26 161 int maxNo = j;
Joeatsumi 0:e647f6de3d26 162 for (int k = j; k < row; k++) {
Joeatsumi 0:e647f6de3d26 163 if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k;
Joeatsumi 0:e647f6de3d26 164 }
Joeatsumi 0:e647f6de3d26 165 if (maxNo != j) {
Joeatsumi 0:e647f6de3d26 166 temp.SwapRow(j + 1, maxNo + 1);
Joeatsumi 0:e647f6de3d26 167 decSign *= -1;
Joeatsumi 0:e647f6de3d26 168 }
Joeatsumi 0:e647f6de3d26 169 // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す
Joeatsumi 0:e647f6de3d26 170 if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
Joeatsumi 0:e647f6de3d26 171 maxNo = j;
Joeatsumi 0:e647f6de3d26 172 for (int k = j; k < col; k++) {
Joeatsumi 0:e647f6de3d26 173 if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k;
Joeatsumi 0:e647f6de3d26 174 }
Joeatsumi 0:e647f6de3d26 175 if (maxNo != j) {
Joeatsumi 0:e647f6de3d26 176 temp.SwapCol(j + 1, maxNo + 1);
Joeatsumi 0:e647f6de3d26 177 decSign *= -1;
Joeatsumi 0:e647f6de3d26 178 }
Joeatsumi 0:e647f6de3d26 179
Joeatsumi 0:e647f6de3d26 180 // 列内、行内の最大要素を選んでも小さすぎる場合はエラー
Joeatsumi 0:e647f6de3d26 181 if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
Joeatsumi 0:e647f6de3d26 182 if (row != col) error("failed to calculate det : Division by Zero");
Joeatsumi 0:e647f6de3d26 183 }
Joeatsumi 0:e647f6de3d26 184 }
Joeatsumi 0:e647f6de3d26 185
Joeatsumi 0:e647f6de3d26 186 float c1 = 1.0f / temp.components[j * col + j];
Joeatsumi 0:e647f6de3d26 187
Joeatsumi 0:e647f6de3d26 188 for (int i = j + 1; i < row; i++) {
Joeatsumi 0:e647f6de3d26 189 float c2 = temp.components[i * col + j] * c1;
Joeatsumi 0:e647f6de3d26 190 for (int k = j; k < col; k++) {
Joeatsumi 0:e647f6de3d26 191 temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k];
Joeatsumi 0:e647f6de3d26 192 }
Joeatsumi 0:e647f6de3d26 193 }
Joeatsumi 0:e647f6de3d26 194
Joeatsumi 0:e647f6de3d26 195 }
Joeatsumi 0:e647f6de3d26 196
Joeatsumi 0:e647f6de3d26 197 if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f;
Joeatsumi 0:e647f6de3d26 198
Joeatsumi 0:e647f6de3d26 199 float retVal = 1.0f;
Joeatsumi 0:e647f6de3d26 200 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 201 retVal *= temp.components[i * col + i];
Joeatsumi 0:e647f6de3d26 202 }
Joeatsumi 0:e647f6de3d26 203
Joeatsumi 0:e647f6de3d26 204 return retVal * decSign;
Joeatsumi 0:e647f6de3d26 205 }
Joeatsumi 0:e647f6de3d26 206
Joeatsumi 0:e647f6de3d26 207 Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{
Joeatsumi 0:e647f6de3d26 208 if (row != col) error("failed to LU decomposition: matrix is not square");
Joeatsumi 0:e647f6de3d26 209 if (sign != 0) *sign = 1;
Joeatsumi 0:e647f6de3d26 210 if (p != 0) {
Joeatsumi 0:e647f6de3d26 211 if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect");
Joeatsumi 0:e647f6de3d26 212 // 置換行列は最初に単位行列にしておく
Joeatsumi 0:e647f6de3d26 213 memset(p->components, 0, sizeof(float) * row * col);
Joeatsumi 0:e647f6de3d26 214 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 215 p->components[i * col + i] = 1.0f;
Joeatsumi 0:e647f6de3d26 216 }
Joeatsumi 0:e647f6de3d26 217 }
Joeatsumi 0:e647f6de3d26 218 Matrix retVal(*this);
Joeatsumi 0:e647f6de3d26 219
Joeatsumi 0:e647f6de3d26 220 for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う
Joeatsumi 0:e647f6de3d26 221 // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する
Joeatsumi 0:e647f6de3d26 222 int maxNo = d;
Joeatsumi 0:e647f6de3d26 223 for (int i = d; i < row; i++) {
Joeatsumi 0:e647f6de3d26 224 if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i;
Joeatsumi 0:e647f6de3d26 225 }
Joeatsumi 0:e647f6de3d26 226 if (maxNo != d) {
Joeatsumi 0:e647f6de3d26 227 retVal.SwapRow(d + 1, maxNo + 1);
Joeatsumi 0:e647f6de3d26 228 if (sign != 0) *sign *= -1;
Joeatsumi 0:e647f6de3d26 229 if (p != 0) {
Joeatsumi 0:e647f6de3d26 230 p->SwapRow(d + 1, maxNo + 1);
Joeatsumi 0:e647f6de3d26 231 }
Joeatsumi 0:e647f6de3d26 232 }
Joeatsumi 0:e647f6de3d26 233 float c = retVal.components[d * col + d];
Joeatsumi 0:e647f6de3d26 234 if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero");
Joeatsumi 0:e647f6de3d26 235
Joeatsumi 0:e647f6de3d26 236 // d行d列目以降の行列について計算
Joeatsumi 0:e647f6de3d26 237 for (int i = d+1; i < row; i++) {
Joeatsumi 0:e647f6de3d26 238 retVal.components[i * col + d] /= c;
Joeatsumi 0:e647f6de3d26 239 for (int j = d+1; j < col; j++) {
Joeatsumi 0:e647f6de3d26 240 retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d];
Joeatsumi 0:e647f6de3d26 241 }
Joeatsumi 0:e647f6de3d26 242 }
Joeatsumi 0:e647f6de3d26 243 }
Joeatsumi 0:e647f6de3d26 244
Joeatsumi 0:e647f6de3d26 245 retVal.CleanUp();
Joeatsumi 0:e647f6de3d26 246
Joeatsumi 0:e647f6de3d26 247 return retVal;
Joeatsumi 0:e647f6de3d26 248 }
Joeatsumi 0:e647f6de3d26 249
Joeatsumi 0:e647f6de3d26 250 float Matrix::Inverse(Matrix& invm) const{
Joeatsumi 0:e647f6de3d26 251 if (row != col) error("failed to get Inv. : matrix is not square");
Joeatsumi 0:e647f6de3d26 252
Joeatsumi 0:e647f6de3d26 253 Matrix P(*this);
Joeatsumi 0:e647f6de3d26 254 Matrix LU(LU_Decompose(0, &P));
Joeatsumi 0:e647f6de3d26 255
Joeatsumi 0:e647f6de3d26 256 // 分解した行列の対角成分の積から行列式を求める
Joeatsumi 0:e647f6de3d26 257 // det = 0　ならfalse
Joeatsumi 0:e647f6de3d26 258 float det = 1.0f;
Joeatsumi 0:e647f6de3d26 259 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 260 det *= LU.components[i * col + i];
Joeatsumi 0:e647f6de3d26 261 }
Joeatsumi 0:e647f6de3d26 262 if (fabs(det) < NEARLY_ZERO) {
Joeatsumi 0:e647f6de3d26 263 return fabs(det);
Joeatsumi 0:e647f6de3d26 264 }
Joeatsumi 0:e647f6de3d26 265
Joeatsumi 0:e647f6de3d26 266 // U、Lそれぞれの逆行列を計算する
Joeatsumi 0:e647f6de3d26 267 Matrix U_inv = Matrix(row, col);
Joeatsumi 0:e647f6de3d26 268 Matrix L_inv = Matrix(row, col);
Joeatsumi 0:e647f6de3d26 269
Joeatsumi 0:e647f6de3d26 270 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 271 for (int i = 0; i <= j; i++) {
Joeatsumi 0:e647f6de3d26 272 int i_U = j - i; // U行列の逆行列は対角成分から上へ向かって
Joeatsumi 0:e647f6de3d26 273 // 左から順番に値を計算する
Joeatsumi 0:e647f6de3d26 274
Joeatsumi 0:e647f6de3d26 275 int j_L = col - 1 - j; // L行列の逆行列は右から順番に
Joeatsumi 0:e647f6de3d26 276 int i_L = j_L + i; // 対角成分から下へ向かって計算する
Joeatsumi 0:e647f6de3d26 277
Joeatsumi 0:e647f6de3d26 278 if (i_U != j) { // 非対角成分
Joeatsumi 0:e647f6de3d26 279 float temp_U = 0.0f;
Joeatsumi 0:e647f6de3d26 280 float temp_L = 0.0f;
Joeatsumi 0:e647f6de3d26 281
Joeatsumi 0:e647f6de3d26 282 for (int k = 0; k < i; k++) {
Joeatsumi 0:e647f6de3d26 283
Joeatsumi 0:e647f6de3d26 284 temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)];
Joeatsumi 0:e647f6de3d26 285
Joeatsumi 0:e647f6de3d26 286 if (k == 0) {
Joeatsumi 0:e647f6de3d26 287 temp_L -= LU.components[i_L * col + j_L];
Joeatsumi 0:e647f6de3d26 288 } else {
Joeatsumi 0:e647f6de3d26 289 temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k];
Joeatsumi 0:e647f6de3d26 290 }
Joeatsumi 0:e647f6de3d26 291
Joeatsumi 0:e647f6de3d26 292 }
Joeatsumi 0:e647f6de3d26 293
Joeatsumi 0:e647f6de3d26 294 U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U];
Joeatsumi 0:e647f6de3d26 295 L_inv.components[i_L * col + j_L] = temp_L;
Joeatsumi 0:e647f6de3d26 296
Joeatsumi 0:e647f6de3d26 297 } else { // 対角成分
Joeatsumi 0:e647f6de3d26 298 if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) {
Joeatsumi 0:e647f6de3d26 299 U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U];
Joeatsumi 0:e647f6de3d26 300 }
Joeatsumi 0:e647f6de3d26 301 }
Joeatsumi 0:e647f6de3d26 302 }
Joeatsumi 0:e647f6de3d26 303 }
Joeatsumi 0:e647f6de3d26 304
Joeatsumi 0:e647f6de3d26 305 invm = U_inv * L_inv * P;
Joeatsumi 0:e647f6de3d26 306
Joeatsumi 0:e647f6de3d26 307 return -1.0f;
Joeatsumi 0:e647f6de3d26 308 }
Joeatsumi 0:e647f6de3d26 309
Joeatsumi 0:e647f6de3d26 310 Matrix Matrix::Transpose() const{
Joeatsumi 0:e647f6de3d26 311 //if (row != col) error("failed to get Trans. : matrix is not square");
Joeatsumi 0:e647f6de3d26 312 Matrix retVal(col, row);
Joeatsumi 0:e647f6de3d26 313
Joeatsumi 0:e647f6de3d26 314 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 315 for (int j = 0; j < col; j++) {
Joeatsumi 0:e647f6de3d26 316 retVal.components[j * row + i] = this->components[i * col + j];
Joeatsumi 0:e647f6de3d26 317 }
Joeatsumi 0:e647f6de3d26 318 }
Joeatsumi 0:e647f6de3d26 319
Joeatsumi 0:e647f6de3d26 320 return retVal;
Joeatsumi 0:e647f6de3d26 321 }
Joeatsumi 0:e647f6de3d26 322
Joeatsumi 0:e647f6de3d26 323 Matrix operator+(const Matrix& lhm, const Matrix& rhm) {
Joeatsumi 0:e647f6de3d26 324 Matrix temp = Matrix(lhm);
Joeatsumi 0:e647f6de3d26 325 temp += rhm;
Joeatsumi 0:e647f6de3d26 326 return temp;
Joeatsumi 0:e647f6de3d26 327 }
Joeatsumi 0:e647f6de3d26 328
Joeatsumi 0:e647f6de3d26 329 Matrix operator-(const Matrix& lhm, const Matrix& rhm) {
Joeatsumi 0:e647f6de3d26 330 Matrix temp = Matrix(lhm);
Joeatsumi 0:e647f6de3d26 331 temp -= rhm;
Joeatsumi 0:e647f6de3d26 332 return temp;
Joeatsumi 0:e647f6de3d26 333 }
Joeatsumi 0:e647f6de3d26 334
Joeatsumi 0:e647f6de3d26 335 Matrix operator*(const Matrix& lhm, const Matrix& rhm) {
Joeatsumi 0:e647f6de3d26 336 if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention.");
Joeatsumi 0:e647f6de3d26 337 int row = lhm.GetRow();
Joeatsumi 0:e647f6de3d26 338 int col = rhm.GetCol();
Joeatsumi 0:e647f6de3d26 339 int sum = lhm.GetCol();
Joeatsumi 0:e647f6de3d26 340 Matrix temp(row, col);
Joeatsumi 0:e647f6de3d26 341
Joeatsumi 0:e647f6de3d26 342 for (int i = 1; i <= row; i++) {
Joeatsumi 0:e647f6de3d26 343 for (int j = 1; j <= col; j++) {
Joeatsumi 0:e647f6de3d26 344 float temp_c = 0.0f;
Joeatsumi 0:e647f6de3d26 345 for (int k = 1; k <= sum; k++) {
Joeatsumi 0:e647f6de3d26 346 temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j);
Joeatsumi 0:e647f6de3d26 347 }
Joeatsumi 0:e647f6de3d26 348 temp.SetComp(i, j, temp_c);
Joeatsumi 0:e647f6de3d26 349 }
Joeatsumi 0:e647f6de3d26 350 }
Joeatsumi 0:e647f6de3d26 351
Joeatsumi 0:e647f6de3d26 352 return temp;
Joeatsumi 0:e647f6de3d26 353 }
Joeatsumi 0:e647f6de3d26 354
Joeatsumi 0:e647f6de3d26 355 void Matrix::CleanUp() {
Joeatsumi 0:e647f6de3d26 356 int num = row*col;
Joeatsumi 0:e647f6de3d26 357 float maxComp = 0.0f;
Joeatsumi 0:e647f6de3d26 358 for (int i = 0; i < num; i++) {
Joeatsumi 0:e647f6de3d26 359 if (maxComp < fabs(components[i])) maxComp = fabs(components[i]);
Joeatsumi 0:e647f6de3d26 360 }
Joeatsumi 0:e647f6de3d26 361 if (maxComp > NEARLY_ZERO) {
Joeatsumi 0:e647f6de3d26 362 for (int i = 0; i < num; i++) {
Joeatsumi 0:e647f6de3d26 363 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
Joeatsumi 0:e647f6de3d26 364 }
Joeatsumi 0:e647f6de3d26 365 }
Joeatsumi 0:e647f6de3d26 366 }
Joeatsumi 0:e647f6de3d26 367
Joeatsumi 0:e647f6de3d26 368 void Matrix::SwapRow(int rowNo1, int rowNo2) {
Joeatsumi 0:e647f6de3d26 369 if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!");
Joeatsumi 0:e647f6de3d26 370 float* temp = new float[col];
Joeatsumi 0:e647f6de3d26 371
Joeatsumi 0:e647f6de3d26 372 memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col);
Joeatsumi 0:e647f6de3d26 373 memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col);
Joeatsumi 0:e647f6de3d26 374 memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col);
Joeatsumi 0:e647f6de3d26 375
Joeatsumi 0:e647f6de3d26 376 delete[] temp;
Joeatsumi 0:e647f6de3d26 377 }
Joeatsumi 0:e647f6de3d26 378
Joeatsumi 0:e647f6de3d26 379 void Matrix::SwapCol(int colNo1, int colNo2) {
Joeatsumi 0:e647f6de3d26 380 if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!");
Joeatsumi 0:e647f6de3d26 381 float temp = 0.0f;
Joeatsumi 0:e647f6de3d26 382
Joeatsumi 0:e647f6de3d26 383 for (int i = 0; i < row; i++) {
Joeatsumi 0:e647f6de3d26 384 temp = components[i * col + colNo1];
Joeatsumi 0:e647f6de3d26 385 components[i * col + colNo1] = components[i * col + colNo2];
Joeatsumi 0:e647f6de3d26 386 components[i * col + colNo2] = temp;
Joeatsumi 0:e647f6de3d26 387 }
Joeatsumi 0:e647f6de3d26 388 }