Advanced Robot Control System  5.1-REV.51SF170515
Matrix.hh
[詳解]
1 // 行列/ベクトル計算クラス
2 // 2016/08/19 Yuki YOKOKURA
3 //
4 // 行列/ベクトルの様々な計算を行うクラス
5 // (機能の充実性を求めるならば,このクラスの代わりに「Eigen」の導入をオススメしておく)
6 //
7 // Copyright (C) 2011-2016 Yuki YOKOKURA
8 // This program is free software;
9 // you can redistribute it and/or modify it under the terms of the GNU General Public License
10 // as published by the Free Software Foundation; either version 3 of the License, or any later version.
11 // This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
12 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 // See the GNU General Public License for more details <http://www.gnu.org/licenses/>.
14 // In addition, you can negotiate about other options of licenses instead of GPL.
15 // If you would like to get other licenses, please contact us <yokokura(a)vos.nagaokaut.ac.jp>.
16 //
17 // 以下,コメント。
18 // ・各々の関数における計算結果はMATLABと比較して合っていることを確認している。
19 // ・ただし,LU分解などの答えが一意に求まらない関数では,当然,MATLABとは異なる結果を出力する。
20 // ・古代的なC/C++の記述法と最新の現代的なC++の記述法が混在しているが,そこはご愛嬌ということで。
21 //
22 // 今後の予定
23 // ・生配列をarrayに書き換える
24 // ・double型直書きではなくて,テンプレートに書き換える
25 // ・QR分解,SVD,rank,固有値,固有ベクトルを返す関数を実装する
26 // ・複素数に対応させる
27 
28 #ifndef MATRIX
29 #define MATRIX
30 
31 #include <string>
32 #include <cassert>
33 
34 #define PrintMatSize(a) (_PrintMatSize(a,#a)) // 行列サイズ表示マクロ
35 #define PrintMatrix(a,b) (_PrintMatrix(a,b,#a)) // 行列要素表示マクロ
36 
37 namespace ARCS { // ARCS名前空間
38  class Matrix {
39  public:
40  // LU分解の際の並べ替えが偶数回/奇数回発生したことを返すための定義
41  enum LUperm {
42  ODD, // 奇数
43  EVEN // 偶数
44  };
45  Matrix(unsigned int n, unsigned int m); // コンストラクタ
46  ~Matrix(); // デストラクタ
47  Matrix(const Matrix& right); // コピーコンストラクタ
48  Matrix& operator=(const Matrix& right); // 行列代入演算子
49  Matrix operator+(void) const; // 単項プラス演算子
50  Matrix operator-(void) const; // 単項マイナス演算子
51  Matrix operator+(const Matrix& right) const;// 行列加算演算子 (行列同士の加算の場合)
52  Matrix operator+(const double& right) const;// 行列加算演算子 (行列+スカラーの場合)
53  Matrix operator-(const Matrix& right) const;// 行列減算演算子 (行列同士の減算の場合)
54  Matrix operator-(const double& right) const;// 行列減算演算子 (行列-スカラーの場合)
55  Matrix operator*(const Matrix& right) const;// 行列乗算演算子 (行列同士の乗算の場合)
56  Matrix operator*(const double& right) const;// 行列乗算演算子 (行列*スカラーの場合)
57  Matrix& operator+=(const Matrix& right); // 行列加算代入演算子 (行列=行列+行列の場合)
58  Matrix& operator+=(const double& right); // 行列加算代入演算子 (行列=行列+スカラーの場合)
59  Matrix& operator-=(const Matrix& right); // 行列減算代入演算子 (行列=行列-行列の場合)
60  Matrix& operator-=(const double& right); // 行列減算代入演算子 (行列=行列-スカラーの場合)
61  Matrix operator^(const unsigned int& right);// 行列べき乗演算子
62  void SetElem(unsigned int m, unsigned int n, double val); // 指定した要素番号に値を設定する関数 m行目,n列目にvalを代入
63  double GetElem(unsigned int m, unsigned int n) const; // m行n列目の値を返す関数
64  void FillAll(double u); // すべての要素を指定した値で埋める関数
65  unsigned int GetWidthLength(void) const; // 行列の幅(列数)を返す関数
66  unsigned int GetHeightLength(void) const; // 行列の高さ(行数)を返す関数
67  // 以下はフレンド関数
68  friend Matrix operator+(const double& left, const Matrix& right); // 行列加算演算子 (スカラー+行列の場合)
69  friend Matrix operator-(const double& left, const Matrix& right); // 行列減算演算子 (スカラー-行列の場合)
70  friend Matrix operator*(const double& left, const Matrix& right); // 行列乗算演算子 (スカラー*行列の場合)
71  friend void _PrintMatSize(const Matrix& u, const std::string& varname);
72  // 行列のサイズの表示 u : 表示する行列 この関数はマクロを介して呼ばれることを想定している。
73  friend void _PrintMatrix(const Matrix& u, const std::string& format, const std::string& varname);
74  // 行列の要素を表示 format : 表示形式 (%1.3e とか %5.3f とか printfと同じ)
75  // u : 表示する行列 この関数はマクロを介して呼ばれることを想定している。
76  friend Matrix tp(const Matrix& u); // 転置行列を返す関数
77  friend double tr(const Matrix& u); // 行列のトレースを返す関数
78  friend double prod(const Matrix& u); // 行列の対角要素の総積を返す関数
79  friend Matrix diag(const Matrix& u); // 行列の対角要素を返す関数
80  friend double det(const Matrix& u); // 行列式の値を返す関数
81  friend Matrix inv(const Matrix& A); // 逆行列を返す関数 (LU分解とAx=bの解から求めるので高速なはず)
82  friend Matrix lpinv(const Matrix& A); // 左擬似逆行列を返す関数 (Aが縦長行列の場合)
83  friend Matrix rpinv(const Matrix& A); // 右擬似逆行列を返す関数 (Aが横長行列の場合)
84  friend Matrix abs(const Matrix& U); // 行列要素の絶対値を返す関数
85  friend Matrix sumrow(const Matrix& U); // 行方向へ加算して列ベクトルを出力する関数
86  friend Matrix sumcolumn(const Matrix& U); // 列方向へ加算して行ベクトルを出力する関数
87  friend double max(const Matrix& u); // ベクトル要素の最大値を返す関数
88  friend double absmax(const Matrix& u); // ベクトル要素の絶対値の最大値を返す関数
89  friend unsigned int maxidx(const Matrix& u);// ベクトル要素の最大値の要素番号を返す関数
90  friend unsigned int absmaxidx(const Matrix& u);// ベクトル要素の絶対値の最大値の要素番号を返す関数
91  friend double infnorm(const Matrix& U); // 行列の絶対値ノルムを返す関数
92  friend Matrix getrow(const Matrix& u, unsigned int m); // 指定した行を抽出する関数
93  friend void setrow(Matrix& u, const Matrix& v, unsigned int m); // 指定した行を行ベクトルで上書きする関数
94  friend void swaprow(Matrix& U, unsigned int m1, unsigned int m2); // 指定した行と行を入れ替える関数
95  friend void fillrow(Matrix& U, double a, unsigned int m, unsigned int n1, unsigned int n2);
96  // m行目のn1列目からm2列目を数値aで埋める関数 (n1 <= n2 であること)
97  friend Matrix getcolumn(const Matrix& u, unsigned int n); // 指定した列を抽出する関数
98  friend void setcolumn(Matrix& u, const Matrix& v, unsigned int n); // 指定した列を列ベクトルで上書きする関数
99  friend void swapcolumn(Matrix& U, unsigned int n1, unsigned int n2);// 指定した列と列を入れ替える関数
100  friend void fillcolumn(Matrix& U, double a, unsigned int n, unsigned int m1, unsigned int m2);
101  // n列目のm1行目からm2行目を数値aで埋める関数 (m1 <= m2 であること)
102  friend Matrix orderrow(const Matrix& U, const Matrix& v); // 並び替え記憶列ベクトルvに従って,入力行列Uの行を並び替える関数
103  friend Matrix reorderrow(const Matrix& U, const Matrix& v); // 並び替え記憶列ベクトルvが昇順になるように,入力行列Uの行を並び替えて元に戻す関数
104  friend Matrix solve(const Matrix& A, const Matrix& b); // Ax = b の形の線形連立1次方程式をxについて解く関数
105  friend Matrix expm(const Matrix& A, unsigned int M); // 行列指数関数 e^(A) A : 入力行列,M : パデ近似の次数
106  friend Matrix integral_expm(const Matrix& U, const double T, const unsigned long N, const unsigned int P);
107  // 指数行列の数値定積分[0,T)をする関数 U:入力行列,T:積分範囲の終わり,N:分割数,P:パデ近似の次数
108  friend Matrix::LUperm LU(const Matrix& A, Matrix& L, Matrix& U, Matrix& v);
109  // LU分解 A:入力行列,L:下三角行列,U:上三角行列,v:並べ替え記憶列ベクトル
110  // 並べ替えが偶数回/奇数回発生したことを返す。返す値は Matrix::ODD か Matrix::EVEN のどちらか
111 
112  // 行列要素に値を設定する関数 (非スレッドセーフなので注意)
113  // (テンプレート関数は宣言と実体を分離しないのが一般的だが不満だ。なんとかしてほしい。)
114  template<typename T1, typename... T2> // 可変長引数テンプレート
115  void Set(const T1& u1, const T2&... u2){ // 再帰で順番に可変長引数を読み込んでいく
116  assert(Nindex < N); // 行カウンタが行の長さ以内かチェック
117  assert(Mindex < M); // 列カウンタが列の長さ以内かチェック
118  Data[Nindex][Mindex] = (double)u1; // 行列の要素を埋める
119  Nindex++; // 行カウンタをインクリメント
120  if(Nindex == N){ // 行カウンタが最後まで行き着いたら,
121  Nindex = 0; // 行カウンタを零に戻して,
122  Mindex++; // その代わりに,列カウンタをインクリメント
123  }
124  Set(u2...); // 自分自身を呼び出す(再帰)
125  }
126  void Set(){
127  // 再帰の最後に呼ばれる関数
128  Nindex = 0; // すべての作業が終わったので,
129  Mindex = 0; // 行,列カウンタを零に戻しておく
130  }
131 
132  private:
133  static constexpr double epsilon = 1e-12; // 零とみなす閾値
134  unsigned int N; // 行列の幅(列の数)
135  unsigned int M; // 行列の高さ(行の数)
136  double** Data; // データ格納用変数 配列要素の順番は Data[N列][M行] なので注意
137  unsigned int Nindex; // 横用カウンタ
138  unsigned int Mindex; // 縦用カウンタ
139  };
140 
141  // 以下は非メンバ関数
142  Matrix zeros(unsigned int n, unsigned int m); // m行n列の零行列を返す関数
143  Matrix ones(unsigned int n, unsigned int m); // m行n列の要素がすべて1の行列を返す関数
144  Matrix ident(unsigned int n); // n行n列の単位行列を返す関数
145  Matrix eye(unsigned int n); // n行n列の単位行列を返す関数
146 }
147 
148 
149 
150 #endif
151 
friend double det(const Matrix &u)
Definition: Matrix.cc:319
Definition: Matrix.hh:38
Matrix ones(unsigned int n, unsigned int m)
Definition: Matrix.cc:708
A
Definition: GraphPlotter.m:12
unsigned int N
Definition: Matrix.hh:134
friend Matrix solve(const Matrix &A, const Matrix &b)
Definition: Matrix.cc:551
friend Matrix::LUperm LU(const Matrix &A, Matrix &L, Matrix &U, Matrix &v)
Definition: Matrix.cc:639
friend Matrix sumcolumn(const Matrix &U)
Definition: Matrix.cc:378
Matrix operator^(const unsigned int &right)
Definition: Matrix.cc:196
friend Matrix integral_expm(const Matrix &U, const double T, const unsigned long N, const unsigned int P)
Definition: Matrix.cc:621
Matrix & operator+=(const Matrix &right)
Definition: Matrix.cc:160
Definition: Matrix.hh:42
friend void swapcolumn(Matrix &U, unsigned int n1, unsigned int n2)
Definition: Matrix.cc:510
friend double max(const Matrix &u)
Definition: Matrix.cc:387
#define assert(a)
ARCS用assertマクロ a : assert条件
Definition: ARCSassert.hh:17
friend Matrix tp(const Matrix &u)
Definition: Matrix.cc:286
friend unsigned int absmaxidx(const Matrix &u)
Definition: Matrix.cc:438
static constexpr double epsilon
Definition: Matrix.hh:133
double ** Data
Definition: Matrix.hh:136
Matrix zeros(unsigned int n, unsigned int m)
Definition: Matrix.cc:702
unsigned int M
Definition: Matrix.hh:135
friend void setrow(Matrix &u, const Matrix &v, unsigned int m)
Definition: Matrix.cc:466
friend Matrix inv(const Matrix &A)
Definition: Matrix.cc:334
friend double tr(const Matrix &u)
Definition: Matrix.cc:295
friend Matrix sumrow(const Matrix &U)
Definition: Matrix.cc:369
void Set()
Definition: Matrix.hh:126
Definition: Matrix.hh:43
Matrix operator*(const Matrix &right) const
Definition: Matrix.cc:139
friend double infnorm(const Matrix &U)
Definition: Matrix.cc:453
friend void setcolumn(Matrix &u, const Matrix &v, unsigned int n)
Definition: Matrix.cc:502
Definition: ControlFunctions.hh:17
friend Matrix reorderrow(const Matrix &U, const Matrix &v)
Definition: Matrix.cc:540
friend Matrix getcolumn(const Matrix &u, unsigned int n)
Definition: Matrix.cc:494
unsigned int GetHeightLength(void) const
Definition: Matrix.cc:231
friend unsigned int maxidx(const Matrix &u)
Definition: Matrix.cc:423
Matrix & operator-=(const Matrix &right)
Definition: Matrix.cc:178
friend Matrix diag(const Matrix &u)
Definition: Matrix.cc:311
unsigned int GetWidthLength(void) const
Definition: Matrix.cc:226
friend Matrix abs(const Matrix &U)
Definition: Matrix.cc:360
double GetElem(unsigned int m, unsigned int n) const
Definition: Matrix.cc:213
Matrix operator+(void) const
Definition: Matrix.cc:80
Matrix eye(unsigned int n)
Definition: Matrix.cc:723
unsigned int Nindex
Definition: Matrix.hh:137
void Set(const T1 &u1, const T2 &... u2)
Definition: Matrix.hh:115
friend void _PrintMatrix(const Matrix &u, const std::string &format, const std::string &varname)
Definition: Matrix.cc:271
unsigned int Mindex
Definition: Matrix.hh:138
void SetElem(unsigned int m, unsigned int n, double val)
Definition: Matrix.cc:207
Matrix ident(unsigned int n)
Definition: Matrix.cc:715
friend Matrix expm(const Matrix &A, unsigned int M)
Definition: Matrix.cc:582
LUperm
Definition: Matrix.hh:41
friend void fillrow(Matrix &U, double a, unsigned int m, unsigned int n1, unsigned int n2)
Definition: Matrix.cc:485
friend void swaprow(Matrix &U, unsigned int m1, unsigned int m2)
Definition: Matrix.cc:474
Matrix(unsigned int n, unsigned int m)
Definition: Matrix.cc:35
friend Matrix lpinv(const Matrix &A)
Definition: Matrix.cc:348
friend double prod(const Matrix &u)
Definition: Matrix.cc:303
~Matrix()
Definition: Matrix.cc:47
friend double absmax(const Matrix &u)
Definition: Matrix.cc:405
friend Matrix getrow(const Matrix &u, unsigned int m)
Definition: Matrix.cc:458
friend void fillcolumn(Matrix &U, double a, unsigned int n, unsigned int m1, unsigned int m2)
Definition: Matrix.cc:521
Matrix operator-(void) const
Definition: Matrix.cc:89
friend Matrix rpinv(const Matrix &A)
Definition: Matrix.cc:354
friend void _PrintMatSize(const Matrix &u, const std::string &varname)
Definition: Matrix.cc:265
Matrix & operator=(const Matrix &right)
Definition: Matrix.cc:70
friend Matrix orderrow(const Matrix &U, const Matrix &v)
Definition: Matrix.cc:530
void FillAll(double u)
Definition: Matrix.cc:219