LAURUS / Mbed 2 deprecated Laurus_acc_gyro

read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

Dependencies:   mbed

Matrix.cpp@3:40559ebef0f1, 2015-05-13 (annotated)

Committer:
ojan
Date:
Wed May 13 04:02:27 2015 +0000
Revision:
3:40559ebef0f1
Parent:
2:4a6b46653abf 
estimate gravity vector & geomagnetic vector

Who changed what in which revision?

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