Akashi_2021_Bteam / odometry

odometry

Dependencies:   arrc_mbed BNO055

matrix.h@1:4062af6224fa, 2021-07-03 (annotated)

Committer:
robokenken
Date:
Sat Jul 03 00:15:03 2021 +0000
Revision:
1:4062af6224fa
Parent:
0:dea3c70f4911 
nakasima

Who changed what in which revision?

UserRevisionLine numberNew 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