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

Dependencies:   mbed

Matrix.h

Committer:
ojan
Date:
2015-04-16
Revision:
1:2eca9b376580
Child:
2:4a6b46653abf

File content as of revision 1:2eca9b376580:

#pragma once
#include "ErrorLogger.h"

class Matrix
{
public:
    /********** コンストラクタ デストラクタ **********/
    Matrix(int row, int col);
    Matrix(int row, int col, float* comps);
    ~Matrix();
    Matrix(const Matrix& m);

    /********** メンバ演算子 **********/
    Matrix& operator=(const Matrix& m);
    Matrix& operator+=(const Matrix& m);
    Matrix& operator-=(const Matrix& m);
    Matrix& operator*=(const Matrix& m);
    Matrix& operator*=(float c);
    Matrix& operator/=(float c);

    /********** その他関数 **********/
    /*
        行列の成分を設定
        引数:rowNo 行番号
              colNo 列番号
              val 設定値
    */
    void SetComp(int rowNo, int colNo, float val);

    /*
        行列の成分を全て設定。全成分を一度に指定する必要がある。
        引数:pComps 設定値の入ったfloat配列。
    */
    void SetComps(float* pComps);
    /*
        行列式を計算する。行列が正方行列で無い場合にはエラー。
    */
    float Determinant() const;

    /*
        行列をLU分解する
        引数:sign (省略可)置換操作の符号を格納するポインタ
             p (省略可)置換行列を格納する行列のポインタ。(分解する行列と同じ列数の正方行列)
        返り値:LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
    */
    Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
    
    /*
        逆行列を生成する
        返り値で逆行列が存在するか否かを判断
        引数:逆行列を格納する行列
        返り値:逆行列が存在するか否か
    */
    bool Inverse(Matrix& invm) const;
    
    /*
        転置行列を生成する
        返り値:転置行列
    */
    Matrix Transpose() const;
    
    /*
        行列の行の入れ替えを行う
        引数:rowNo1 行番号1
             rowNo2 行番号2
    */
    void SwapRow(int rowNo1, int rowNo2);

    /********** インライン関数 **********/
    inline int GetRow() const {
        return row;
    }

    inline int GetCol() const {
        return col;
    }

    inline const float* GetpComponents() const {
        return (const float*)components;
    }

    inline float GetComp(int rowNo, int colNo) const {
        if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error !!");
        return components[(rowNo-1)*col + (colNo-1)];
    }

private:
    int row;
    int col;
    float* components;

    /*
        行列の成分の中で無視できるほど小さい値を0と置き換える(掃除する)
    */
    void CleanUp();

};

// グローバル演算子
Matrix operator+(const Matrix& lhm, const Matrix& rhm);
Matrix operator-(const Matrix& lhm, const Matrix& rhm);
Matrix operator*(const Matrix& lhm, const Matrix& rhm);