Atsumi Toda / Mbed 2 deprecated Matrix_calculation_test

慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 （行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します）

Dependencies:   mbed

Matrix/Matrix.cpp@0:6a28eb668082, 2019-01-30 (annotated)

Committer:
Joeatsumi
Date:
Wed Jan 30 11:39:03 2019 +0000
Revision:
0:6a28eb668082
Direction cosine matrix and it's calculation.

Who changed what in which revision?

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