LAURUS / Mbed 2 deprecated LAURUS_program

output only raw data (acceleration, anguler rate, geomagnetism, air pressure)

Dependencies:   mbed SDFileSystem ConfigFile

Matrix/Matrix.cpp@3:5358a691a100, 2015-05-30 (annotated)

Committer:
ojan
Date:
Sat May 30 18:08:34 2015 +0000
Revision:
3:5358a691a100
Child:
23:79cdc1432160
estimate geomagnetism vector with Kalman Filter

Who changed what in which revision?

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