output only raw data (acceleration, anguler rate, geomagnetism, air pressure)
Dependencies: mbed SDFileSystem ConfigFile
Diff: Matrix/Matrix.cpp
- Revision:
- 23:79cdc1432160
- Parent:
- 3:5358a691a100
--- a/Matrix/Matrix.cpp Tue Jun 23 15:23:38 2015 +0000 +++ b/Matrix/Matrix.cpp Wed Jun 24 16:16:14 2015 +0000 @@ -1,4 +1,3 @@ -#include "mbed.h" #include "myConstants.h" #include "Matrix.h" @@ -6,7 +5,7 @@ Matrix::Matrix(int row, int col) : row(row), col(col), components(0) { components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); for(int i=0; i<row*col; i++) components[i] = 0.0f; if (row == col) { for (int i = 0; i < row; i++) { @@ -17,7 +16,7 @@ Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) { components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, comps, sizeof(float)*row*col); } @@ -28,7 +27,7 @@ Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) { components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, m.GetpComponents(), sizeof(float)*row*col); } @@ -48,14 +47,14 @@ col = m.col; delete[] components; components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, m.GetpComponents(), sizeof(float)*row*col); return *this; } Matrix& Matrix::operator+=(const Matrix& m) { - if (row != m.GetRow() || col != m.GetCol()) AbortWithMsg("Irregular Dimention"); + if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { @@ -69,7 +68,7 @@ } Matrix& Matrix::operator-=(const Matrix& m) { - if (row != m.GetRow() || col != m.GetCol()) AbortWithMsg("Irregular Dimention"); + if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { @@ -83,7 +82,7 @@ } /* Matrix& Matrix::operator*=(const Matrix& m) { - if (col != m.GetRow()) AbortWithMsg("Irregular Dimention"); + if (col != m.GetRow()) error("Irregular Dimention"); Matrix temp = Matrix(*this); col = m.GetCol(); @@ -116,7 +115,7 @@ } Matrix& Matrix::operator/=(float c) { - if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero"); + if (fabs(c) < NEARLY_ZERO) error("Division by Zero"); for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { components[i*col + j] /= c; @@ -127,7 +126,7 @@ } void Matrix::SetComp(int rowNo, int colNo, float val) { - if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error"); + if (rowNo > row || colNo > col) error("Index Out of Bounds Error"); components[(rowNo-1)*col + (colNo-1)] = val; } @@ -136,7 +135,7 @@ } float Matrix::Determinant() const{ - if (row != col) AbortWithMsg("failed to calculate det. : matrix is not square"); + if (row != col) error("failed to calculate det. : matrix is not square"); int decSign = 0; float retVal = 1.0f; @@ -151,7 +150,7 @@ } float Matrix::det() const { - if (row != col) AbortWithMsg("failed to calculate det : matrix is not square"); + if (row != col) error("failed to calculate det : matrix is not square"); Matrix temp(*this); int decSign = 1; @@ -180,7 +179,7 @@ // 列内、行内の最大要素を選んでも小さすぎる場合はエラー if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { - if (row != col) AbortWithMsg("failed to calculate det : Division by Zero"); + if (row != col) error("failed to calculate det : Division by Zero"); } } @@ -206,10 +205,10 @@ } Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{ - if (row != col) AbortWithMsg("failed to LU decomposition: matrix is not square"); + if (row != col) error("failed to LU decomposition: matrix is not square"); if (sign != 0) *sign = 1; if (p != 0) { - if (p->row != row || p->row != p->col) AbortWithMsg("failed to LU decomposition: permitation matrix is incorrect"); + if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect"); // 置換行列は最初に単位行列にしておく memset(p->components, 0, sizeof(float) * row * col); for (int i = 0; i < row; i++) { @@ -232,7 +231,7 @@ } } float c = retVal.components[d * col + d]; - if (fabs(c) < NEARLY_ZERO) AbortWithMsg("failed to LU decomposition: Division by Zero"); + if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero"); // d行d列目以降の行列について計算 for (int i = d+1; i < row; i++) { @@ -249,7 +248,7 @@ } float Matrix::Inverse(Matrix& invm) const{ - if (row != col) AbortWithMsg("failed to get Inv. : matrix is not square"); + if (row != col) error("failed to get Inv. : matrix is not square"); Matrix P(*this); Matrix LU(LU_Decompose(0, &P)); @@ -309,7 +308,7 @@ } Matrix Matrix::Transpose() const{ - //if (row != col) AbortWithMsg("failed to get Trans. : matrix is not square"); + //if (row != col) error("failed to get Trans. : matrix is not square"); Matrix retVal(col, row); for (int i = 0; i < row; i++) { @@ -334,7 +333,7 @@ } Matrix operator*(const Matrix& lhm, const Matrix& rhm) { - if(lhm.GetCol() != rhm.GetRow()) AbortWithMsg("Matrix product Error: Irregular Dimention."); + if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention."); int row = lhm.GetRow(); int col = rhm.GetCol(); int sum = lhm.GetCol(); @@ -367,7 +366,7 @@ } void Matrix::SwapRow(int rowNo1, int rowNo2) { - if (rowNo1 > row || rowNo2 > row) AbortWithMsg("Index Out of Bounds Error !!"); + if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!"); float* temp = new float[col]; memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col); @@ -378,7 +377,7 @@ } void Matrix::SwapCol(int colNo1, int colNo2) { - if (colNo1 > col || colNo2 > col) AbortWithMsg("Index Out of Bounds Error !!"); + if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!"); float temp = 0.0f; for (int i = 0; i < row; i++) {