odometry
Dependencies: arrc_mbed BNO055
matrix.h@0:dea3c70f4911, 2021-06-21 (annotated)
- Committer:
- robokenken
- Date:
- Mon Jun 21 07:35:27 2021 +0000
- Revision:
- 0:dea3c70f4911
omuni wheel odometry
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
robokenken | 0:dea3c70f4911 | 1 | /** @file |
robokenken | 0:dea3c70f4911 | 2 | * |
robokenken | 0:dea3c70f4911 | 3 | * My program. |
robokenken | 0:dea3c70f4911 | 4 | */ |
robokenken | 0:dea3c70f4911 | 5 | |
robokenken | 0:dea3c70f4911 | 6 | #ifndef MATRIX2_LIB |
robokenken | 0:dea3c70f4911 | 7 | #define MATRIX2_LIB |
robokenken | 0:dea3c70f4911 | 8 | |
robokenken | 0:dea3c70f4911 | 9 | #include<vector> |
robokenken | 0:dea3c70f4911 | 10 | #include<stdarg.h> |
robokenken | 0:dea3c70f4911 | 11 | #include<mbed.h> |
robokenken | 0:dea3c70f4911 | 12 | |
robokenken | 0:dea3c70f4911 | 13 | using namespace std; |
robokenken | 0:dea3c70f4911 | 14 | |
robokenken | 0:dea3c70f4911 | 15 | |
robokenken | 0:dea3c70f4911 | 16 | /** Matrix class |
robokenken | 0:dea3c70f4911 | 17 | * |
robokenken | 0:dea3c70f4911 | 18 | * This is the simple matrix class. |
robokenken | 0:dea3c70f4911 | 19 | * You can use some basic culculation(add, subtract, multiple). |
robokenken | 0:dea3c70f4911 | 20 | */ |
robokenken | 0:dea3c70f4911 | 21 | Serial pc(USBTX, USBRX); |
robokenken | 0:dea3c70f4911 | 22 | |
robokenken | 0:dea3c70f4911 | 23 | template <class T> |
robokenken | 0:dea3c70f4911 | 24 | class Matrix{ |
robokenken | 0:dea3c70f4911 | 25 | private: |
robokenken | 0:dea3c70f4911 | 26 | int row; |
robokenken | 0:dea3c70f4911 | 27 | int col; |
robokenken | 0:dea3c70f4911 | 28 | vector< vector<T> > matrix; |
robokenken | 0:dea3c70f4911 | 29 | public: |
robokenken | 0:dea3c70f4911 | 30 | //Serial pc(USBTX, USBRX); |
robokenken | 0:dea3c70f4911 | 31 | |
robokenken | 0:dea3c70f4911 | 32 | /** Constructor Matrix |
robokenken | 0:dea3c70f4911 | 33 | * This is constructor. |
robokenken | 0:dea3c70f4911 | 34 | * @param n number of matrix's row |
robokenken | 0:dea3c70f4911 | 35 | * @param m number of matrix's col |
robokenken | 0:dea3c70f4911 | 36 | * @param initNum digit you want fill in the matrix first |
robokenken | 0:dea3c70f4911 | 37 | */ |
robokenken | 0:dea3c70f4911 | 38 | Matrix(int n, int m, T initNum):row(n), col(m), matrix( vector< vector<T> >(n, vector<T>(m))){ |
robokenken | 0:dea3c70f4911 | 39 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 40 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 41 | matrix[i][j] = initNum; |
robokenken | 0:dea3c70f4911 | 42 | } |
robokenken | 0:dea3c70f4911 | 43 | } |
robokenken | 0:dea3c70f4911 | 44 | } |
robokenken | 0:dea3c70f4911 | 45 | |
robokenken | 0:dea3c70f4911 | 46 | Matrix(int n, int m):row(n), col(m), matrix( vector< vector<T> >(n, vector<T>(m))){} |
robokenken | 0:dea3c70f4911 | 47 | |
robokenken | 0:dea3c70f4911 | 48 | ~Matrix(){} |
robokenken | 0:dea3c70f4911 | 49 | |
robokenken | 0:dea3c70f4911 | 50 | void show(){ |
robokenken | 0:dea3c70f4911 | 51 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 52 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 53 | pc.printf("%lf", matrix[i][j]); |
robokenken | 0:dea3c70f4911 | 54 | } |
robokenken | 0:dea3c70f4911 | 55 | pc.printf("\n"); |
robokenken | 0:dea3c70f4911 | 56 | } |
robokenken | 0:dea3c70f4911 | 57 | pc.printf("\n"); |
robokenken | 0:dea3c70f4911 | 58 | } |
robokenken | 0:dea3c70f4911 | 59 | |
robokenken | 0:dea3c70f4911 | 60 | /** Row |
robokenken | 0:dea3c70f4911 | 61 | * Use when you want to refer number of matrix's row |
robokenken | 0:dea3c70f4911 | 62 | * @retval this will return number of matrix's row |
robokenken | 0:dea3c70f4911 | 63 | */ |
robokenken | 0:dea3c70f4911 | 64 | int Row(){ return row; } |
robokenken | 0:dea3c70f4911 | 65 | int Col(){ return col; } |
robokenken | 0:dea3c70f4911 | 66 | |
robokenken | 0:dea3c70f4911 | 67 | |
robokenken | 0:dea3c70f4911 | 68 | vector<T> operator[](const int i) const{return matrix[i];} |
robokenken | 0:dea3c70f4911 | 69 | vector<T>& operator[](const int i){return matrix[i];} |
robokenken | 0:dea3c70f4911 | 70 | |
robokenken | 0:dea3c70f4911 | 71 | void set(int num, ...){ |
robokenken | 0:dea3c70f4911 | 72 | va_list ap; |
robokenken | 0:dea3c70f4911 | 73 | va_start(ap, num); |
robokenken | 0:dea3c70f4911 | 74 | for(int i=0; i<row; i++){ |
robokenken | 0:dea3c70f4911 | 75 | for(int j=0; j<col; j++){ |
robokenken | 0:dea3c70f4911 | 76 | matrix[i][j] = va_arg(ap, T); |
robokenken | 0:dea3c70f4911 | 77 | //pc.printf("%lf\n", va_arg(ap, T)); |
robokenken | 0:dea3c70f4911 | 78 | } |
robokenken | 0:dea3c70f4911 | 79 | } |
robokenken | 0:dea3c70f4911 | 80 | va_end(ap); |
robokenken | 0:dea3c70f4911 | 81 | } |
robokenken | 0:dea3c70f4911 | 82 | |
robokenken | 0:dea3c70f4911 | 83 | |
robokenken | 0:dea3c70f4911 | 84 | Matrix& operator=(const Matrix& a){ |
robokenken | 0:dea3c70f4911 | 85 | if((row == a.row) && (col == a.col)){ |
robokenken | 0:dea3c70f4911 | 86 | row = a.row; |
robokenken | 0:dea3c70f4911 | 87 | col = a.col; |
robokenken | 0:dea3c70f4911 | 88 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 89 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 90 | matrix[i][j] = a[i][j]; |
robokenken | 0:dea3c70f4911 | 91 | } |
robokenken | 0:dea3c70f4911 | 92 | } |
robokenken | 0:dea3c70f4911 | 93 | }else{ |
robokenken | 0:dea3c70f4911 | 94 | |
robokenken | 0:dea3c70f4911 | 95 | } |
robokenken | 0:dea3c70f4911 | 96 | return *this; |
robokenken | 0:dea3c70f4911 | 97 | } |
robokenken | 0:dea3c70f4911 | 98 | |
robokenken | 0:dea3c70f4911 | 99 | Matrix& operator+=(const Matrix& a){ |
robokenken | 0:dea3c70f4911 | 100 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 101 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 102 | matrix[i][j] += a[i][j]; |
robokenken | 0:dea3c70f4911 | 103 | } |
robokenken | 0:dea3c70f4911 | 104 | } |
robokenken | 0:dea3c70f4911 | 105 | return *this; |
robokenken | 0:dea3c70f4911 | 106 | } |
robokenken | 0:dea3c70f4911 | 107 | |
robokenken | 0:dea3c70f4911 | 108 | Matrix operator+(const Matrix& a) const { |
robokenken | 0:dea3c70f4911 | 109 | return Matrix(*this) += a; |
robokenken | 0:dea3c70f4911 | 110 | } |
robokenken | 0:dea3c70f4911 | 111 | |
robokenken | 0:dea3c70f4911 | 112 | Matrix& operator-=(const Matrix& a){ |
robokenken | 0:dea3c70f4911 | 113 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 114 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 115 | matrix[i][j] -= a[i][j]; |
robokenken | 0:dea3c70f4911 | 116 | } |
robokenken | 0:dea3c70f4911 | 117 | } |
robokenken | 0:dea3c70f4911 | 118 | return *this; |
robokenken | 0:dea3c70f4911 | 119 | } |
robokenken | 0:dea3c70f4911 | 120 | |
robokenken | 0:dea3c70f4911 | 121 | Matrix operator-(const Matrix& a) const { |
robokenken | 0:dea3c70f4911 | 122 | return Matrix(*this) -= a; |
robokenken | 0:dea3c70f4911 | 123 | } |
robokenken | 0:dea3c70f4911 | 124 | |
robokenken | 0:dea3c70f4911 | 125 | Matrix& operator*=(const Matrix& a){ |
robokenken | 0:dea3c70f4911 | 126 | Matrix<T> tmp(row, a.col); |
robokenken | 0:dea3c70f4911 | 127 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 128 | for(int j=0;j<a.col;j++){ |
robokenken | 0:dea3c70f4911 | 129 | for(int k=0;k<col;k++){ |
robokenken | 0:dea3c70f4911 | 130 | tmp[i][j] += matrix[i][k] * a[k][j]; |
robokenken | 0:dea3c70f4911 | 131 | } |
robokenken | 0:dea3c70f4911 | 132 | } |
robokenken | 0:dea3c70f4911 | 133 | } |
robokenken | 0:dea3c70f4911 | 134 | col = a.col; |
robokenken | 0:dea3c70f4911 | 135 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 136 | matrix[i].resize(col); |
robokenken | 0:dea3c70f4911 | 137 | } |
robokenken | 0:dea3c70f4911 | 138 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 139 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 140 | matrix[i][j] = tmp[i][j]; |
robokenken | 0:dea3c70f4911 | 141 | } |
robokenken | 0:dea3c70f4911 | 142 | } |
robokenken | 0:dea3c70f4911 | 143 | |
robokenken | 0:dea3c70f4911 | 144 | return *this; |
robokenken | 0:dea3c70f4911 | 145 | } |
robokenken | 0:dea3c70f4911 | 146 | |
robokenken | 0:dea3c70f4911 | 147 | Matrix operator*(const Matrix& a) const { |
robokenken | 0:dea3c70f4911 | 148 | return Matrix(*this) *= a; |
robokenken | 0:dea3c70f4911 | 149 | } |
robokenken | 0:dea3c70f4911 | 150 | Matrix& operator*=(const T& a){ |
robokenken | 0:dea3c70f4911 | 151 | for(int i=0;i<row;i++){ |
robokenken | 0:dea3c70f4911 | 152 | for(int j=0;j<col;j++){ |
robokenken | 0:dea3c70f4911 | 153 | matrix[i][j] *= a; |
robokenken | 0:dea3c70f4911 | 154 | } |
robokenken | 0:dea3c70f4911 | 155 | } |
robokenken | 0:dea3c70f4911 | 156 | return *this; |
robokenken | 0:dea3c70f4911 | 157 | } |
robokenken | 0:dea3c70f4911 | 158 | |
robokenken | 0:dea3c70f4911 | 159 | Matrix operator*(const T& a) const { |
robokenken | 0:dea3c70f4911 | 160 | return Matrix(*this) *= a; |
robokenken | 0:dea3c70f4911 | 161 | } |
robokenken | 0:dea3c70f4911 | 162 | }; |
robokenken | 0:dea3c70f4911 | 163 | |
robokenken | 0:dea3c70f4911 | 164 | |
robokenken | 0:dea3c70f4911 | 165 | #endif |