00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031
00032 #include "fors_polynomial.h"
00033
00034 #include "fors_utils.h"
00035
00036 #include <cpl.h>
00037 #include <math.h>
00038 #include <string.h>
00039
00040
00044
00045
00047
00048
00049
00050
00051 static bool
00052 fors_polynomial_is_coeff_set( const cpl_polynomial *p,
00053 const int *powers);
00054
00055 static bool
00056 fors_polynomial_powers_next( const cpl_polynomial *p,
00057 int *powers);
00058
00059
00060
00061
00062
00063
00064 #undef cleanup
00065 #define cleanup
00066
00072
00073 static bool
00074 fors_polynomial_is_coeff_set( const cpl_polynomial *p,
00075 const int *powers)
00076 {
00077 cassure_automsg( p != NULL,
00078 CPL_ERROR_NULL_INPUT,
00079 return false);
00080 cassure_automsg( powers != NULL,
00081 CPL_ERROR_NULL_INPUT,
00082 return false);
00083
00084 return (fabs(cpl_polynomial_get_coeff(p, powers)) > DBL_EPSILON);
00085 }
00086
00087
00088 #undef cleanup
00089 #define cleanup
00090
00096
00097 static bool
00098 fors_polynomial_powers_next( const cpl_polynomial *p,
00099 int *powers)
00100 {
00101 int dim = 0,
00102 ndims,
00103 ndegs;
00104 bool overflow = false;
00105 cpl_errorstate errstat = cpl_errorstate_get();
00106
00107 cassure_automsg( p != NULL,
00108 CPL_ERROR_NULL_INPUT,
00109 return true);
00110 cassure_automsg( powers != NULL,
00111 CPL_ERROR_NULL_INPUT,
00112 return true);
00113
00114 ndims = cpl_polynomial_get_dimension(p);
00115 ndegs = cpl_polynomial_get_degree(p);
00116 passure(cpl_errorstate_is_equal(errstat), return true);
00117
00118 powers[dim]++;
00119 while(dim < ndims && powers[dim] > ndegs)
00120 {
00121 powers[dim] = 0;
00122 dim++;
00123 overflow = (!(dim < ndims));
00124 if (!overflow)
00125 powers[dim]++;
00126 }
00127
00128 return overflow;
00129 }
00130
00131
00132
00133
00134
00135
00136 #undef cleanup
00137 #define cleanup \
00138 do { \
00139 if (pows != NULL) \
00140 cpl_free(pows); \
00141 pows = NULL; \
00142 } while (0)
00143
00148
00149 int
00150 fors_polynomial_count_coeff( const cpl_polynomial *p)
00151 {
00152 int ndims,
00153 Ncoeff = 0;
00154 int *pows = NULL;
00155 bool overflow = false;
00156 cpl_errorstate errstat = cpl_errorstate_get();
00157
00158 cassure_automsg( p != NULL,
00159 CPL_ERROR_NULL_INPUT,
00160 return 0);
00161
00162 ndims = cpl_polynomial_get_dimension(p);
00163 passure(cpl_errorstate_is_equal(errstat), return 0);
00164
00165 pows = cpl_calloc(ndims, sizeof(*pows));
00166
00167 while (!overflow)
00168 {
00169 if (fors_polynomial_is_coeff_set(p, pows))
00170 Ncoeff++;
00171
00172 overflow = fors_polynomial_powers_next(p, pows);
00173 }
00174
00175 passure(cpl_errorstate_is_equal(errstat), return 0);
00176 cleanup;
00177
00178 return Ncoeff;
00179 }
00180
00181
00182 #undef cleanup
00183 #define cleanup
00184
00190
00191 int
00192 fors_polynomial_powers_find_first_coeff( const cpl_polynomial *p,
00193 int *powers)
00194 {
00195 int ndims,
00196 dim;
00197 bool overflow;
00198 cpl_errorstate errstat = cpl_errorstate_get();
00199
00200 cassure_automsg( p != NULL,
00201 CPL_ERROR_NULL_INPUT,
00202 return true);
00203 cassure_automsg( powers != NULL,
00204 CPL_ERROR_NULL_INPUT,
00205 return true);
00206
00207 ndims = cpl_polynomial_get_dimension(p);
00208 for (dim = 0; dim < ndims; dim++)
00209 powers[dim] = 0;
00210
00211 if (fors_polynomial_is_coeff_set(p, powers))
00212 return false;
00213
00214 overflow = fors_polynomial_powers_find_next_coeff(p, powers);
00215 passure(cpl_errorstate_is_equal(errstat), return true);
00216 cleanup;
00217
00218 return overflow;
00219 }
00220
00221
00222 #undef cleanup
00223 #define cleanup
00224
00230
00231 int
00232 fors_polynomial_powers_find_next_coeff( const cpl_polynomial *p,
00233 int *powers)
00234 {
00235 bool overflow;
00236 cpl_errorstate errstat = cpl_errorstate_get();
00237
00238 cassure_automsg( p != NULL,
00239 CPL_ERROR_NULL_INPUT,
00240 return true);
00241 cassure_automsg( powers != NULL,
00242 CPL_ERROR_NULL_INPUT,
00243 return true);
00244
00245 overflow = fors_polynomial_powers_next(p, powers);
00246 while (!overflow)
00247 {
00248 if (fors_polynomial_is_coeff_set(p, powers))
00249 break;
00250
00251 overflow = fors_polynomial_powers_next(p, powers);
00252 }
00253
00254 passure(cpl_errorstate_is_equal(errstat), return true);
00255 cleanup;
00256
00257 return overflow;
00258 }
00259
00260
00261 #undef cleanup
00262 #define cleanup \
00263 do { \
00264 if (pows != NULL) \
00265 cpl_free(pows); \
00266 pows = NULL; \
00267 } while (0)
00268
00275
00276 cpl_error_code
00277 fors_polynomial_set_existing_coeff( cpl_polynomial *p,
00278 const double *coeffs,
00279 int n_coeffs)
00280 {
00281 int n,
00282 ndims;
00283 int *pows = NULL;
00284 bool overflow = false;
00285 cpl_errorstate errstat = cpl_errorstate_get();
00286
00287 if (p == NULL)
00288 return 0;
00289
00290
00291 cassure_automsg( p != NULL,
00292 CPL_ERROR_NULL_INPUT,
00293 return cpl_error_get_code());
00294 cassure_automsg( coeffs != NULL,
00295 CPL_ERROR_NULL_INPUT,
00296 return cpl_error_get_code());
00297 cassure_automsg( n_coeffs > 0,
00298 CPL_ERROR_ILLEGAL_INPUT,
00299 return cpl_error_get_code());
00300
00301
00302 ndims = cpl_polynomial_get_dimension(p);
00303 pows = cpl_calloc(ndims, sizeof(*pows));
00304
00305 overflow = fors_polynomial_powers_find_first_coeff(p, pows);
00306 n = 0;
00307 while (!overflow)
00308 {
00309 cassure( n < n_coeffs,
00310 CPL_ERROR_ACCESS_OUT_OF_RANGE,
00311 return cpl_error_get_code(),
00312 "p contains more coefficients "
00313 "than coeffs");
00314 cpl_polynomial_set_coeff(p, pows, coeffs[n]);
00315 n++;
00316
00317 overflow = fors_polynomial_powers_find_next_coeff(p, pows);
00318 }
00319
00320 cleanup;
00321
00322 return cpl_errorstate_is_equal(errstat) ?
00323 CPL_ERROR_NONE :
00324 cpl_error_get_code();
00325 }
00326
00327
00328 #undef cleanup
00329 #define cleanup \
00330 do { \
00331 if (powersA != NULL) { cpl_free(powersA); powersA = NULL;} \
00332 if (powersB != NULL) { cpl_free(powersB); powersB = NULL;} \
00333 if (powersE != NULL) { cpl_free(powersE); powersE = NULL;} \
00334 cpl_polynomial_delete(ep); ep = NULL; \
00335 } while (0)
00336
00354
00355 cpl_polynomial*
00356 fors_polynomial_create_variance_polynomial( const cpl_polynomial *p_def,
00357 const cpl_matrix *cov_coeff)
00358 {
00359 int n_dims,
00360 n_coeffs,
00361 n_col,
00362 ia = 0;
00363 int *powersA = NULL,
00364 *powersB = NULL,
00365 *powersE = NULL;
00366 cpl_polynomial *ep = NULL;
00367 bool overflowa = false;
00368 cpl_errorstate errstat = cpl_errorstate_get();
00369
00370
00371 cassure_automsg( p_def != NULL,
00372 CPL_ERROR_NULL_INPUT,
00373 return ep);
00374 cassure_automsg( cov_coeff != NULL,
00375 CPL_ERROR_NULL_INPUT,
00376 return ep);
00377
00378 n_coeffs = fors_polynomial_count_coeff(p_def);
00379 n_dims = cpl_polynomial_get_dimension(p_def);
00380 n_col = cpl_matrix_get_ncol(cov_coeff);
00381 passure(cpl_errorstate_is_equal(errstat), return ep);
00382
00383 cassure( n_coeffs == n_col,
00384 CPL_ERROR_INCOMPATIBLE_INPUT,
00385 return ep,
00386 "number of p_def coefficients "
00387 "!= nr of columns");
00388 cassure( n_col == cpl_matrix_get_nrow(
00389 cov_coeff),
00390 CPL_ERROR_ILLEGAL_INPUT,
00391 return ep,
00392 "cov_coeff is not square");
00393
00394 ep = cpl_polynomial_new(n_dims);
00395 powersA = cpl_calloc(n_dims, sizeof(*powersA));
00396 powersB = cpl_calloc(n_dims, sizeof(*powersB));
00397 powersE = cpl_calloc(n_dims, sizeof(*powersE));
00398 passure(cpl_errorstate_is_equal(errstat), return ep);
00399
00400 overflowa = fors_polynomial_powers_find_first_coeff(p_def, powersA);
00401 while (!overflowa)
00402 {
00403 bool overflowb = false;
00404 int ib = 0;
00405
00406 overflowb = fors_polynomial_powers_find_first_coeff(p_def, powersB);
00407 while (!overflowb)
00408 {
00409 double coeff;
00410 int d;
00411 passure(cpl_errorstate_is_equal(errstat), return ep);
00412
00413
00414
00415
00416
00417
00418
00419
00420 for (d = 0; d < n_dims; d++)
00421 powersE[d] = powersA[d] + powersB[d];
00422
00423 coeff = cpl_polynomial_get_coeff(ep, powersE);
00424 cpl_polynomial_set_coeff(ep, powersE,
00425 coeff
00426 + cpl_matrix_get(
00427 cov_coeff, ia, ib));
00428
00429 ib++;
00430 overflowb = fors_polynomial_powers_find_next_coeff(p_def, powersB);
00431 }
00432 ia++;
00433 overflowa = fors_polynomial_powers_find_next_coeff(p_def, powersA);
00434 }
00435 passure(cpl_errorstate_is_equal(errstat), return ep);
00436
00437 cpl_polynomial *ret_ep = ep; ep = NULL;
00438 cleanup;
00439
00440 return ret_ep;
00441 }
00442
00443
00444 #undef cleanup
00445 #define cleanup \
00446 do { \
00447 if (pows != NULL) \
00448 cpl_free(pows); \
00449 pows = NULL; \
00450 if (ndxstr != NULL) \
00451 cpl_free(ndxstr); \
00452 ndxstr = NULL; \
00453 } while (0)
00454
00463
00464 cpl_error_code
00465 fors_polynomial_dump( const cpl_polynomial *p,
00466 const char *name,
00467 cpl_msg_severity level,
00468 const cpl_polynomial *p_def)
00469 {
00470 int ndims,
00471 ndegs,
00472 dim;
00473 int *pows = NULL;
00474 char *ndxstr = NULL;
00475 char max_ndx_str[15];
00476 cpl_errorstate errstat = cpl_errorstate_get();
00477
00478
00479 cassure_automsg( p != NULL,
00480 CPL_ERROR_NULL_INPUT,
00481 return cpl_error_get_code());
00482
00483 if (p_def != NULL)
00484 {
00485 ndims = cpl_polynomial_get_dimension(p_def);
00486 cassure_automsg( ndims
00487 = cpl_polynomial_get_dimension(p),
00488 CPL_ERROR_INCOMPATIBLE_INPUT,
00489 return cpl_error_get_code());
00490 }
00491 else
00492 {
00493 ndims = cpl_polynomial_get_dimension(p);
00494 p_def = p;
00495 }
00496
00497 ndegs = cpl_polynomial_get_degree(p_def);
00498
00499 pows = cpl_calloc(ndims, sizeof(*pows));
00500
00501 sprintf(max_ndx_str, "%d", ndegs);
00502 ndxstr = cpl_calloc(ndims*(strlen(max_ndx_str)+1), sizeof(*ndxstr));
00503
00504
00505
00506
00507
00508 while (pows[0] <= ndegs)
00509 {
00510 if (fors_polynomial_is_coeff_set(p_def, pows))
00511 {
00512 double coeff;
00513 coeff = cpl_polynomial_get_coeff(p, pows);
00514
00515 sprintf(ndxstr, "%d", pows[0]);
00516 for (dim = 1; dim < ndims; dim++)
00517 sprintf(ndxstr+strlen(ndxstr), ",%d", pows[dim]);
00518 fors_msg( level,
00519 "%s_%s = %e",
00520 (name != NULL) ? name : "p",
00521 ndxstr,
00522 coeff);
00523 }
00524
00525 dim = ndims-1;
00526 pows[dim]++;
00527 while(dim > 0 && pows[dim] > ndegs)
00528 {
00529 pows[dim] = 0;
00530 dim--;
00531 pows[dim]++;
00532 }
00533 }
00534
00535 cleanup;
00536
00537 return cpl_errorstate_is_equal(errstat) ?
00538 CPL_ERROR_NONE :
00539 cpl_error_get_code();
00540 }
00541
00542
00543 #undef cleanup
00544 #define cleanup \
00545 do { \
00546 if (ndxstr != NULL) \
00547 cpl_free(ndxstr); \
00548 ndxstr = NULL; \
00549 } while (0)
00550
00561
00562 char*
00563 fors_polynomial_sprint_coeff( const cpl_polynomial *p,
00564 int *powers,
00565 const char *prefix)
00566 {
00567 int ndims,
00568 ndegs,
00569 dim,
00570 max_nr,
00571 max_nr_strlen;
00572 char *ndxstr = NULL;
00573 char max_ndx_str[15];
00574
00575
00576 cassure_automsg( p != NULL,
00577 CPL_ERROR_NULL_INPUT,
00578 return ndxstr);
00579 cassure_automsg( powers != NULL,
00580 CPL_ERROR_NULL_INPUT,
00581 return ndxstr);
00582
00583 ndims = cpl_polynomial_get_dimension(p);
00584 ndegs = cpl_polynomial_get_degree(p);
00585
00586
00587
00588 max_nr = ndegs;
00589 for (dim = 0; dim < ndims; dim++)
00590 if (powers[dim] > max_nr)
00591 max_nr = powers[dim];
00592
00593 sprintf(max_ndx_str, "%d", max_nr);
00594 max_nr_strlen = strlen(max_ndx_str);
00595 if (prefix != NULL && prefix[0] != '\0')
00596 {
00597 int len = strlen(prefix) + 1 + ndims*(max_nr_strlen+1);
00598 ndxstr = cpl_calloc(len, sizeof(*ndxstr));
00599 sprintf(ndxstr, "%s_", prefix);
00600 }
00601 else
00602 {
00603 ndxstr = cpl_calloc(ndims*(max_nr_strlen+1), sizeof(*ndxstr));
00604 }
00605
00606 snprintf(ndxstr+strlen(ndxstr), max_nr_strlen+1,
00607 (powers[0] >= 0) ? "%d" : "?",
00608 powers[0]);
00609 for (dim = 1; dim < ndims; dim++)
00610 snprintf(ndxstr+strlen(ndxstr), max_nr_strlen+2,
00611 (powers[dim] >= 0) ? ",%d" : "?",
00612 powers[dim]);
00613
00614 return ndxstr;
00615 }
00616