hsm.c
Go to the documentation of this file.
00001 #include <stdlib.h>
00002 #include <math.h>
00003 #include <time.h>
00004 #include <assert.h>
00005 #include <time.h>
00006 #include "../csm_all.h"
00007 
00008 #include "hsm.h"
00009 
00010 hsm_buffer hsm_buffer_alloc(struct hsm_params*p) {
00011         assert(p->max_norm>0);
00012         assert(p->linear_cell_size>0);
00013         assert(p->angular_cell_size_deg>0);
00014         assert(p->num_angular_hypotheses >0);
00015         assert(p->linear_xc_max_npeaks>0);
00016         assert(p->xc_ndirections>0);
00017         
00018         hsm_buffer b = (hsm_buffer) malloc(sizeof(struct hsm_buffer_struct));
00019 
00020         b->num_angular_cells = (int) ceil(360.0 / p->angular_cell_size_deg);
00021         b->num_linear_cells = 1 + 2 * (int) ceil(p->max_norm / p->linear_cell_size);
00022         b->linear_cell_size = p->linear_cell_size;
00023         b->rho_min = - p->max_norm;
00024         b->rho_max = + p->max_norm;
00025 
00026         b->hs            =  (double*)  calloc((size_t)b->num_angular_cells, sizeof(double));
00027         b->hs_cross_corr =  (double*)  calloc((size_t)b->num_angular_cells, sizeof(double));
00028         b->ht            =  (double**) calloc((size_t)b->num_angular_cells, sizeof(double*));
00029 
00030         for(int i=0; i<b->num_angular_cells; i++) {
00031                 b->ht[i] = (double*) calloc((size_t)b->num_linear_cells, sizeof(double));
00032                 for(int r=0;r<b->num_linear_cells;r++)
00033                         b->ht[i][r] = 0;
00034         }
00035 
00036         b->theta = (double*) calloc((size_t)b->num_angular_cells, sizeof(double));
00037         b->sint  = (double*) calloc((size_t)b->num_angular_cells, sizeof(double));
00038         b->cost  = (double*) calloc((size_t)b->num_angular_cells, sizeof(double));
00039         for(int i=0; i<b->num_angular_cells; i++) {
00040                 b->theta[i] = (2 * M_PI * i) / b->num_angular_cells;
00041                 b->sint[i] = sin(b->theta[i]);
00042                 b->cost[i] = cos(b->theta[i]);
00043         }
00044 
00045         b->hs_cross_corr = (double*) calloc((size_t)b->num_angular_cells, sizeof(double));
00046 
00047         b->max_num_results = (int) p->num_angular_hypotheses * pow( (float) p->linear_xc_max_npeaks,  (float) p->xc_ndirections);
00048 
00049         b->num_valid_results = 0;
00050         b->results = (double**) calloc((size_t)b->max_num_results, sizeof(double*));
00051         for(int i=0;i<b->max_num_results; i++)
00052                 b->results[i] = (double*) calloc(3, sizeof(double));
00053 
00054         b->results_quality = (double*) calloc((size_t)b->max_num_results, sizeof(double));
00055 
00056         double zero[3] = {0,0,0};
00057         hsm_compute_ht_base(b, zero);
00058 
00059         return b;
00060 }
00061 
00062 void hsm_buffer_free(hsm_buffer b) {
00063         
00064         free(b->hs);
00065         for(int i=0; i<b->num_angular_cells; i++)
00066                 free(b->ht[i]);
00067         free(b->ht);
00068         
00069         free(b->theta);
00070         free(b->sint);
00071         free(b->cost);
00072         
00073         free(b->hs_cross_corr);
00074         for(int i=0;i<b->max_num_results; i++)
00075                 free(b->results[i]);
00076         free(b->results);
00077 
00078         free(b->results_quality);
00079         free(b);
00080 }
00081 
00082 void hsm_compute_ht_base(hsm_buffer b, const double base_pose[3]) {
00083         b->disp[0] = base_pose[0];
00084         b->disp[1] = base_pose[1];
00085         b->disp[2] = base_pose[2];
00086         b->disp_th_cos = cos(base_pose[2]);
00087         b->disp_th_sin = sin(base_pose[2]);
00088 }
00089 
00090 void hsm_compute_ht_point(hsm_buffer b, double x0, double y0, double weight) {
00091 
00092         double x1 = x0 * b->disp_th_cos - y0 * b->disp_th_sin + b->disp[0];
00093         double y1 = x0 * b->disp_th_sin + y0 * b->disp_th_cos + b->disp[1];
00094         
00095         for(int i=0; i<b->num_angular_cells; i++) {
00096                 double rho = x1 * b->cost[i] + y1 * b->sint[i];
00097                 int rho_index;
00098                 double alpha;
00099                 if(!hsm_rho2index(b, rho, &rho_index, &alpha)) {
00100                         continue;
00101                 }
00102 
00103                 b->ht[i][rho_index] += (1-fabs(alpha)) * weight;
00104 
00105                 if( (alpha > 0) && (rho_index < b->num_linear_cells - 1))
00106                         b->ht[i][rho_index+1] += (fabs(alpha)) * weight;
00107 
00108                 if( (alpha < 0) && (rho_index > 0))
00109                         b->ht[i][rho_index-1] += (fabs(alpha)) * weight;
00110         }
00111 }
00112 
00113 double mdax(double a, double b) {
00114         return a>b?a:b;
00115 }
00116 
00119 int hsm_rho2index(hsm_buffer b, double rho, int *rho_index, double *alpha) {
00120         *rho_index = 0; *alpha = NAN;
00121         if ( (rho <= b->rho_min) || (rho >= b->rho_max) )
00122                 return 0;
00123 
00124         /* x belongs to [0, n) */
00125         double x = b->num_linear_cells * (rho-b->rho_min) / (b->rho_max-b->rho_min);
00126         
00127         if(x==b->num_linear_cells) x*=0.99999;
00128         
00129         *rho_index = (int) floor( x );
00130         *alpha = (*rho_index+0.5)-x;
00131 
00132         assert(fabs(*alpha) <= 0.5001);
00133         assert(*rho_index >= 0);
00134         assert(*rho_index < b->num_linear_cells);
00135 
00136         return 1;
00137 }
00138 
00139 
00140 void hsm_compute_spectrum(hsm_buffer b) {
00141         for(int t=0; t<b->num_angular_cells; t++) {
00142                 b->hs[t] = 0;
00143                 for(int r=0;r<b->num_linear_cells;r++)
00144                         b->hs[t] = max(b->hs[t], b->ht[t][r]);
00145         }
00146 }
00147 
00148 void hsm_compute_spectrum_norm(hsm_buffer b) {
00149         for(int t=0; t<b->num_angular_cells; t++) {
00150                 b->hs[t] = 0;
00151                 for(int r=0;r<b->num_linear_cells;r++)
00152                         b->hs[t] += b->ht[t][r] * b->ht[t][r];
00153         }
00154 }
00155 
00156 void hsm_match(struct hsm_params*p, hsm_buffer b1, hsm_buffer b2) {
00157         sm_log_push("hsm_match");
00158         /* Let's measure the time */
00159         clock_t hsm_match_start = clock();
00160         
00161         assert(b1->num_angular_cells == b2->num_angular_cells);
00162         assert(p->max_translation > 0);
00163         assert(b1->linear_cell_size > 0);
00164 
00165         b1->num_valid_results = 0;
00166 
00167         /* Compute cross-correlation of spectra */
00168         hsm_circular_cross_corr_stupid(b1->num_angular_cells, b2->hs, b1->hs, b1->hs_cross_corr);
00169 
00170         /* Find peaks in cross-correlation */
00171         int peaks[p->num_angular_hypotheses], npeaks;
00172         hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->angular_hyp_min_distance_deg, 0, p->num_angular_hypotheses, peaks, &npeaks);
00173 
00174         sm_debug("Found %d peaks (max %d) in cross correlation.\n", npeaks, p->num_angular_hypotheses);
00175 
00176         if(npeaks == 0) {
00177                 sm_error("Cross correlation of spectra has 0 peaks.\n");
00178                 sm_log_pop();
00179                 return;
00180         }
00181 
00182         sm_log_push("loop on theta hypotheses");
00183         /* lag e' quanto 2 si sposta a destra rispetto a 1 */
00184         for(int np=0;np<npeaks;np++) {
00185                 int lag = peaks[np];
00186                 double theta_hypothesis = lag * (2*M_PI/b1->num_angular_cells);
00187 
00188                 sm_debug("Theta hyp#%d: lag %d, angle %fdeg\n", np, lag, rad2deg(theta_hypothesis));
00189 
00190                 /* Superimpose the two spectra */
00191                 double mult[b1->num_angular_cells];
00192                 for(int r=0;r<b1->num_angular_cells;r++)
00193                         mult[r] = b1->hs[r] * b2->hs[pos_mod(r-lag, b1->num_angular_cells)];
00194 
00195                 /* Find directions where both are intense */
00196                 int directions[p->xc_ndirections], ndirections;
00197                 hsm_find_peaks_circ(b1->num_angular_cells, b1->hs_cross_corr, p->xc_directions_min_distance_deg, 1, p->xc_ndirections, directions, &ndirections);
00198 
00199                 if(ndirections<2) {
00200                         sm_error("Too few directions.\n");
00201                 }
00202                 
00203                 #define MAX_NPEAKS 1024
00204                 assert(p->linear_xc_max_npeaks<MAX_NPEAKS);
00205 
00206                 struct {
00207                         /* Direction of cross correlation */
00208                         double angle;
00209                         int nhypotheses;
00210                         struct {
00211                                 double delta;
00212                                 double value;
00213                         // } hypotheses[p->linear_xc_max_npeaks];
00214                         } hypotheses[MAX_NPEAKS];
00215                 } dirs[ndirections];
00216 
00217 
00218                 sm_debug("Using %d (max %d) correlations directions.\n", ndirections, p->xc_ndirections);
00219 
00220                 int max_lag = (int) ceil(p->max_translation / b1->linear_cell_size);
00221                 int min_lag = -max_lag;
00222                 sm_debug("Max lag: %d cells (max t: %f, cell size: %f)\n",
00223                         max_lag, p->max_translation, b1->linear_cell_size);
00224 
00225                 sm_log_push("loop on xc direction");
00226                 /* For each correlation direction */
00227                 for(int cd=0;cd<ndirections;cd++) {
00228 
00229                         dirs[cd].angle =  theta_hypothesis + (directions[cd]) * (2*M_PI/b1->num_angular_cells);
00230 
00231                         printf(" cd %d angle = %d deg\n", cd, (int) rad2deg(dirs[cd].angle));
00232 
00233                         /* Do correlation */
00234                         int    lags  [2*max_lag + 1];
00235                         double xcorr [2*max_lag + 1];
00236 
00237                         int i1 = pos_mod(directions[cd]        , b1->num_angular_cells);
00238                         int i2 = pos_mod(directions[cd] + lag  , b1->num_angular_cells);
00239                         double *f1 = b1->ht[i1];
00240                         double *f2 = b2->ht[i2];
00241 
00242                         hsm_linear_cross_corr_stupid(
00243                                 b2->num_linear_cells,f2,
00244                                 b1->num_linear_cells,f1,
00245                                 xcorr, lags, min_lag, max_lag);
00246 
00247                         /* Find peaks of cross-correlation */
00248                         int linear_peaks[p->linear_xc_max_npeaks], linear_npeaks;
00249 
00250                         hsm_find_peaks_linear(
00251                                 2*max_lag + 1, xcorr, p->linear_xc_peaks_min_distance/b1->linear_cell_size,
00252                                 p->linear_xc_max_npeaks, linear_peaks, &linear_npeaks);
00253 
00254                         sm_debug("theta hyp #%d: Found %d (max %d) peaks for correlation.\n",
00255                                 cd, linear_npeaks, p->linear_xc_max_npeaks);
00256 
00257                         dirs[cd].nhypotheses = linear_npeaks;
00258                         sm_log_push("Considering each peak of linear xc");
00259                         for(int lp=0;lp<linear_npeaks;lp++) {
00260                                 int linear_xc_lag = lags[linear_peaks[lp]];
00261                                 double value = xcorr[linear_peaks[lp]];
00262                                 double linear_xc_lag_m = linear_xc_lag * b1->linear_cell_size;
00263                                 sm_debug("lag: %d  delta: %f  value: %f \n", linear_xc_lag, linear_xc_lag_m, value);
00264                                 dirs[cd].hypotheses[lp].delta = linear_xc_lag_m;
00265                                 dirs[cd].hypotheses[lp].value = value;
00266                         }
00267                         sm_log_pop();
00268                         
00269                         if(p->debug_true_x_valid) {
00270                                 double true_delta = cos(dirs[cd].angle) * p->debug_true_x[0] + 
00271                                         sin(dirs[cd].angle) * p->debug_true_x[1];
00272                                 sm_debug("true_x    delta = %f \n", true_delta );
00273                         }
00274 
00275                 } /* xc direction */
00276                 sm_log_pop();
00277 
00278                 sm_debug("Now doing all combinations. How many are there?\n");
00279                 int possible_choices[ndirections];
00280                 int num_combinations = 1;
00281                 for(int cd=0;cd<ndirections;cd++) {
00282                         possible_choices[cd] = dirs[cd].nhypotheses;
00283                         num_combinations *= dirs[cd].nhypotheses;
00284                 }
00285                 sm_debug("Total: %d combinations\n", num_combinations);
00286                 sm_log_push("For each combination..");
00287                 for(int comb=0;comb<num_combinations;comb++) {
00288                         int choices[ndirections];
00289                         hsm_generate_combinations(ndirections, possible_choices, comb, choices);
00290 
00291                         /* Linear least squares */
00292                         double M[2][2]={{0,0},{0,0}}; double Z[2]={0,0};
00293                         /* heuristic quality value */
00294                         double sum_values = 0;
00295                         for(int cd=0;cd<ndirections;cd++) {
00296                                 double angle = dirs[cd].angle;
00297                                 double c = cos(angle), s = sin(angle);
00298                                 double w = dirs[cd].hypotheses[choices[cd]].value;
00299                                 double y = dirs[cd].hypotheses[choices[cd]].delta;
00300 
00301                                 M[0][0] += c * c * w;
00302                                 M[1][0] += c * s * w;
00303                                 M[0][1] += c * s * w;
00304                                 M[1][1] += s * s * w;
00305                                 Z[0] += w * c * y;
00306                                 Z[1] += w * s * y;
00307 
00308                                 sum_values += w;
00309                         }
00310 
00311                         double det = M[0][0]*M[1][1]-M[0][1]*M[1][0];
00312                         double Minv[2][2];
00313                         Minv[0][0] = M[1][1] * (1/det);
00314                         Minv[1][1] = M[0][0] * (1/det);
00315                         Minv[0][1] = -M[0][1] * (1/det);
00316                         Minv[1][0] = -M[1][0] * (1/det);
00317 
00318                         double t[2] = {
00319                                 Minv[0][0]*Z[0] + Minv[0][1]*Z[1],
00320                                 Minv[1][0]*Z[0] + Minv[1][1]*Z[1]};
00321 
00322                         /* copy result in results slot */
00323 
00324                         int k = b1->num_valid_results;
00325                         b1->results[k][0] = t[0];
00326                         b1->results[k][1] = t[1];
00327                         b1->results[k][2] = theta_hypothesis;
00328                         b1->results_quality[k] = sum_values;
00329                         b1->num_valid_results++;
00330                 }
00331                 sm_log_pop();
00332 
00333         } /* theta hypothesis */
00334         sm_log_pop();
00335 
00336 /*      for(int i=0;i<b1->num_valid_results;i++) {
00337                 printf("#%d %.0fdeg %.1fm %.1fm  quality %f \n",i,
00338                         rad2deg(b1->results[i][2]),
00339                         b1->results[i][0],
00340                         b1->results[i][1],
00341                         b1->results_quality[i]);
00342         }*/
00343 
00344 
00345         /* Sorting based on values */
00346         int indexes[b1->num_valid_results];
00347         for(int i=0;i<b1->num_valid_results;i++)
00348                 indexes[i] = i;
00349 
00350         qsort_descending(indexes, (size_t) b1->num_valid_results, b1->results_quality);
00351 
00352         /* copy in the correct order*/
00353         double*results_tmp[b1->num_valid_results];
00354         double results_quality_tmp[b1->num_valid_results];
00355         for(int i=0;i<b1->num_valid_results;i++) {
00356                 results_tmp[i] = b1->results[i];
00357                 results_quality_tmp[i] = b1->results_quality[i];
00358         }
00359 
00360         for(int i=0;i<b1->num_valid_results;i++) {
00361                 b1->results[i] = results_tmp[indexes[i]];
00362                 b1->results_quality[i] = results_quality_tmp[indexes[i]];
00363         }
00364 
00365         for(int i=0;i<b1->num_valid_results;i++) {
00366                 char near[256]="";
00367                 double *x = b1->results[i];
00368                 if(p->debug_true_x_valid) {
00369                         double err_th = rad2deg(fabs(angleDiff(p->debug_true_x[2],x[2])));
00370                         double err_m = hypot(p->debug_true_x[0]-x[0],
00371                                 p->debug_true_x[1]-x[1]);
00372                         const char * ast = (i == 0) && (err_th > 2) ? "   ***** " : "";
00373                         sprintf(near, "th err %4d  err_m  %5f %s",(int)err_th ,err_m,ast);
00374                 }
00375                 if(i<10)
00376                 printf("after #%d %3.1fm %.1fm %3.0fdeg quality %5.0f \t%s\n",i,
00377                         x[0],
00378                         x[1], rad2deg(x[2]), b1->results_quality[i], near);
00379         }
00380         
00381         
00382         /* How long did it take? */
00383         clock_t hsm_match_stop = clock();
00384         int ticks = hsm_match_stop-hsm_match_start;
00385         double ctime = ((double)ticks) / CLOCKS_PER_SEC;
00386         sm_debug("Time: %f sec (%d ticks)\n", ctime, ticks);
00387         
00388         sm_log_pop();
00389 }
00390 
00391 
00392 void hsm_generate_combinations(int nslots, const int possible_choices[],
00393         int i, int i_choice[])
00394 {
00395         for(int slot=0;slot<nslots;slot++) {
00396                 i_choice[slot] = i % possible_choices[slot];
00397                 i = (i - i % possible_choices[slot]) / possible_choices[slot];
00398         }
00399 }
00400 
00401 void hsm_find_peaks_circ(int n, const double*f, double min_angle_deg, int unidir, int max_peaks,
00402         int*peaks, int* npeaks)
00403 {
00404         sm_log_push("hsm_find_peaks_circ");
00405 
00406         assert(max_peaks>0);
00407 
00408         /* Find all local maxima for the function */
00409         int maxima[n], nmaxima;
00410         hsm_find_local_maxima_circ(n, f, maxima, &nmaxima);
00411 
00412         sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
00413 
00414         /* Sort based on value */
00415         qsort_descending(maxima, (size_t) nmaxima, f);
00416 
00417         *npeaks = 0;
00418 
00419         sm_log_push("For each maximum");
00420         /* Only retain a subset of these */
00421         for(int m=0;m<nmaxima;m++) {
00422                 /* Here's a candidate maximum */
00423                 int candidate = maxima[m];
00424                 double candidate_angle = candidate * (2*M_PI/n);
00425                 /* Check that is not too close to the already accepted maxima */
00426                 int acceptable = 1;
00427                 for(int a=0;a<*npeaks;a++) {
00428                         int other = peaks[a];
00429                         double other_angle = other * (2*M_PI/n);
00430 
00431                         if(hsm_is_angle_between_smaller_than_deg(candidate_angle,other_angle,min_angle_deg)) {
00432                                 acceptable = 0; break;
00433                         }
00434 
00435                         /* If unidir, check also +M_PI */
00436                         if(unidir)
00437                         if(hsm_is_angle_between_smaller_than_deg(candidate_angle+M_PI,other_angle,min_angle_deg)) {
00438                                 acceptable = 0; break;
00439                         }
00440 
00441                 }
00442 
00443                 sm_debug("%saccepting candidate %d; lag = %d value = %f\n",
00444                         acceptable?"":"not ", m, maxima[m], f[maxima[m]]);
00445 
00446                 if(acceptable) {
00447                         peaks[*npeaks] = candidate;
00448                         (*npeaks) ++;
00449                 }
00450 
00451                 if(*npeaks>=max_peaks) break;
00452         }
00453         sm_log_pop();
00454 
00455         sm_debug("found %d (max %d) maxima.\n", *npeaks, max_peaks);
00456         sm_log_pop();
00457 }
00458 
00459 
00460 void hsm_find_peaks_linear(int n, const double*f, double min_dist, int max_peaks,
00461         int*peaks, int* npeaks)
00462 {
00463         sm_log_push("hsm_find_peaks_linear");
00464 
00465         assert(max_peaks>0);
00466 
00467         /* Find all local maxima for the function */
00468         int maxima[n], nmaxima;
00469         hsm_find_local_maxima_linear(n,f,maxima,&nmaxima);
00470 
00471         sm_debug("Found %d of %d are local maxima.\n", nmaxima, n);
00472 
00473         /* Sort based on value */
00474         qsort_descending(maxima, (size_t) nmaxima,  f);
00475 
00476         *npeaks = 0;
00477         sm_log_push("for each maximum");
00478         /* Only retain a subset of these */
00479         for(int m=0;m<nmaxima;m++) {
00480                 /* Here's a candidate maximum */
00481                 int candidate = maxima[m];
00482                 /* Check that is not too close to the already accepted maxima */
00483                 int acceptable = 1;
00484                 for(int a=0;a<*npeaks;a++) {
00485                         int other = peaks[a];
00486 
00487                         if(abs(other-candidate) < min_dist) {
00488                                 acceptable = 0; break;
00489                         }
00490                 }
00491 
00492                 sm_debug("%s accepting candidate %d; lag = %d value = %f\n",
00493                         acceptable?"":"not", m, maxima[m], f[maxima[m]]);
00494 
00495                 if(acceptable) {
00496                         peaks[*npeaks] = candidate;
00497                         (*npeaks) ++;
00498                 }
00499 
00500                 if(*npeaks >= max_peaks) break;
00501         }
00502         sm_log_pop("");
00503         sm_debug("Found %d (max %d) maxima.\n", *npeaks, max_peaks);
00504 
00505         sm_log_pop();
00506 }
00507 
00508 
00509 int hsm_is_angle_between_smaller_than_deg(double angle1, double angle2, double threshold_deg) {
00510         double dot = cos(angle1)*cos(angle2) + sin(angle1)*sin(angle2);
00511         return (dot > cos(threshold_deg * M_PI/180));
00512 }
00513 
00514 void hsm_find_local_maxima_circ(int n, const double*f, int*maxima, int*nmaxima) {
00515         *nmaxima = 0;
00516         for(int i=0;i<n;i++) {
00517                 double val = f[i];
00518                 double left  = f[ pos_mod(i-1,n) ];
00519                 double right = f[ pos_mod(i+1,n) ];
00520                 if( (val>0) && (val>left) && (val>right))
00521                         maxima[(*nmaxima)++] = i;
00522         }
00523 }
00524 
00525 
00526 void hsm_find_local_maxima_linear(int n, const double*f, int*maxima, int*nmaxima) {
00527         *nmaxima = 0;
00528         for(int i=1;i<n-1;i++) {
00529                 double val = f[i];
00530                 double left = f[i-1];
00531                 double right = f[i+1];
00532                 if( (val>0) && (val>left) && (val>right))
00533                         maxima[(*nmaxima)++] = i;
00534         }
00535 }
00536 
00537 
00538 
00539 void hsm_circular_cross_corr_stupid(int n, const double *a, const double *b, double*res) {
00540         /* Two copies of f1 */
00541         double aa[2*n];
00542         for(int i=0;i<2*n;i++) aa[i] = a[i%n];
00543         for(int lag=0;lag<n;lag++) {
00544                 res[lag] = 0;
00545                 for(int j=0;j<n;j++)
00546                         res[lag] += b[j] * aa[j+lag];
00547         }
00548 }
00549 
00550 
00551 void hsm_linear_cross_corr_stupid(int na, const double *a, int nb, const double *b, double*res, int*lags, int min_lag, int max_lag) {
00552         assert(a); 
00553         assert(b);
00554         assert(res);
00555         assert(lags);
00556         
00557         for(int l=min_lag;l<=max_lag;l++) {
00558                 lags[l-min_lag] = l;
00559                 
00560                 double r = 0;
00561                 for(int j=0; (j<nb) && (j+l<na);j++) {
00562                         // j + l >= 0
00563                         if(j+l>=0)
00564                         r += b[j] * a[j+l];
00565                 }
00566         
00567                 res[l-min_lag] = r;
00568 
00569         }
00570 }
00571 
00572 
00573 const double *qsort_descending_values = 0;
00574 
00575 int compare_descending(const void *index_pt1, const void *index_pt2) {
00576         int i1 = *( (const int*) index_pt1);
00577         int i2 = *( (const int*) index_pt2);
00578         const double * f = qsort_descending_values;
00579         return f[i1] < f[i2] ? +1 : f[i1] == f[i2] ? 0 : -1;
00580 }
00581 
00582 void qsort_descending(int *indexes, size_t nmemb, const double*values)
00583 {
00584         qsort_descending_values = values;
00585         qsort(indexes, nmemb, sizeof(int), compare_descending);
00586 }
00587 
00588 
00589 
00591 int pos_mod(int a, int b) {
00592         return ((a%b)+b)%b;
00593 }
00594 
00595 
00596 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42