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
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 #define COLUMN_ORDER1 "Order1"
00063 #define COLUMN_ORDER2 "Order2"
00064 #define COLUMN_COEFF "Coeff"
00065
00067
00068
00069
00070 #include <xsh_utils_polynomial.h>
00071
00072 #include <xsh_utils.h>
00073
00074 #include <xsh_dump.h>
00075 #include <xsh_msg.h>
00076 #include <xsh_error.h>
00077
00078 #include <cpl.h>
00079
00080
00081
00082
00085 struct _polynomial
00086 {
00088 cpl_polynomial *pol;
00089
00091 cpl_vector *vec;
00092 double *vec_data;
00093
00094 int dimension;
00095
00097 double *shift;
00098
00100 double *scale;
00101 };
00102
00103
00104
00105
00106 cpl_matrix * xsh_matrix_product_normal_create(const cpl_matrix * self)
00107 {
00108
00109 long double sum;
00110 cpl_matrix * product;
00111 const double * ai = cpl_matrix_get_data_const(self);
00112 const double * aj;
00113 double * bwrite;
00114 const cpl_size m = cpl_matrix_get_nrow(self);
00115 const cpl_size n = cpl_matrix_get_ncol(self);
00116 cpl_size i, j, k;
00117
00118
00119 cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT, NULL);
00120
00121 #if 0
00122
00123
00124
00125 product = cpl_matrix_new(m, m);
00126 bwrite = cpl_matrix_get_data(product);
00127 #else
00128 bwrite = (double *) cpl_malloc((size_t)m * (size_t)m * sizeof(double));
00129 product = cpl_matrix_wrap(m, m, bwrite);
00130 #endif
00131
00132
00133 for (i = 0; i < m; i++, bwrite += m, ai += n) {
00134 aj = ai;
00135 for (j = i; j < m; j++, aj += n) {
00136 sum = 0.0;
00137 for (k = 0; k < n; k++) {
00138 sum += (long double)ai[k] * (long double)aj[k];
00139 }
00140 bwrite[j] = sum;
00141 }
00142 }
00143
00144
00145
00146 return product;
00147
00148 }
00149
00150
00151
00152 cpl_matrix *xsh_matrix_solve_normal(const cpl_matrix *coeff,
00153 const cpl_matrix *rhs)
00154 {
00155
00156 cpl_matrix * solution;
00157 cpl_matrix * At;
00158 cpl_matrix * AtA;
00159
00160 cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL);
00161 cpl_ensure(rhs != NULL, CPL_ERROR_NULL_INPUT, NULL);
00162
00163
00164 At = cpl_matrix_transpose_create(coeff);
00165 solution = cpl_matrix_product_create(At, rhs);
00166
00167 AtA = xsh_matrix_product_normal_create(At);
00168
00169 cpl_matrix_delete(At);
00170
00171
00172
00173
00174
00175 if (cpl_matrix_decomp_chol(AtA) != CPL_ERROR_NONE ||
00176 cpl_matrix_solve_chol(AtA, solution) != CPL_ERROR_NONE) {
00177 cpl_matrix_delete(solution);
00178 solution = NULL;
00179
00180 (void)cpl_error_set_where(cpl_func);
00181 }
00182
00183 cpl_matrix_delete(AtA);
00184
00185 return solution;
00186
00187 }
00188
00189
00190
00191
00192
00193
00194
00205
00206 polynomial *
00207 xsh_polynomial_new(const cpl_polynomial *pol)
00208 {
00209 polynomial *p = NULL;
00210 int i;
00211
00212
00213 assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00214
00215
00216 p = cpl_calloc(1, sizeof(polynomial)) ;
00217 assure_mem( p );
00218
00219 check_msg( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");
00220
00221
00222 p->vec = cpl_vector_new(p->dimension);
00223 assure_mem( p->vec );
00224 p->vec_data = cpl_vector_get_data(p->vec);
00225
00226
00227 p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
00228 assure_mem( p->shift );
00229
00230 p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
00231 assure_mem( p->scale );
00232 for (i = 0; i <= p->dimension; i++)
00233 p->scale[i] = 1.0;
00234
00235 check_msg( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
00236
00237 cleanup:
00238 if (cpl_error_get_code() != CPL_ERROR_NONE)
00239 xsh_polynomial_delete(&p);
00240
00241 return p;
00242 }
00243
00244
00252
00253 polynomial *
00254 xsh_polynomial_new_zero(int dim)
00255 {
00256 polynomial *result = NULL;
00257 cpl_polynomial *p = NULL;
00258
00259 assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);
00260
00261 p = cpl_polynomial_new(dim);
00262 assure_mem( p );
00263
00264 result = xsh_polynomial_new(p);
00265 assure_mem( result );
00266
00267 cleanup:
00268 xsh_free_polynomial(&p);
00269
00270 return result;
00271 }
00272
00273
00280
00281 void
00282 xsh_polynomial_delete(polynomial **p)
00283 {
00284 xsh_polynomial_delete_const((const polynomial **)p);
00285 }
00286
00287
00294
00295 void
00296 xsh_polynomial_delete_const(const polynomial **p)
00297 {
00298 if (*p == NULL) return;
00299 cpl_polynomial_delete((*p)->pol);
00300 cpl_vector_delete((*p)->vec);
00301 cpl_free((*p)->shift);
00302 cpl_free((*p)->scale);
00303 xsh_free(*p);
00304 *p = NULL;
00305 return;
00306 }
00307
00313
00314 int
00315 xsh_polynomial_get_degree(const polynomial *p)
00316 {
00317 int result = -1;
00318 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00319
00320 result = cpl_polynomial_get_degree(p->pol);
00321
00322 cleanup:
00323 return result;
00324 }
00325
00326
00332
00333 polynomial *
00334 xsh_polynomial_duplicate(const polynomial *p)
00335 {
00336 polynomial *result = NULL;
00337 int dimension;
00338 int i;
00339
00340 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00341 dimension = xsh_polynomial_get_dimension(p);
00342
00343 check_msg( result = xsh_polynomial_new(p->pol),
00344 "Error allocating polynomial");
00345
00346 for (i = 0; i <= dimension; i++)
00347 {
00348 result->shift[i] = p->shift[i];
00349 result->scale[i] = p->scale[i];
00350 }
00351
00352 cleanup:
00353 if (cpl_error_get_code() != CPL_ERROR_NONE)
00354 {
00355 xsh_polynomial_delete(&result);
00356 return NULL;
00357 }
00358
00359 return result;
00360 }
00361
00362
00363
00374
00375 cpl_table *
00376 xsh_polynomial_convert_to_table(const polynomial *p)
00377 {
00378 cpl_table *t = NULL;
00379 int degree;
00380 int i, j, row;
00381
00382
00383 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00384 assure( xsh_polynomial_get_dimension(p) == 2,
00385 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
00386
00387 degree = cpl_polynomial_get_degree(p->pol);
00388
00389
00390
00391 t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
00392 cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
00393 cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
00394 cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);
00395
00396 row = 0;
00397
00398
00399 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00400 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00401 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;
00402
00403 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00404 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00405 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;
00406
00407 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00408 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00409 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;
00410
00411
00412 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00413 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00414 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[0]); row++;
00415
00416 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00417 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00418 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[1]); row++;
00419
00420 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00421 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00422 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;
00423
00424
00425 for (i = 0; i <= degree; i++){
00426 for (j = 0; j+i <= degree; j++){
00427 double coeff;
00428 cpl_size power[2];
00429 power[0] = i;
00430 power[1] = j;
00431
00432 coeff = cpl_polynomial_get_coeff(p->pol, power);
00433 cpl_table_set_int (t, COLUMN_ORDER1, row, power[0]);
00434 cpl_table_set_int (t, COLUMN_ORDER2, row, power[1]);
00435 cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
00436
00437 row++;
00438 }
00439 }
00440
00441 cleanup:
00442 return t;
00443 }
00444
00445
00454
00455 polynomial *
00456 xsh_polynomial_convert_from_table(cpl_table *t)
00457 {
00458 polynomial *p = NULL;
00459 cpl_polynomial *pol = NULL;
00460 cpl_type type;
00461 int i;
00462
00463
00464 check_msg( pol = cpl_polynomial_new(2), "Error initializing polynomial");
00465
00466
00467 assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
00468 assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT,
00469 "No '%s' column found in table", COLUMN_ORDER1);
00470 assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
00471 "No '%s' column found in table", COLUMN_ORDER2);
00472 assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
00473 "No '%s' column found in table", COLUMN_COEFF );
00474
00475 type = cpl_table_get_column_type(t, COLUMN_ORDER1);
00476 assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
00477 "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
00478 xsh_tostring_cpl_type(type));
00479
00480 type = cpl_table_get_column_type(t, COLUMN_ORDER2);
00481 assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
00482 "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
00483 xsh_tostring_cpl_type(type));
00484
00485 type = cpl_table_get_column_type(t, COLUMN_COEFF);
00486 assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
00487 "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
00488 xsh_tostring_cpl_type(type));
00489
00490 assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
00491 "Table must contain at least one coefficient");
00492
00493
00494 for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
00495 double coeff;
00496 cpl_size power[2];
00497
00498 check_msg(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
00499 power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
00500 coeff = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
00501 "Error reading table row %d", i);
00502
00503 xsh_msg_debug("Pol.coeff.(%" CPL_SIZE_FORMAT ", %" CPL_SIZE_FORMAT ") = %e", power[0], power[1], coeff);
00504
00505 check_msg( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
00506 }
00507 p = xsh_polynomial_new(pol);
00508
00509
00510 xsh_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
00511 xsh_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
00512 xsh_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
00513 xsh_polynomial_shift (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
00514 xsh_polynomial_shift (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
00515 xsh_polynomial_shift (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
00516
00517 cleanup:
00518 xsh_free_polynomial(&pol);
00519 if (cpl_error_get_code() != CPL_ERROR_NONE)
00520 xsh_polynomial_delete(&p);
00521
00522 return p;
00523 }
00524
00525
00526
00532
00533 int
00534 xsh_polynomial_get_dimension(const polynomial *p)
00535 {
00536 int dim = -1;
00537 assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00538
00539
00540 dim = p->dimension;
00541
00542 cleanup:
00543 return dim;
00544 }
00545
00546
00554
00555 void xsh_polynomial_dump(const polynomial *p, FILE *stream)
00556 {
00557 if (p == NULL)
00558 fprintf(stream, "Null polynomial\n");
00559 else {
00560 int i;
00561 cpl_polynomial_dump(p->pol, stream);
00562 fprintf(stream, "shift_y \t= %f \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
00563 for (i = 1; i <= xsh_polynomial_get_dimension(p); i++)
00564 {
00565 fprintf(stream, "shift_x%d \t= %f \tscale_x%d \t= %f\n",
00566 i, p->shift[i], i, p->scale[i]);
00567 }
00568 }
00569 return;
00570 }
00571
00572
00586
00587 cpl_error_code
00588 xsh_polynomial_rescale(polynomial *p, int varno, double scale)
00589 {
00590 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00591 assure(0 <= varno && varno <= xsh_polynomial_get_dimension(p),
00592 CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606 p->shift[varno] *= scale;
00607 p->scale[varno] *= scale;
00608
00609 cleanup:
00610 return cpl_error_get_code();
00611 }
00612
00613
00627
00628 cpl_error_code
00629 xsh_polynomial_shift(polynomial *p, int varno, double shift)
00630 {
00631 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00632 assure(0 <= varno && varno <= xsh_polynomial_get_dimension(p),
00633 CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644 p->shift[varno] += shift;
00645
00646 cleanup:
00647 return cpl_error_get_code();
00648 }
00649
00650
00659
00660 double
00661 xsh_polynomial_evaluate_1d(const polynomial *p, double x)
00662 {
00663 double result = 0;
00664
00665 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00666 assure(xsh_polynomial_get_dimension(p) == 1,
00667 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00668
00669 check_msg( result =
00670 cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
00671 * p->scale[0] + p->shift[0],
00672 "Could not evaluate polynomial");
00673
00674 cleanup:
00675 return result;
00676 }
00677
00678
00679
00689
00690
00691 double
00692 xsh_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
00693 {
00694 double result = 0;
00695
00696 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00697 assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
00698 "Polynomial must be 2d. It's %dd", p->dimension);
00699 {
00700 double scale = p->scale[0];
00701 double shift = p->shift[0];
00702
00703
00704
00705 p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
00706 p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
00707
00708 result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
00709 }
00710
00711 cleanup:
00712 return result;
00713 }
00714
00715
00728
00729 double
00730 xsh_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
00731 {
00732 double result = 0;
00733 cpl_size power[1];
00734 double coeff0;
00735
00736 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00737 assure(xsh_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT,
00738 "Polynomial must be 1d");
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748 power[0] = 0;
00749 check_msg(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
00750 cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
00751 "Error setting coefficient");
00752
00753 check_msg( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
00754 &result, multiplicity), "Could not find root");
00755
00756 cpl_polynomial_set_coeff(p->pol, power, coeff0);
00757
00758
00759 result = result * p->scale[1] + p->shift[1];
00760
00761 cleanup:
00762 return result;
00763 }
00764
00765
00782
00783 double
00784 xsh_polynomial_solve_2d(const polynomial *p, double value, double guess,
00785 int multiplicity, int varno, double x_value)
00786 {
00787 double result = 0;
00788 polynomial *pol_1d = NULL;
00789
00790 assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00791 "Illegal variable number: %d", varno);
00792
00793 check_msg( pol_1d = xsh_polynomial_collapse(p, varno, x_value),
00794 "Could not collapse polynomial");
00795
00796 check_msg( result = xsh_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
00797 "Could not find root");
00798
00799 cleanup:
00800 xsh_polynomial_delete(&pol_1d);
00801 return result;
00802 }
00803
00804
00813
00814 double
00815 xsh_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
00816 {
00817 double result = 0;
00818 cpl_size power[2];
00819
00820 assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00821 "Illegal variable number (%d)", varno);
00822
00823 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00824 assure(xsh_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
00825 "Polynomial must be 2d. It's %dd", xsh_polynomial_get_dimension(p));
00826
00827
00828
00829
00830
00831
00832
00833 x1 = (x1 - p->shift[1])/p->scale[1];
00834 x2 = (x2 - p->shift[2])/p->scale[2];
00835
00836
00837
00838
00839 {
00840 int degree = cpl_polynomial_get_degree(p->pol);
00841 double yj = 1;
00842 int i, j;
00843
00844 result = 0;
00845 for (j = 0, yj = 1;
00846 j <= degree; j++,
00847 yj *= (varno == 1) ? x2 : x1)
00848 {
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859 double sum = 0;
00860 for (i = degree; i >= 1; i--)
00861 {
00862 double c_ij;
00863
00864 power[0] = (varno == 1) ? i : j;
00865 power[1] = (varno == 1) ? j : i;
00866
00867 c_ij = cpl_polynomial_get_coeff(p->pol, power);
00868
00869 sum += (i * c_ij);
00870 if (i >= 2) sum *= (varno == 1) ? x1 : x2;
00871 }
00872
00873
00874 result += yj * sum;
00875 }
00876 }
00877
00878 result *= p->scale[0];
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891 cleanup:
00892 return result;
00893 }
00894
00895
00902
00903 double
00904 xsh_polynomial_derivative_1d(const polynomial *p, double x)
00905 {
00906 double result = 0;
00907 double dummy;
00908
00909 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00910 assure(xsh_polynomial_get_dimension(p) == 1,
00911 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00912
00913 check_msg( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
00914 "Error evaluating derivative");
00915
00916 cleanup:
00917 return result;
00918 }
00919
00920
00927
00928 polynomial *
00929 xsh_polynomial_add_2d(const polynomial *p1, const polynomial *p2)
00930 {
00931 polynomial *result = NULL;
00932 cpl_polynomial *pol = NULL;
00933
00934 assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00935 assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00936 assure(xsh_polynomial_get_dimension(p1) == 2,
00937 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00938 assure(xsh_polynomial_get_dimension(p2) == 2,
00939 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950 {
00951 int degree, i, j;
00952
00953 degree = xsh_max_int(xsh_polynomial_get_degree(p1),
00954 xsh_polynomial_get_degree(p2));
00955
00956 pol = cpl_polynomial_new(2);
00957 for (i = 0; i <= degree; i++)
00958 for (j = 0; j <= degree; j++) {
00959 double coeff1, coeff2;
00960 cpl_size power[2];
00961
00962
00963 coeff1 = xsh_polynomial_get_coeff_2d(p1, i, j);
00964 coeff2 = xsh_polynomial_get_coeff_2d(p2, i, j);
00965
00966 power[0] = i;
00967 power[1] = j;
00968 cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
00969 }
00970 }
00971
00972 result = xsh_polynomial_new(pol);
00973
00974 cleanup:
00975 xsh_free_polynomial(&pol);
00976 return result;
00977 }
00978
00979
00992
00993 static cpl_error_code
00994 derivative_cpl_polynomial(cpl_polynomial *p, int varno)
00995 {
00996 int dimension, degree;
00997 int i, j;
00998 cpl_size power[2];
00999
01000 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01001 dimension = cpl_polynomial_get_dimension(p);
01002 degree = cpl_polynomial_get_degree(p);
01003 assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT,
01004 "Illegal dimension: %d", dimension);
01005 assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01006 "Illegal variable number: %d", varno);
01007
01008 if (dimension == 1)
01009 {
01010
01011 for(i = 0; i <= degree; i++)
01012 {
01013 double coeff;
01014 power[0] = i+1;
01015
01016
01017 coeff = cpl_polynomial_get_coeff(p, power);
01018
01019 power[0] = i;
01020 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01021 }
01022 }
01023
01024 if (dimension == 2)
01025 {
01026
01027 for(i = 0; i <= degree; i++)
01028 {
01029 for(j = 0; i + j <= degree; j++)
01030 {
01031 double coeff;
01032 power[varno - 1] = i+1;
01033 power[2 - varno] = j;
01034
01035 coeff = cpl_polynomial_get_coeff(p, power);
01036
01037 power[varno - 1] = i;
01038
01039 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
01040 }
01041 }
01042 }
01043
01044 cleanup:
01045 return cpl_error_get_code();
01046 }
01047
01048
01058
01059 cpl_error_code
01060 xsh_polynomial_derivative(polynomial *p, int varno)
01061 {
01062 int dimension;
01063
01064 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01065 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
01066 assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01067 "Illegal variable number: %d", varno);
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081 p->shift[0] = 0;
01082 p->scale[0] = p->scale[0] / p->scale[varno];
01083
01084 check_msg( derivative_cpl_polynomial(p->pol, varno),
01085 "Error calculating derivative of CPL-polynomial");
01086
01087 cleanup:
01088 return cpl_error_get_code();
01089 }
01090
01091
01092
01101
01102 double
01103 xsh_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
01104 {
01105 polynomial *pp = NULL;
01106 int dimension;
01107 double result = 0;
01108 double factorial;
01109
01110 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01111 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
01112 assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01113 assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
01114 assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);
01115
01116
01117
01118
01119
01120
01121 pp = xsh_polynomial_duplicate(p);
01122
01123 factorial = 1;
01124 while(degree1 > 0)
01125 {
01126 check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
01127
01128 factorial *= degree1;
01129 degree1 -= 1;
01130 }
01131
01132 while(degree2 > 0)
01133 {
01134 check_msg( xsh_polynomial_derivative(pp, 2), "Error calculating derivative");
01135
01136 factorial *= degree2;
01137 degree2 -= 1;
01138 }
01139
01140 check_msg( result = xsh_polynomial_evaluate_2d(pp, 0, 0) / factorial,
01141 "Error evaluating polynomial");
01142
01143 cleanup:
01144 xsh_polynomial_delete(&pp);
01145 return result;
01146 }
01147
01157
01158 double
01159 xsh_polynomial_get_coeff_1d(const polynomial *p, int degree)
01160 {
01161 polynomial *pp = NULL;
01162 int dimension;
01163 double result = 0;
01164 double factorial;
01165
01166 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01167 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
01168 assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01169 assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
01170
01171
01172
01173
01174
01175
01176 pp = xsh_polynomial_duplicate(p);
01177
01178 factorial = 1;
01179 while(degree > 0)
01180 {
01181 check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
01182
01183 factorial *= degree;
01184 degree -= 1;
01185 }
01186
01187 check_msg( result = xsh_polynomial_evaluate_1d(pp, 0) / factorial,
01188 "Error evaluating polynomial");
01189
01190 cleanup:
01191 xsh_polynomial_delete(&pp);
01192 return result;
01193 }
01194
01195
01196
01212
01213 polynomial *
01214 xsh_polynomial_collapse(const polynomial *p, int varno, double value)
01215 {
01216 polynomial *result = NULL;
01217 cpl_polynomial *pol = NULL;
01218 cpl_size *power = NULL;
01219
01220 int i, j;
01221 int degree, dimension;
01222
01223 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01224 dimension = xsh_polynomial_get_dimension(p);
01225 assure(dimension > 0, CPL_ERROR_ILLEGAL_INPUT,
01226 "Polynomial has non-positive dimension: %d", dimension);
01227 assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
01228 "Don't collapse a 1d polynomial. Evaluate it!");
01229
01230
01231
01232
01233 assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
01234
01235 assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01236 "Wrong variable number");
01237 value = (value - p->shift[varno]) / p->scale[varno];
01238
01239
01240 degree = cpl_polynomial_get_degree(p->pol);
01241 pol = cpl_polynomial_new(dimension - 1);
01242 power = cpl_malloc(sizeof(cpl_size) * dimension);
01243 assure_mem( power );
01244 for (i = 0; i <= degree; i++)
01245 {
01246 double coeff;
01247
01248 power[2-varno] = i;
01249
01250
01251 coeff = 0;
01252 for (j = degree - i; j >= 0; j--)
01253 {
01254 power[varno-1] = j;
01255 coeff += cpl_polynomial_get_coeff(p->pol, power);
01256 if (j > 0) coeff *= value;
01257 }
01258
01259 power[0] = i;
01260 cpl_polynomial_set_coeff(pol, power, coeff);
01261 }
01262
01263
01264 result = xsh_polynomial_new(pol);
01265
01266
01267 j = 0;
01268 for(i = 0; i <= dimension - 1; i++)
01269 {
01270 if (i == varno)
01271 {
01272
01273 j += 2;
01274
01275 }
01276 else
01277 {
01278 result->shift[i] = p->shift[j];
01279 result->scale[i] = p->scale[j];
01280 j += 1;
01281 }
01282 }
01283
01284 assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(),
01285 "Error collapsing polynomial");
01286
01287 cleanup:
01288 cpl_free(power); power = NULL;
01289 xsh_free_polynomial(&pol);
01290 if (cpl_error_get_code() != CPL_ERROR_NONE)
01291 {
01292 xsh_polynomial_delete(&result);
01293 }
01294 return result;
01295 }
01296
01297
01298
01299
01319
01320 polynomial * xsh_polynomial_fit_1d(
01321 const cpl_vector * x_pos,
01322 const cpl_vector * values,
01323 const cpl_vector * sigmas,
01324 int poly_deg,
01325 double * mse)
01326 {
01327 int nc ;
01328 int np ;
01329 cpl_matrix * ma = NULL;
01330 cpl_matrix * mb = NULL;
01331 cpl_matrix * mx = NULL;
01332 const double * x_pos_data ;
01333 const double * values_data ;
01334 const double * sigmas_data = NULL;
01335 double mean_x, mean_z;
01336 polynomial * result = NULL;
01337 cpl_polynomial * out ;
01338 cpl_vector * x_val = NULL;
01339 int i, j ;
01340
01341
01342 assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
01343 assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT,
01344 "Polynomial degree is %d. Must be non-negative", poly_deg);
01345 np = cpl_vector_get_size(x_pos) ;
01346
01347 nc = 1 + poly_deg ;
01348 assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
01349 "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
01350 np, poly_deg, nc);
01351
01352
01353
01354
01355
01356 ma = cpl_matrix_new(np, nc) ;
01357 mb = cpl_matrix_new(np, 1) ;
01358
01359
01360 mean_x = cpl_vector_get_mean(x_pos);
01361 mean_z = cpl_vector_get_mean(values);
01362
01363
01364 x_pos_data = cpl_vector_get_data_const(x_pos) ;
01365 values_data = cpl_vector_get_data_const(values) ;
01366 if (sigmas != NULL)
01367 {
01368 sigmas_data = cpl_vector_get_data_const(sigmas) ;
01369 }
01370
01371 if (sigmas != NULL)
01372 {
01373 for (i=0 ; i<np ; i++)
01374 {
01375
01376 if (sigmas_data[i] == 0)
01377 {
01378 xsh_free_matrix(&ma) ;
01379 xsh_free_matrix(&mb) ;
01380 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01381 "Sigmas must be non-zero");
01382 }
01383 for (j=0 ; j<nc ; j++)
01384 {
01385 cpl_matrix_set(ma, i, j,
01386 xsh_pow_int(x_pos_data[i] - mean_x, j) /
01387 sigmas_data[i]) ;
01388 }
01389
01390 cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
01391 }
01392 }
01393 else
01394 {
01395 for (i=0 ; i<np ; i++)
01396 {
01397 for (j=0 ; j<nc ; j++)
01398 {
01399 cpl_matrix_set(ma, i, j,
01400 xsh_pow_int(x_pos_data[i] - mean_x, j) / 1);
01401 }
01402
01403 cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01404 }
01405 }
01406
01407
01408 check_msg( mx = xsh_matrix_solve_normal(ma, mb),
01409 "Could not invert matrix");
01410 xsh_free_matrix(&ma);
01411 xsh_free_matrix(&mb);
01412
01413
01414 out = cpl_polynomial_new(1) ;
01415 cpl_size deg=0;
01416 for (deg=0 ; deg<nc ; deg++) {
01417 cpl_polynomial_set_coeff(out, °, cpl_matrix_get(mx, deg, 0)) ;
01418 }
01419 xsh_free_matrix(&mx);
01420
01421
01422 if (mse != NULL) {
01423 *mse = 0.00 ;
01424 x_val = cpl_vector_new(1) ;
01425 for (i=0 ; i<np ; i++)
01426 {
01427 double residual;
01428 cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
01429
01430 residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
01431 *mse += residual*residual;
01432 }
01433 xsh_free_vector(&x_val) ;
01434
01435 *mse /= (double)np ;
01436 }
01437
01438
01439 result = xsh_polynomial_new(out);
01440 xsh_free_polynomial(&out);
01441
01442 xsh_polynomial_shift(result, 0, mean_z);
01443 xsh_polynomial_shift(result, 1, mean_x);
01444
01445 cleanup:
01446 xsh_free_vector(&x_val);
01447 xsh_free_matrix(&ma);
01448 xsh_free_matrix(&mb);
01449 xsh_free_matrix(&mx);
01450 return result;
01451 }
01452
01453
01454
01498
01499 polynomial *
01500 xsh_polynomial_fit_2d(const cpl_bivector * xy_pos, const cpl_vector * values,
01501 const cpl_vector * sigmas, int poly_deg1, int poly_deg2, double * mse,
01502 double * red_chisq, polynomial ** variance) {
01503 int nc;
01504 int degx, degy;
01505 int * degx_tab;
01506 int * degy_tab;
01507 int np;
01508 cpl_matrix * ma;
01509 cpl_matrix * mb;
01510 cpl_matrix * mx;
01511 cpl_matrix * mat;
01512 cpl_matrix * mat_ma;
01513 cpl_matrix * cov = NULL;
01514 const double * xy_pos_data_x;
01515 const double * xy_pos_data_y;
01516 const double * values_data;
01517 const double * sigmas_data = NULL;
01518 const cpl_vector* xy_pos_x;
01519 const cpl_vector* xy_pos_y;
01520 double mean_x, mean_y, mean_z;
01521 cpl_polynomial * out;
01522 cpl_polynomial * variance_cpl;
01523 polynomial * result = NULL;
01524 cpl_size * powers;
01525 int i_nc=0;
01526
01527
01528 assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
01529 assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT,
01530 "Polynomial degree1 is %d", poly_deg1);
01531 assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT,
01532 "Polynomial degree2 is %d", poly_deg2);
01533 np = cpl_bivector_get_size(xy_pos);
01534
01535
01536 assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL,
01537 CPL_ERROR_ILLEGAL_INPUT,
01538 "Cannot calculate variance or chi_sq without knowing");
01539
01540
01541 nc = (1 + poly_deg1) * (1 + poly_deg2);
01542
01543 assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX,
01544 "%d coefficients. Only %d points", nc, np);
01545
01546
01547
01548
01549 assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT,
01550 "%d coefficients. %d points. Cannot calculate chi square", nc, np);
01551
01552 degx_tab = cpl_malloc(nc * sizeof(int));
01553 assure_mem( degx_tab);
01554
01555 degy_tab = cpl_malloc(nc * sizeof(int));
01556 if (degy_tab == NULL) {
01557 cpl_free(degx_tab);
01558 assure_mem( false);
01559 }
01560
01561 {
01562 int i = 0;
01563 for (degy = 0; degy <= poly_deg2; degy++) {
01564 for (degx = 0; degx <= poly_deg1; degx++) {
01565 degx_tab[i] = degx;
01566 degy_tab[i] = degy;
01567 i++;
01568 }
01569 }
01570 }
01571
01572
01573
01574
01575
01576 ma = cpl_matrix_new(np, nc);
01577 mb = cpl_matrix_new(np, 1);
01578
01579
01580 xy_pos_x = cpl_bivector_get_x_const(xy_pos);
01581 xy_pos_y = cpl_bivector_get_y_const(xy_pos);
01582
01583 mean_x = cpl_vector_get_mean(xy_pos_x);
01584 mean_y = cpl_vector_get_mean(xy_pos_y);
01585 mean_z = cpl_vector_get_mean(values);
01586
01587
01588
01589 xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x);
01590 xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y);
01591 values_data = cpl_vector_get_data_const(values);
01592 if (sigmas != NULL) {
01593 sigmas_data = cpl_vector_get_data_const(sigmas);
01594 }
01595
01596 if (sigmas != NULL) {
01597 int i;
01598 for (i = 0; i < np; i++) {
01599 double *ma_data = cpl_matrix_get_data(ma);
01600 double *mb_data = cpl_matrix_get_data(mb);
01601
01602 int j = 0;
01603 double valy = 1;
01604
01605
01606 if (sigmas_data[i] == 0) {
01607 xsh_free_matrix(&ma);
01608 xsh_free_matrix(&mb);
01609 cpl_free(degx_tab);
01610 cpl_free(degy_tab);
01611 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01612 "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
01613 }
01614 i_nc=i*nc;
01615 for (degy = 0; degy <= poly_deg2; degy++) {
01616 double valx = 1;
01617 for (degx = 0; degx <= poly_deg1; degx++) {
01618 ma_data[j + i_nc] = valx * valy / sigmas_data[i];
01619 valx *= (xy_pos_data_x[i] - mean_x);
01620 j++;
01621 }
01622 valy *= (xy_pos_data_y[i] - mean_y);
01623 }
01624
01625
01626
01627 mb_data[i] = (values_data[i] - mean_z) / sigmas_data[i];
01628 }
01629 } else
01630 {
01631 int i;
01632 for (i = 0; i < np; i++) {
01633 double *ma_data = cpl_matrix_get_data(ma);
01634 double *mb_data = cpl_matrix_get_data(mb);
01635
01636 double valy = 1;
01637 i_nc=i*nc;
01638 int j = 0;
01639 for (degy = 0; degy <= poly_deg2; degy++) {
01640 double valx = 1;
01641 for (degx = 0; degx <= poly_deg1; degx++) {
01642 ma_data[j + i_nc] = valx * valy / 1;
01643 valx *= (xy_pos_data_x[i] - mean_x);
01644 j++;
01645 }
01646 valy *= (xy_pos_data_y[i] - mean_y);
01647 }
01648
01649
01650
01651 mb_data[i] = values_data[i] - mean_z;
01652 }
01653 }
01654
01655
01656
01657 if (variance != NULL) {
01658 mat = cpl_matrix_transpose_create(ma);
01659 if (mat != NULL) {
01660 mat_ma = cpl_matrix_product_create(mat, ma);
01661 if (mat_ma != NULL) {
01662 cov = cpl_matrix_invert_create(mat_ma);
01663
01664
01665
01666
01667 variance_cpl = cpl_polynomial_new(2);
01668 }
01669 }
01670 xsh_free_matrix(&mat);
01671 xsh_free_matrix(&mat_ma);
01672 }
01673
01674
01675 mx = xsh_matrix_solve_normal(ma, mb);
01676
01677 xsh_free_matrix(&ma);
01678 xsh_free_matrix(&mb);
01679 if (mx == NULL) {
01680 cpl_free(degx_tab);
01681 cpl_free(degy_tab);
01682 xsh_free_matrix(&cov);
01683 assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed");
01684 }
01685
01686
01687 out = cpl_polynomial_new(2);
01688 powers = cpl_malloc(2 * sizeof(cpl_size));
01689 if (powers == NULL) {
01690 cpl_free(degx_tab);
01691 cpl_free(degy_tab);
01692 xsh_free_matrix(&mx);
01693 xsh_free_matrix(&cov);
01694 xsh_free_polynomial(&out);
01695 assure_mem( false);
01696 }
01697
01698 {
01699 int i;
01700 for (i = 0; i < nc; i++) {
01701 powers[0] = degx_tab[i];
01702 powers[1] = degy_tab[i];
01703 cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0));
01704
01705
01706 if (variance != NULL &&
01707 cov != NULL && variance_cpl != NULL
01708 ) {
01709 int j;
01710 for (j = 0; j < nc; j++) {
01711 double coeff;
01712
01713
01714
01715
01716
01717
01718 powers[0] = degx_tab[i] + degx_tab[j];
01719 powers[1] = degy_tab[i] + degy_tab[j];
01720
01721 coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
01722 cpl_polynomial_set_coeff(variance_cpl, powers,
01723 coeff + cpl_matrix_get(cov, i, j));
01724 }
01725 }
01726 }
01727 }
01728
01729 cpl_free(powers);
01730 cpl_free(degx_tab);
01731 cpl_free(degy_tab);
01732 xsh_free_matrix(&cov);
01733 xsh_free_matrix(&mx);
01734
01735
01736 result = xsh_polynomial_new(out);
01737 xsh_free_polynomial(&out);
01738 xsh_polynomial_shift(result, 0, mean_z);
01739 xsh_polynomial_shift(result, 1, mean_x);
01740 xsh_polynomial_shift(result, 2, mean_y);
01741
01742
01743 if (variance != NULL) {
01744 *variance = xsh_polynomial_new(variance_cpl);
01745 xsh_free_polynomial(&variance_cpl);
01746
01747
01748
01749
01750 xsh_polynomial_shift(*variance, 1, mean_x);
01751 xsh_polynomial_shift(*variance, 2, mean_y);
01752
01753
01754
01755 }
01756
01757
01758 if (mse != NULL || red_chisq != NULL) {
01759 int i;
01760
01761 if (mse != NULL)
01762 *mse = 0.00;
01763 if (red_chisq != NULL)
01764 *red_chisq = 0.00;
01765 for (i = 0; i < np; i++) {
01766 double regress = xsh_polynomial_evaluate_2d(result, xy_pos_data_x[i],
01767 xy_pos_data_y[i]);
01768 double residual = values_data[i] - regress;
01769
01770 if (mse != NULL) {
01771 *mse += residual * residual;
01772 }
01773 if (red_chisq != NULL) {
01774 *red_chisq += (residual / sigmas_data[i]) * (residual / sigmas_data[i]) ;
01775 }
01776 }
01777
01778 if (mse != NULL)
01779 *mse /= (double) np;
01780
01781 if (red_chisq != NULL) {
01782 passure( np > nc, "%d %d", np, nc);
01783
01784 *red_chisq /= (double) (np - nc);
01785 }
01786 }
01787
01788 cleanup: return result;
01789 }
01790
01791