odometry

Dependencies:   arrc_mbed BNO055

Revision:
0:dea3c70f4911
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/matrix.h	Mon Jun 21 07:35:27 2021 +0000
@@ -0,0 +1,165 @@
+/** @file
+ *
+ * My program.
+ */
+
+#ifndef MATRIX2_LIB
+#define MATRIX2_LIB
+
+#include<vector>
+#include<stdarg.h>
+#include<mbed.h>
+
+using namespace std;
+
+
+/** Matrix class
+ *
+ * This is the simple matrix class.
+ * You can use some basic culculation(add, subtract, multiple). 
+ */
+Serial pc(USBTX, USBRX);
+ 
+template <class T>
+class Matrix{
+    private:
+        int row;
+        int col;
+        vector< vector<T> > matrix;
+    public:
+        //Serial pc(USBTX, USBRX);
+    
+        /** Constructor Matrix
+        * This is constructor.
+        * @param n number of matrix's row
+        * @param m number of matrix's col
+        * @param initNum digit you want fill in the matrix first 
+        */
+        Matrix(int n, int m, T initNum):row(n), col(m), matrix( vector< vector<T> >(n, vector<T>(m))){
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    matrix[i][j] = initNum;
+                }
+            }
+        }
+        
+        Matrix(int n, int m):row(n), col(m), matrix( vector< vector<T> >(n, vector<T>(m))){}
+        
+        ~Matrix(){}
+        
+        void show(){
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    pc.printf("%lf", matrix[i][j]);
+                }
+                pc.printf("\n");
+            }
+            pc.printf("\n");
+        }
+        
+        /** Row
+        * Use when you want to refer number of matrix's row
+        * @retval this will return number of matrix's row
+        */
+        int Row(){ return row; }
+        int Col(){ return col; }
+            
+
+        vector<T> operator[](const int i) const{return matrix[i];}
+        vector<T>& operator[](const int i){return matrix[i];}
+        
+        void set(int num, ...){
+            va_list ap;
+            va_start(ap, num);
+            for(int i=0; i<row; i++){
+                for(int j=0; j<col; j++){
+                    matrix[i][j] = va_arg(ap, T);
+                    //pc.printf("%lf\n", va_arg(ap, T));
+                }
+            }
+            va_end(ap);
+        }
+            
+
+        Matrix& operator=(const Matrix& a){
+            if((row == a.row) && (col == a.col)){
+                row = a.row;
+                col = a.col;
+                for(int i=0;i<row;i++){
+                    for(int j=0;j<col;j++){
+                        matrix[i][j] = a[i][j];
+                    }
+                }
+            }else{
+
+            }
+            return *this;
+        }
+
+        Matrix& operator+=(const Matrix& a){
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    matrix[i][j] += a[i][j];
+                }
+            }
+            return *this;
+        }
+
+        Matrix operator+(const Matrix& a) const {
+            return Matrix(*this) += a;
+        }
+
+        Matrix& operator-=(const Matrix& a){
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    matrix[i][j] -= a[i][j];
+                }
+            }
+            return *this;
+        }
+        
+        Matrix operator-(const Matrix& a) const {
+            return Matrix(*this) -= a;
+        }
+
+        Matrix& operator*=(const Matrix& a){
+            Matrix<T> tmp(row, a.col);
+            for(int i=0;i<row;i++){
+                for(int j=0;j<a.col;j++){
+                    for(int k=0;k<col;k++){
+                        tmp[i][j] += matrix[i][k] * a[k][j];
+                    }
+                }
+            }
+            col = a.col;
+            for(int i=0;i<row;i++){
+                matrix[i].resize(col);
+            }
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    matrix[i][j] = tmp[i][j];
+                }
+            }
+
+            return *this;
+        }
+        
+        Matrix operator*(const Matrix& a) const {
+            return Matrix(*this) *= a;
+        }
+                Matrix& operator*=(const T& a){
+            for(int i=0;i<row;i++){
+                for(int j=0;j<col;j++){
+                    matrix[i][j] *= a;
+                }
+            }
+            return *this;
+        }
+
+        Matrix operator*(const T& a) const {
+            return Matrix(*this) *= a;
+        }
+};
+
+
+#endif
\ No newline at end of file