Atsumi Toda / Mbed OS UAV_Logger4

UAV_Logger1から改造。サーボ機構のプログラムを追加

Dependencies:   MPU6050_alter HMC5883L

Matrix/Matrix.cpp@3:3fa7882a5fd0, 2019-07-24 (annotated)

Committer:
Joeatsumi
Date:
Wed Jul 24 12:00:01 2019 +0000
Revision:
3:3fa7882a5fd0
Parent:
2:e6496a794bde 
AlteredUAV_Logger1;

Who changed what in which revision?

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