MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 #include <opencv2/opencv.hpp> 00002 #include <string.h> 00003 #include <iostream> 00004 #include <time.h> 00005 #include <ctime> // For time() 00006 #include <cstdlib> // For srand() and rand() 00007 #include "matFuns.h" 00008 00009 pos3d base2global(const pos3d& base, const pose& pos){ 00010 float costh = (float)cos(pos.th); 00011 float sinth = (float)sin(pos.th); 00012 return pos3d( costh * base.getX() - sinth * base.getY() + pos.x, 00013 sinth * base.getX() + costh * base.getY() + pos.y, 00014 base.getZ()); 00015 } 00016 00017 pos3d global2base(const pos3d& global, const pose& pos){ 00018 float auxx = global.getX() - pos.x; 00019 float auxy = global.getY() - pos.y; 00020 float costh = (float)cos(pos.th); 00021 float sinth = (float)sin(pos.th); 00022 return pos3d( costh*auxx + sinth*auxy, 00023 - sinth*auxx + costh*auxy, 00024 global.getZ()); 00025 } 00026 00027 posCil3d cart2cil(const pos3d& xyz){ 00028 return posCil3d( sqrt(xyz.getX()*xyz.getX()+xyz.getY()*xyz.getY()), 00029 atan2(xyz.getY(),xyz.getX()), 00030 xyz.getZ()); 00031 } 00032 00033 float poly(const double* pol, const double x, int grad){ 00034 float y=0; 00035 for(int i = 0; i <= grad; i++) y += pol[i]*pow(x,grad-i); 00036 return(y); 00037 } 00038 00039 00040 00041 /// this function implements a random number with gaussian probability 00042 /// the genaration uses the box-muller transform using the polar form 00043 /// http://en.wikipedia.org/wiki/Box-Muller_transform 00044 00045 00046 double normrnd(double mu, double sigma){ 00047 00048 static bool deviateAvailable=false; // flag 00049 static double storedDeviate; // deviate from previous calculation 00050 double polar, rsquared, var1, var2; 00051 00052 static int myrnd=0; 00053 //srand(myrnd); 00054 //srand((unsigned)time(0)); 00055 // If no deviate has been stored, the polar Box-Muller transformation is 00056 // performed, producing two independent normally-distributed random 00057 // deviates. One is stored for the next round, and one is returned. 00058 00059 if (!deviateAvailable) { 00060 // choose pairs of uniformly distributed deviates, discarding those 00061 // that don't fall within the unit circle 00062 do { 00063 myrnd = rand(); 00064 var1=2.0*( double(myrnd)/double(RAND_MAX) ) - 1.0; 00065 myrnd = rand(); 00066 var2=2.0*( double(myrnd)/double(RAND_MAX) ) - 1.0; 00067 rsquared=var1*var1+var2*var2; 00068 } while ( rsquared>=1.0 || rsquared == 0.0); 00069 00070 // calculate polar tranformation for each deviate 00071 polar=sqrt(-2.0*log(rsquared)/rsquared); 00072 00073 // store first deviate and set flag 00074 storedDeviate=var1*polar; 00075 deviateAvailable=true; 00076 00077 // return second deviate 00078 return var2*polar*sigma + mu; 00079 } 00080 00081 // If a deviate is available from a previous call to this function, it is 00082 // returned, and the flag is set to false. 00083 else { 00084 deviateAvailable=false; 00085 return storedDeviate*sigma + mu; 00086 } 00087 } 00088 00089 00090 float gauss(int px, int py, float centerx, float centery, float sigma){ 00091 float difx = (px-centerx); 00092 float dify = (py-centery); 00093 return (exp(-(difx*difx+dify*dify)/(2.0f*sigma*sigma))); 00094 } 00095 00096 bool intersect(line& line1, line& line2, pointf& result){ 00097 //printf("line1 x1: %f, y1: %f, x2: %f, y2: %f\n", line1.x1, line1.y1, line1.x2, line1.y2); 00098 float A1 = line1.y2 - line1.y1; 00099 float B1 = line1.x1 - line1.x2; 00100 float C1 = A1*line1.x1 + B1*line1.y1; 00101 00102 float A2 = line2.y2 - line2.y1; 00103 float B2 = line2.x1 - line2.x2; 00104 float C2 = A2*line2.x1 + B2*line2.y1; 00105 00106 float det = A1*B2 - A2*B1; 00107 if (!det) return false; 00108 else{ 00109 // punto de corte entre las dos rectas 00110 result.x = (B2*C1 - B1*C2)/det; 00111 result.y = (A1*C2 - A2*C1)/det; 00112 } 00113 00114 const float delta = -0.01f; //Ojo! deberia ser 0.0, por errores de redondeo hay que tomar -0.1 como margen 00115 00116 // comprobamos que el punto de corte este definido dentro de los 2 segmentos 00117 return (((line1.x1-result.x)*(result.x-line1.x2)>=delta && 00118 (line1.y1-result.y)*(result.y-line1.y2)>=delta && 00119 (line2.x1-result.x)*(result.x-line2.x2)>=delta && 00120 (line2.y1-result.y)*(result.y-line2.y2)>=delta ) ? true : false ); //no se intersectan 00121 } 00122 00123 matrix::matrix():rows(0),cols(0),val(0){} 00124 00125 matrix::~matrix(){if (val) delete[] val;} 00126 00127 matrix::matrix(int r, int c): rows(r),cols(c),val(new float[r*c]){clear();} 00128 00129 matrix::matrix(const matrix &m): rows(m.rows),cols(m.cols),val(new float[m.rows*m.cols]){ 00130 memcpy(val, m.val, rows*cols*sizeof(float)); 00131 } 00132 00133 matrix::matrix(const pos3d &p): rows(3), cols(1), val(new float[3]){ 00134 set(0,0,p.getX()); 00135 set(1,0,p.getY()); 00136 set(2,0,p.getZ()); 00137 } 00138 00139 00140 matrix::matrix(const posCil3d &p): 00141 rows(3), 00142 cols(1), 00143 val(new float[3]) 00144 { 00145 set(0,0,p.getR()); 00146 set(1,0,p.getAlfa()); 00147 set(2,0,p.getZ()); 00148 } 00149 00150 00151 void matrix::clear(){ 00152 for (int i = 0; i<rows*cols; i++) val[i] = 0.0f; 00153 } 00154 00155 00156 pos3d matrix::toPos3d() const { 00157 if(rows != 3 || cols != 1) { printf("ToPos3D: Need to be a 3x1 matrix \n"); } 00158 assert(rows == 3 && cols == 1); 00159 pos3d p(get(0,0),get(1,0),get(2,0)); 00160 return p; 00161 } 00162 00163 pose matrix::toPose() const { 00164 if(rows != 3 || cols != 1) { printf("ToPose: Need to be a 3x1 matrix \n"); } 00165 assert(rows == 3 && cols == 1); 00166 return pose(get(0,0),get(1,0),get(2,0)); 00167 } 00168 00169 matrix& matrix::operator=(const matrix& m){ 00170 if(m.rows != rows || m.cols != cols){ 00171 if (val) delete[] val; 00172 rows = m.rows; 00173 cols = m.cols; 00174 val = new float[rows*cols]; 00175 } 00176 memcpy(val, m.val, rows*cols*sizeof(float)); 00177 return *this; 00178 } 00179 00180 matrix& matrix::operator=(const pos3d& p){ 00181 if(rows != 3 || cols != 1){ 00182 if (val) delete[] val; 00183 rows = 3; 00184 cols = 1; 00185 val = new float[rows*cols]; 00186 } 00187 set(0,0,p.getX()); 00188 set(1,0,p.getY()); 00189 set(2,0,p.getZ()); 00190 return *this; 00191 } 00192 00193 matrix& matrix::operator=(const posCil3d& p){ 00194 if(rows != 3 || cols != 1){ 00195 if (val) delete[] val; 00196 rows = 3; 00197 cols = 1; 00198 val = new float[rows*cols]; 00199 } 00200 set(0,0,p.getR()); 00201 set(1,0,p.getAlfa()); 00202 set(2,0,p.getZ()); 00203 00204 return *this; 00205 } 00206 00207 matrix& matrix::operator+= (const matrix& m){ 00208 if(m.rows != rows || m.cols != cols) 00209 printf("SUM: Matrix dimensions don't agree\n"); 00210 else 00211 for (int i = 0; i<rows*cols; i++) val[i]+=m.val[i]; 00212 return *this; 00213 } 00214 00215 matrix matrix::operator+(const matrix& m) const{ 00216 matrix mat(*this); 00217 mat += m; 00218 return mat; 00219 } 00220 00221 matrix& matrix::operator-= (const matrix& m){ 00222 if(m.rows != rows || m.cols != cols) 00223 printf("SUM: Matrix dimensions don't agree\n"); 00224 else 00225 for (int i = 0; i<rows*cols; i++) val[i]-=m.val[i]; 00226 return *this; 00227 } 00228 00229 matrix matrix::operator-(const matrix& m) const { 00230 matrix mat(*this); 00231 mat -= m; 00232 return mat; 00233 } 00234 00235 matrix& matrix::operator*= (const float& f) { 00236 for (int i = 0; i<rows*cols; i++) val[i]*=f; 00237 return *this; 00238 } 00239 00240 matrix matrix::operator*(const float& f) const { 00241 matrix mat(*this); 00242 mat *= f; 00243 return mat; 00244 } 00245 00246 matrix matrix::operator*(const matrix& m) const{ 00247 matrix mat(rows, m.cols); 00248 if (this->cols != m.rows) printf("MUL: Matrix dimensions don't agree\n"); 00249 else{ 00250 int id1,id2,id3=0,id4; 00251 for (int i = 0; i < cols; i++){ 00252 id1 = 0; 00253 id2 = 0; 00254 for (int r = 0; r<rows; r++){ 00255 id4 = id2+i; 00256 if (val[id4]!=0){ 00257 for (int c = 0; c<m.cols; c++){ 00258 mat.val[id1+c] += val[id4] * m.val[id3+c]; 00259 } 00260 } 00261 id1 += mat.cols; 00262 id2 += cols; 00263 } 00264 id3 += m.cols; 00265 } 00266 } 00267 return mat; 00268 } 00269 00270 matrix matrix::transpose () const { 00271 matrix mat(cols,rows); 00272 for (int r = 0; r<rows; r++) 00273 for (int c = 0; c<cols; c++) 00274 mat.set(c,r, val[r*cols+c]); 00275 return mat; 00276 } 00277 00278 matrix matrix::inverse() const{ 00279 int r; 00280 if (rows != cols) { printf("INV: Matrix must be square\n");} 00281 assert(rows == cols); 00282 matrix mat(rows,cols); 00283 CvMat* ocvMat = cvCreateMat(rows,cols,CV_32FC1); 00284 for (r = 0; r<rows; r++) 00285 for (int c = 0; c<cols; c++) 00286 cvmSet(ocvMat,r,c,get(r,c)); 00287 00288 CvMat* ocvMatInv = cvCreateMat(rows,cols,CV_32FC1); 00289 cvInvert(ocvMat, ocvMatInv, CV_LU ); 00290 for (r = 0; r<rows; r++) 00291 for (int c = 0; c<cols; c++) 00292 mat.set(r,c, (float)cvmGet(ocvMatInv,r,c)); 00293 cvReleaseMat(&ocvMat); 00294 cvReleaseMat(&ocvMatInv); 00295 return mat; 00296 } 00297 00298 float matrix::det() const { 00299 if(rows != 3 || cols !=3){printf("DET: Only 3x3 determinants");} 00300 assert(rows == 3 && cols ==3); 00301 return(get(0,0)*get(1,1)*get(2,2)+get(0,1)*get(1,2)*get(2,0)+get(0,2)*get(1,0)*get(2,1) 00302 -get(0,0)*get(1,2)*get(2,1)-get(0,2)*get(1,1)*get(2,0)-get(0,1)*get(1,0)*get(2,2)); 00303 } 00304 00305 void matrix::print(const char* str) const { 00306 printf("%s\t",str); 00307 for (int r = 0; r<rows; r++){ 00308 for (int c = 0; c<cols; c++) 00309 printf("%e \t", get(r,c)); 00310 //printf("%10.7f \t", get(r,c)); 00311 printf("\n\t"); 00312 } 00313 printf("\n"); 00314 } 00315 00316 const matrix matrix::identity(int r){ 00317 matrix mat(r,r); 00318 for (int i=0; i< r; i++) mat.set(i,i,1); 00319 return mat; 00320 } 00321 00322 void matrix::set(int r, int c, const matrix& mat){ 00323 int x,y; 00324 x = r; 00325 for (int i = 0; i < mat.getRows() ; i++){ 00326 y=c; 00327 for (int j = 0; j < mat.getCols() ; j++){ 00328 set(x,y,mat(i,j)); 00329 y++; 00330 } 00331 x++; 00332 } 00333 } 00334 //////////////////////////////////////////// Ematrix /////////////////////////////////////////////// 00335 00336 Ematrix::Ematrix(): reservedRows(0),reservedCols(0),val(0){} 00337 00338 Ematrix::~Ematrix(){if (val) delete[] val;} 00339 00340 Ematrix::Ematrix(int r, int c, int resr, int resc): 00341 reservedRows(resr), 00342 reservedCols(resc), 00343 rows(r), 00344 cols(c), 00345 val(new float[reservedRows*reservedCols]) 00346 { 00347 clear(); 00348 } 00349 00350 Ematrix::Ematrix(int r, int c): 00351 reservedRows(r), 00352 reservedCols(c), 00353 rows(r), 00354 cols(c), 00355 val(new float[reservedRows*reservedCols]) 00356 { 00357 clear(); 00358 } 00359 00360 void Ematrix::initialize(int r, int c, int resr, int resc){ 00361 reservedRows=resr; 00362 reservedCols=resc; 00363 rows=r; 00364 cols=c; 00365 if (val) delete[] val; 00366 val = new float[reservedRows*reservedCols]; 00367 clear(); 00368 } 00369 00370 Ematrix::Ematrix(const Ematrix &m): 00371 reservedRows(m.reservedRows), 00372 reservedCols(m.reservedCols), 00373 rows(m.rows), 00374 cols(m.cols), 00375 val(new float[reservedRows*reservedCols]) 00376 { 00377 for(int i = 0; i < rows; i++){ 00378 int idx = i*reservedCols; 00379 for(int j = 0; j < cols; j++){ 00380 int idx2 = idx+j; 00381 val[idx2] = m.val[idx2]; 00382 } 00383 } 00384 } 00385 00386 Ematrix::Ematrix(const matrix &m): 00387 reservedRows(m.getRows()), 00388 reservedCols(m.getCols()), 00389 rows(reservedRows), 00390 cols(reservedCols), 00391 val(new float[reservedRows*reservedCols]) 00392 { 00393 for(int i = 0; i < rows; i++){ 00394 int idx = i*reservedCols; 00395 for(int j = 0; j < cols; j++){ 00396 val[idx+j] = m.get(i,j); 00397 } 00398 } 00399 } 00400 00401 00402 Ematrix::Ematrix(const pos3d &p): 00403 reservedRows(3), 00404 reservedCols(1), 00405 rows(3), 00406 cols(1), 00407 val(new float[3]) 00408 { 00409 set(0,0,p.getX()); 00410 set(1,0,p.getY()); 00411 set(2,0,p.getZ()); 00412 } 00413 00414 Ematrix::Ematrix(const posCil3d &p): 00415 reservedRows(3), 00416 reservedCols(1), 00417 rows(3), 00418 cols(1), 00419 val(new float[3]) 00420 { 00421 set(0,0,p.getR()); 00422 set(1,0,p.getAlfa()); 00423 set(2,0,p.getZ()); 00424 } 00425 00426 void Ematrix::clear(){ 00427 int id; 00428 for(int i = 0; i < rows; i++){ 00429 id = i*reservedCols; 00430 for(int j = 0; j < cols; j++) 00431 val[id+j]= 0.0f; 00432 } 00433 } 00434 00435 Ematrix& Ematrix::operator=(const Ematrix& m){ 00436 if(m.rows > reservedRows || m.cols > reservedCols){ 00437 reservedRows = m.rows; 00438 reservedCols = m.cols; 00439 if (val) delete [] val; 00440 val = new float[reservedRows*reservedCols]; 00441 } 00442 rows = m.rows; 00443 cols = m.cols; 00444 for(int i = 0; i < rows; i++) 00445 for(int j = 0; j < cols; j++) 00446 set(i,j, m.get(i,j)); 00447 return *this; 00448 } 00449 00450 Ematrix& Ematrix::operator=(const pos3d& p){ 00451 if(reservedRows < 3 || reservedCols < 1){ 00452 if (val) delete[] val; 00453 rows = 3; 00454 cols = 1; 00455 reservedRows = 3; 00456 reservedCols = 1; 00457 val = new float[reservedRows*reservedCols]; 00458 } 00459 set(0,0,p.getX()); 00460 set(1,0,p.getY()); 00461 set(2,0,p.getZ()); 00462 00463 return *this; 00464 } 00465 00466 Ematrix& Ematrix::operator=(const posCil3d& p){ 00467 if(reservedRows < 3 || reservedCols < 1){ 00468 if (val) delete[] val; 00469 rows = 3; 00470 cols = 1; 00471 reservedRows = 3; 00472 reservedCols = 1; 00473 val = new float[reservedRows*reservedCols]; 00474 } 00475 set(0,0,p.getR()); 00476 set(1,0,p.getAlfa()); 00477 set(2,0,p.getZ()); 00478 return *this; 00479 } 00480 00481 Ematrix Ematrix::operator+(const Ematrix& m) const{ 00482 Ematrix mat(*this); 00483 mat += m; 00484 return mat; 00485 } 00486 00487 Ematrix Ematrix::operator-(const Ematrix& m) const{ 00488 Ematrix mat(*this); 00489 mat -= m; 00490 return mat; 00491 } 00492 00493 Ematrix& Ematrix::operator+= (const Ematrix& m){ 00494 if(m.rows != rows || m.cols != cols) 00495 printf("SUM: Matrix dimensions don't agree\n"); 00496 else{ 00497 int id=0,id2=0; 00498 for(int i = 0; i < rows; i++){ 00499 for(int j = 0; j < cols; j++) 00500 val[id+j] += m.val[id2+j]; 00501 id += reservedCols; 00502 id2 += m.reservedCols; 00503 } 00504 } 00505 return *this; 00506 } 00507 00508 Ematrix& Ematrix::operator-= (const Ematrix& m){ 00509 if(m.rows != rows || m.cols != cols) 00510 printf("SUM: Matrix dimensions don't agree\n"); 00511 else{ 00512 int id=0,id2=0; 00513 for(int i = 0; i < rows; i++){ 00514 for(int j = 0; j < cols; j++) 00515 val[id+j] -= m.val[id2+j]; 00516 id += reservedCols; 00517 id2 += m.reservedCols; 00518 } 00519 } 00520 return *this; 00521 } 00522 00523 Ematrix& Ematrix::operator*= (const float& f) { 00524 int id=0; 00525 for(int i = 0; i < rows; i++){ 00526 for(int j = 0; j < cols; j++) 00527 val[id+j] *= f; 00528 id += reservedCols; 00529 } 00530 return *this; 00531 } 00532 00533 Ematrix Ematrix::operator*(const float& f) const { 00534 Ematrix mat(*this); 00535 mat *= f; 00536 return mat; 00537 } 00538 00539 Ematrix Ematrix::operator*(const Ematrix& m) const{ 00540 Ematrix mat(rows, m.cols, reservedRows, m.reservedCols); 00541 if (this->cols != m.rows) printf("MUL: Matrix dimensions don't agree\n"); 00542 else{ 00543 int id, id1, id2=0, id3; 00544 for (int i = 0; i < cols; i++){ 00545 id = 0; 00546 id3 = 0; 00547 for (int r = 0; r<rows; r++){ 00548 id1 = id3+i; 00549 if (val[id1]!=0){ 00550 for (int c = 0; c<m.cols; c++){ 00551 mat.val[id+c] += val[id1] * m.val[id2+c]; 00552 } 00553 } 00554 id3 +=reservedCols; 00555 id += mat.reservedCols; 00556 } 00557 id2 += m.reservedCols; 00558 } 00559 } 00560 return mat; 00561 } 00562 00563 Ematrix Ematrix::mul2(const Ematrix& m) const{ 00564 Ematrix mat(rows, m.cols, reservedRows, m.reservedCols); 00565 if (this->cols != m.rows) printf("MUL: Matrix dimensions don't agree\n"); 00566 else{ 00567 int id=0, id1, id2, id3; 00568 for (int i = 0; i < cols; i++){ 00569 for (int c = 0; c<m.cols; c++){ 00570 id1 = id+c; 00571 if (m.val[id1]!=0){ 00572 id2 = 0; id3 = 0; 00573 for (int r = 0; r<rows; r++){ 00574 mat.val[id3+c] += val[id2+i] * m.val[id1]; 00575 id2+=reservedCols; 00576 id3+=mat.reservedCols; 00577 } 00578 } 00579 } 00580 id +=m.reservedCols; 00581 } 00582 } 00583 return mat; 00584 } 00585 00586 Ematrix Ematrix::transpose () const { 00587 Ematrix mat(cols,rows,reservedCols,reservedRows); 00588 int id=0; 00589 for (int r = 0; r<rows; r++){ 00590 for (int c = 0; c<cols; c++) 00591 mat.set(c,r, val[id+c]); 00592 id += reservedCols; 00593 } 00594 return mat; 00595 } 00596 00597 Ematrix Ematrix::inverse() const{ 00598 int r,c; 00599 if (rows != cols) { printf("INV: Matrix must be square\n");} 00600 assert(rows == cols); 00601 Ematrix mat(rows,cols,reservedRows,reservedCols); 00602 CvMat* ocvMat = cvCreateMat(rows,cols,CV_32FC1); 00603 int id=0; 00604 for (r = 0; r<rows; r++){ 00605 for ( c = 0; c<cols; c++) 00606 cvmSet(ocvMat,r,c, val[id+c]); 00607 id += reservedCols; 00608 } 00609 00610 CvMat* ocvMatInv = cvCreateMat(rows,cols,CV_32FC1); 00611 cvInvert(ocvMat, ocvMatInv, CV_LU ); 00612 id = 0; 00613 for (r = 0; r<rows; r++){ 00614 for (c = 0; c<cols; c++) 00615 mat.val[id+c] = (float)cvmGet(ocvMatInv,r,c); 00616 id += reservedCols; 00617 } 00618 cvReleaseMat(&ocvMat); 00619 cvReleaseMat(&ocvMatInv); 00620 return mat; 00621 } 00622 00623 float Ematrix::det() const { 00624 if(rows != 3 || cols !=3){printf("DET: Only 3x3 determinants");} 00625 assert(rows == 3 && cols ==3); 00626 return(get(0,0)*get(1,1)*get(2,2)+get(0,1)*get(1,2)*get(2,0)+get(0,2)*get(1,0)*get(2,1) 00627 -get(0,0)*get(1,2)*get(2,1)-get(0,2)*get(1,1)*get(2,0)-get(0,1)*get(1,0)*get(2,2)); 00628 } 00629 00630 void Ematrix::print(const char* str) const { 00631 printf("%s\t",str); 00632 for (int r = 0; r<rows; r++){ 00633 for (int c = 0; c<cols; c++) 00634 printf("%10.7f \t", get(r,c)); 00635 //printf("%e \t", get(r,c)); 00636 printf("\n\t"); 00637 } 00638 printf("\n"); 00639 } 00640 00641 const Ematrix Ematrix::identity(int r){ 00642 Ematrix mat(r,r,r,r); 00643 for (int i=0; i< r; i++) mat.set(i,i,1); 00644 return mat; 00645 } 00646 00647 pos3d Ematrix::toPos3d() const { 00648 if(rows != 3 || cols != 1) { printf("ToPos3D: Need to be a 3x1 matrix \n"); } 00649 assert(rows == 3 && cols == 1); 00650 return pos3d(get(0,0),get(1,0),get(2,0)); 00651 } 00652 00653 pose Ematrix::toPose() const { 00654 if(rows != 3 || cols != 1) { printf("ToPose: Need to be a 3x1 matrix \n"); } 00655 assert(rows == 3 && cols == 1); 00656 return pose(get(0,0),get(1,0),get(2,0)); 00657 } 00658 00659 matrix Ematrix::subMat(int rini, int rend, int cini, int cend) const{ 00660 int sizex = rend-rini+1; 00661 int sizey = cend-cini+1; 00662 matrix mat (sizex, sizey); 00663 int i,j; 00664 i=0; 00665 // printf("Submat [%d:%d, %d:%d]\n",rini,rend,cini,cend); 00666 // printf("(%d,%d) res (%d,%d)",rows,cols,reservedRows,reservedCols); 00667 for (int r = rini; r<= rend; r++){ 00668 j=0; 00669 for (int c = cini; c<=cend; c++){ 00670 mat.set(i,j,get(r,c)); 00671 // printf("[%d,%d] %f ",r,c,get(r,c)); 00672 j++; 00673 } 00674 i++; 00675 // printf("\n"); 00676 } 00677 // printf("\n"); 00678 return mat; 00679 } 00680 00681 void Ematrix::extend(int rinc, int cinc){ 00682 //printf("reserved: %d x %d\n",reservedRows,reservedCols); 00683 //printf("size: %d x %d\n",rows, cols); 00684 if(reservedRows < rows+rinc || reservedCols < cols+cinc){ 00685 int newreservedRows = rows+rinc; 00686 int newreservedCols = cols+cinc; 00687 float* newval = new float[newreservedRows*newreservedCols]; 00688 00689 for(int i = 0; i < rows; i++) 00690 for(int j = 0; j < cols; j++) 00691 newval[i*newreservedCols+j] = val[i*reservedCols+j]; 00692 00693 delete [] val; 00694 val = newval; 00695 reservedRows = newreservedRows; 00696 reservedCols = newreservedCols; 00697 } 00698 int oldcols = cols; 00699 int oldrows = rows; 00700 cols+=cinc; 00701 rows+=rinc; 00702 for (int r = oldrows; r< rows; r++) 00703 for (int c = 0; c< oldcols; c++) 00704 set(r,c,0.0f); 00705 00706 for (int r = 0; r< rows; r++) 00707 for (int c = oldcols; c<cols; c++) 00708 set(r,c,0.0f); 00709 } 00710 00711 void Ematrix::set(int r, int c, const matrix& mat){ 00712 int x,y; 00713 x=r; 00714 for (int i = 0; i < mat.getRows() ; i++){ 00715 y=c; 00716 for (int j = 0; j < mat.getCols() ; j++){ 00717 set(x,y,mat(i,j)); 00718 y++; 00719 } 00720 x++; 00721 } 00722 } 00723 00724 void Ematrix::set(int r, int c, const Ematrix& mat){ 00725 int x,y; 00726 x=r; 00727 for (int i = 0; i < mat.rows ; i++){ 00728 y=c; 00729 for (int j = 0; j < mat.cols ; j++){ 00730 set(x,y,mat(i,j)); 00731 y++; 00732 } 00733 x++; 00734 } 00735 } 00736 00737 void Ematrix::addSubMat(int r, int c, const matrix& mat){ 00738 int x,y; 00739 x=r; 00740 for (int i = 0; i < mat.getRows() ; i++){ 00741 y=c; 00742 for (int j = 0; j < mat.getCols() ; j++){ 00743 set(x,y,get(x,y)+mat(i,j)); 00744 y++; 00745 } 00746 x++; 00747 } 00748 } 00749 00750 00751 float Ematrix::totalsum() const{ 00752 float res = 0.0f; 00753 for (int i = 0; i < rows ; i++){ 00754 for (int j = 0; j < cols ; j++){ 00755 res += fabs(get(i,j)); 00756 } 00757 } 00758 return res; 00759 }