MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 #pragma once 00002 #ifndef __MAT__FUNS__ 00003 #define __MAT__FUNS__ 00004 00005 #include "robotTypes.h" 00006 #include <math.h> 00007 00008 #ifndef PI 00009 #define PI 3.1415926535f // Con 1 palo y 5 ladrillos se pueden hacer mil cosas 00010 #define PIx2 6.28318530f 00011 #define PImed 1.57079632f 00012 #endif 00013 00014 #ifndef EPS 00015 #define EPS 0.000001 00016 #endif 00017 00018 /** 00019 * 00020 * @brief Implents a common matrix and operations 00021 * 00022 */ 00023 class matrix { 00024 00025 protected: 00026 00027 unsigned short rows; 00028 unsigned short cols; 00029 float* val; 00030 00031 public: 00032 matrix(); 00033 matrix(int rows, int cols); 00034 virtual ~matrix(); 00035 matrix(const matrix&); 00036 matrix(const pos3d&); 00037 matrix(const posCil3d&); 00038 00039 void set(int r, int c, float val); 00040 void set(int r, int c, const matrix& mat); 00041 float get(int r, int c) const; 00042 float operator()(int r, int c) const; 00043 00044 int getRows() const; 00045 int getCols() const; 00046 00047 void clear(); 00048 00049 matrix& operator= (const matrix&); 00050 matrix& operator= (const pos3d&); 00051 matrix& operator= (const posCil3d&); 00052 00053 matrix& operator+= (const matrix&); 00054 matrix& operator-= (const matrix&); 00055 matrix& operator*= (const float&); 00056 00057 matrix operator+ (const matrix&) const; 00058 matrix operator- (const matrix&) const; 00059 matrix operator* (const matrix&) const; 00060 matrix operator* (const float&) const; 00061 00062 matrix transpose() const; 00063 matrix inverse() const; 00064 float det() const; 00065 00066 void print(const char* str=0) const; 00067 00068 pos3d toPos3d() const; 00069 pose toPose() const; 00070 00071 static const matrix identity(int size); 00072 00073 }; 00074 00075 inline float matrix::operator()(int r, int c) const {return (r < rows && c < cols)? val[r*cols+c] : 0.0f;}; 00076 inline void matrix::set(int r, int c, float v) {if (r < rows && c < cols) val[r*cols+c] = v;}; 00077 inline float matrix::get(int r, int c) const {return (r < rows && c < cols)? val[r*cols+c] : 0.0f;}; 00078 inline int matrix::getRows() const {return rows;} 00079 inline int matrix::getCols() const {return cols;} 00080 00081 //******************************************************************************************************************* 00082 00083 /// translades from local measure to global position 00084 pos3d base2global(const pos3d& base, const pose& pos); 00085 /// translades from global position to local measure 00086 pos3d global2base(const pos3d& global, const pose& pos); 00087 00088 posCil3d cart2cil(const pos3d& xyz); 00089 double normrnd(double mu=0.0, double sigma=1.0); 00090 float gauss(int px, int py, float centerx, float centery, float sigma); 00091 bool intersect(line& line1, line& line2, pointf& result); 00092 float poly(const double* pol, const double x, int grad); 00093 00094 //******************************************************************************************************************** 00095 00096 /** 00097 * 00098 * @brief Implents an expansible matrix and its operations 00099 * 00100 * This size reserved for the matrix data is independent of the current size 00101 * This matrices can me expanded without relocating the data 00102 */ 00103 class Ematrix{ 00104 00105 protected: 00106 00107 unsigned short reservedRows; 00108 unsigned short reservedCols; 00109 unsigned short rows; 00110 unsigned short cols; 00111 float* val; 00112 00113 public: 00114 00115 Ematrix(); 00116 Ematrix(int rows, int cols, int reserveRows, int reserveCols); 00117 Ematrix(int rows, int cols); 00118 virtual ~Ematrix(); 00119 Ematrix(const Ematrix&); 00120 Ematrix(const matrix&); 00121 Ematrix(const pos3d&); 00122 Ematrix(const posCil3d&); 00123 00124 void set(int r, int c, float val); 00125 void set(int r, int c, const matrix& mat); 00126 void set(int r, int c, const Ematrix& mat); 00127 float get(int r, int c) const; 00128 float operator()(int r, int c) const; 00129 00130 matrix subMat(int rini, int rend, int cini, int cend) const; 00131 void extend(int rinc, int cinc); 00132 void initialize(int rows, int cols, int reserveRows, int reserveCols); 00133 00134 void clear(); 00135 00136 Ematrix& operator= (const Ematrix&); 00137 Ematrix& operator= (const matrix&); 00138 Ematrix& operator= (const pos3d&); 00139 Ematrix& operator= (const posCil3d&); 00140 00141 Ematrix& operator+= (const Ematrix&); 00142 Ematrix& operator-= (const Ematrix&); 00143 00144 Ematrix operator+ (const Ematrix&) const; 00145 Ematrix operator- (const Ematrix&) const; 00146 Ematrix operator* (const Ematrix&) const; 00147 Ematrix mul2 (const Ematrix&) const; 00148 00149 Ematrix& operator*= (const float&); 00150 Ematrix operator* (const float&) const; 00151 00152 void addSubMat(int r, int c, const Ematrix& mat); 00153 void addSubMat(int r, int c, const matrix& mat); 00154 00155 Ematrix transpose() const; 00156 Ematrix inverse() const; 00157 float det() const; 00158 00159 void print(const char* str=0) const; 00160 00161 pos3d toPos3d() const; 00162 pose toPose() const; 00163 00164 static const Ematrix identity(int size); 00165 00166 int getNumRows() const; 00167 int getNumCols() const; 00168 00169 float totalsum() const; 00170 00171 }; 00172 00173 inline float Ematrix::operator()(int r, int c) const {return (r < rows && c < cols)? val[r*reservedCols+c] : 0.0f;}; 00174 inline void Ematrix::set(int r, int c, float v) {if (r < rows && c < cols) val[r*reservedCols+c] = v;}; 00175 inline float Ematrix::get(int r, int c) const {return (r < rows && c < cols)? val[r*reservedCols+c] : 0.0f;}; 00176 00177 inline int Ematrix::getNumRows() const {return rows;}; 00178 inline int Ematrix::getNumCols() const {return cols;}; 00179 00180 00181 #endif 00182 00183