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
00068
00069
00070
00071 #include <xsh_utils_polynomial.h>
00072
00073 #include <xsh_utils.h>
00074
00075 #include <xsh_dump.h>
00076 #include <xsh_msg.h>
00077 #include <xsh_error.h>
00078
00079 #include <cpl.h>
00080
00081
00082
00083
00086 struct _polynomial
00087 {
00089 cpl_polynomial *pol;
00090
00092 cpl_vector *vec;
00093 double *vec_data;
00094
00095 int dimension;
00096
00098 double *shift;
00099
00101 double *scale;
00102 };
00103
00104
00105
00106
00107
00118
00119 polynomial *
00120 xsh_polynomial_new(const cpl_polynomial *pol)
00121 {
00122 polynomial *p = NULL;
00123 int i;
00124
00125
00126 assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00127
00128
00129 p = cpl_calloc(1, sizeof(polynomial)) ;
00130 assure_mem( p );
00131
00132 check_msg( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");
00133
00134
00135 p->vec = cpl_vector_new(p->dimension);
00136 assure_mem( p->vec );
00137 p->vec_data = cpl_vector_get_data(p->vec);
00138
00139
00140 p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
00141 assure_mem( p->shift );
00142
00143 p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
00144 assure_mem( p->scale );
00145 for (i = 0; i <= p->dimension; i++)
00146 p->scale[i] = 1.0;
00147
00148 check_msg( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
00149
00150 cleanup:
00151 if (cpl_error_get_code() != CPL_ERROR_NONE)
00152 xsh_polynomial_delete(&p);
00153
00154 return p;
00155 }
00156
00157
00165
00166 polynomial *
00167 xsh_polynomial_new_zero(int dim)
00168 {
00169 polynomial *result = NULL;
00170 cpl_polynomial *p = NULL;
00171
00172 assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);
00173
00174 p = cpl_polynomial_new(dim);
00175 assure_mem( p );
00176
00177 result = xsh_polynomial_new(p);
00178 assure_mem( result );
00179
00180 cleanup:
00181 xsh_free_polynomial(&p);
00182
00183 return result;
00184 }
00185
00186
00193
00194 void
00195 xsh_polynomial_delete(polynomial **p)
00196 {
00197 xsh_polynomial_delete_const((const polynomial **)p);
00198 }
00199
00200
00207
00208 void
00209 xsh_polynomial_delete_const(const polynomial **p)
00210 {
00211 if (*p == NULL) return;
00212 cpl_polynomial_delete((*p)->pol);
00213 cpl_vector_delete((*p)->vec);
00214 cpl_free((*p)->shift);
00215 cpl_free((*p)->scale);
00216 xsh_free(*p);
00217 *p = NULL;
00218 return;
00219 }
00220
00226
00227 int
00228 xsh_polynomial_get_degree(const polynomial *p)
00229 {
00230 int result = -1;
00231 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00232
00233 result = cpl_polynomial_get_degree(p->pol);
00234
00235 cleanup:
00236 return result;
00237 }
00238
00239
00245
00246 polynomial *
00247 xsh_polynomial_duplicate(const polynomial *p)
00248 {
00249 polynomial *result = NULL;
00250 int dimension;
00251 int i;
00252
00253 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00254 dimension = xsh_polynomial_get_dimension(p);
00255
00256 check_msg( result = xsh_polynomial_new(p->pol),
00257 "Error allocating polynomial");
00258
00259 for (i = 0; i <= dimension; i++)
00260 {
00261 result->shift[i] = p->shift[i];
00262 result->scale[i] = p->scale[i];
00263 }
00264
00265 cleanup:
00266 if (cpl_error_get_code() != CPL_ERROR_NONE)
00267 {
00268 xsh_polynomial_delete(&result);
00269 return NULL;
00270 }
00271
00272 return result;
00273 }
00274
00275
00276
00287
00288 cpl_table *
00289 xsh_polynomial_convert_to_table(const polynomial *p)
00290 {
00291 cpl_table *t = NULL;
00292 int degree;
00293 int i, j, row;
00294
00295
00296 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00297 assure( xsh_polynomial_get_dimension(p) == 2,
00298 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
00299
00300 degree = cpl_polynomial_get_degree(p->pol);
00301
00302
00303
00304 t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
00305 cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
00306 cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
00307 cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);
00308
00309 row = 0;
00310
00311
00312 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00313 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00314 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;
00315
00316 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00317 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00318 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;
00319
00320 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00321 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00322 cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;
00323
00324
00325 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00326 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00327 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[0]); row++;
00328
00329 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00330 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00331 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[1]); row++;
00332
00333 cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
00334 cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
00335 cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;
00336
00337
00338 for (i = 0; i <= degree; i++){
00339 for (j = 0; j+i <= degree; j++){
00340 double coeff;
00341 int power[2];
00342 power[0] = i;
00343 power[1] = j;
00344
00345 coeff = cpl_polynomial_get_coeff(p->pol, power);
00346 cpl_table_set_int (t, COLUMN_ORDER1, row, power[0]);
00347 cpl_table_set_int (t, COLUMN_ORDER2, row, power[1]);
00348 cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
00349
00350 row++;
00351 }
00352 }
00353
00354 cleanup:
00355 return t;
00356 }
00357
00358
00367
00368 polynomial *
00369 xsh_polynomial_convert_from_table(cpl_table *t)
00370 {
00371 polynomial *p = NULL;
00372 cpl_polynomial *pol = NULL;
00373 cpl_type type;
00374 int i;
00375
00376
00377 check_msg( pol = cpl_polynomial_new(2), "Error initializing polynomial");
00378
00379
00380 assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
00381 assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT,
00382 "No '%s' column found in table", COLUMN_ORDER1);
00383 assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
00384 "No '%s' column found in table", COLUMN_ORDER2);
00385 assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
00386 "No '%s' column found in table", COLUMN_COEFF );
00387
00388 type = cpl_table_get_column_type(t, COLUMN_ORDER1);
00389 assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
00390 "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
00391 xsh_tostring_cpl_type(type));
00392
00393 type = cpl_table_get_column_type(t, COLUMN_ORDER2);
00394 assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
00395 "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
00396 xsh_tostring_cpl_type(type));
00397
00398 type = cpl_table_get_column_type(t, COLUMN_COEFF);
00399 assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
00400 "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
00401 xsh_tostring_cpl_type(type));
00402
00403 assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
00404 "Table must contain at least one coefficient");
00405
00406
00407 for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
00408 double coeff;
00409 int power[2];
00410
00411 check_msg(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
00412 power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
00413 coeff = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
00414 "Error reading table row %d", i);
00415
00416 xsh_msg_debug("Pol.coeff.(%d, %d) = %e", power[0], power[1], coeff);
00417
00418 check_msg( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
00419 }
00420 p = xsh_polynomial_new(pol);
00421
00422
00423 xsh_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
00424 xsh_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
00425 xsh_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
00426 xsh_polynomial_shift (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
00427 xsh_polynomial_shift (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
00428 xsh_polynomial_shift (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
00429
00430 cleanup:
00431 xsh_free_polynomial(&pol);
00432 if (cpl_error_get_code() != CPL_ERROR_NONE)
00433 xsh_polynomial_delete(&p);
00434
00435 return p;
00436 }
00437
00438
00439
00445
00446 int
00447 xsh_polynomial_get_dimension(const polynomial *p)
00448 {
00449 int dim = -1;
00450 assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
00451
00452
00453 dim = p->dimension;
00454
00455 cleanup:
00456 return dim;
00457 }
00458
00459
00467
00468 void xsh_polynomial_dump(const polynomial *p, FILE *stream)
00469 {
00470 if (p == NULL)
00471 fprintf(stream, "Null polynomial\n");
00472 else {
00473 int i;
00474 cpl_polynomial_dump(p->pol, stream);
00475 fprintf(stream, "shift_y \t= %f \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
00476 for (i = 1; i <= xsh_polynomial_get_dimension(p); i++)
00477 {
00478 fprintf(stream, "shift_x%d \t= %f \tscale_x%d \t= %f\n",
00479 i, p->shift[i], i, p->scale[i]);
00480 }
00481 }
00482 return;
00483 }
00484
00485
00499
00500 cpl_error_code
00501 xsh_polynomial_rescale(polynomial *p, int varno, double scale)
00502 {
00503 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00504 assure(0 <= varno && varno <= xsh_polynomial_get_dimension(p),
00505 CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519 p->shift[varno] *= scale;
00520 p->scale[varno] *= scale;
00521
00522 cleanup:
00523 return cpl_error_get_code();
00524 }
00525
00526
00540
00541 cpl_error_code
00542 xsh_polynomial_shift(polynomial *p, int varno, double shift)
00543 {
00544 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00545 assure(0 <= varno && varno <= xsh_polynomial_get_dimension(p),
00546 CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557 p->shift[varno] += shift;
00558
00559 cleanup:
00560 return cpl_error_get_code();
00561 }
00562
00563
00572
00573 double
00574 xsh_polynomial_evaluate_1d(const polynomial *p, double x)
00575 {
00576 double result = 0;
00577
00578 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00579 assure(xsh_polynomial_get_dimension(p) == 1,
00580 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00581
00582 check_msg( result =
00583 cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
00584 * p->scale[0] + p->shift[0],
00585 "Could not evaluate polynomial");
00586
00587 cleanup:
00588 return result;
00589 }
00590
00591
00592
00602
00603
00604 double
00605 xsh_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
00606 {
00607 double result = 0;
00608
00609 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00610 assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
00611 "Polynomial must be 2d. It's %dd", p->dimension);
00612 {
00613 double scale = p->scale[0];
00614 double shift = p->shift[0];
00615
00616
00617
00618 p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
00619 p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
00620
00621 result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
00622 }
00623
00624 cleanup:
00625 return result;
00626 }
00627
00628
00641
00642 double
00643 xsh_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
00644 {
00645 double result = 0;
00646 int power[1];
00647 double coeff0;
00648
00649 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00650 assure(xsh_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT,
00651 "Polynomial must be 1d");
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661 power[0] = 0;
00662 check_msg(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
00663 cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
00664 "Error setting coefficient");
00665
00666 check_msg( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
00667 &result, multiplicity), "Could not find root");
00668
00669 cpl_polynomial_set_coeff(p->pol, power, coeff0);
00670
00671
00672 result = result * p->scale[1] + p->shift[1];
00673
00674 cleanup:
00675 return result;
00676 }
00677
00678
00695
00696 double
00697 xsh_polynomial_solve_2d(const polynomial *p, double value, double guess,
00698 int multiplicity, int varno, double x_value)
00699 {
00700 double result = 0;
00701 polynomial *pol_1d = NULL;
00702
00703 assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00704 "Illegal variable number: %d", varno);
00705
00706 check_msg( pol_1d = xsh_polynomial_collapse(p, varno, x_value),
00707 "Could not collapse polynomial");
00708
00709 check_msg( result = xsh_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
00710 "Could not find root");
00711
00712 cleanup:
00713 xsh_polynomial_delete(&pol_1d);
00714 return result;
00715 }
00716
00717
00726
00727 double
00728 xsh_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
00729 {
00730 double result = 0;
00731 int power[2];
00732
00733 assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
00734 "Illegal variable number (%d)", varno);
00735
00736 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00737 assure(xsh_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
00738 "Polynomial must be 2d. It's %dd", xsh_polynomial_get_dimension(p));
00739
00740
00741
00742
00743
00744
00745
00746 x1 = (x1 - p->shift[1])/p->scale[1];
00747 x2 = (x2 - p->shift[2])/p->scale[2];
00748
00749
00750
00751
00752 {
00753 int degree = cpl_polynomial_get_degree(p->pol);
00754 double yj = 1;
00755 int i, j;
00756
00757 result = 0;
00758 for (j = 0, yj = 1;
00759 j <= degree; j++,
00760 yj *= (varno == 1) ? x2 : x1)
00761 {
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772 double sum = 0;
00773 for (i = degree; i >= 1; i--)
00774 {
00775 double c_ij;
00776
00777 power[0] = (varno == 1) ? i : j;
00778 power[1] = (varno == 1) ? j : i;
00779
00780 c_ij = cpl_polynomial_get_coeff(p->pol, power);
00781
00782 sum += (i * c_ij);
00783 if (i >= 2) sum *= (varno == 1) ? x1 : x2;
00784 }
00785
00786
00787 result += yj * sum;
00788 }
00789 }
00790
00791 result *= p->scale[0];
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804 cleanup:
00805 return result;
00806 }
00807
00808
00815
00816 double
00817 xsh_polynomial_derivative_1d(const polynomial *p, double x)
00818 {
00819 double result = 0;
00820 double dummy;
00821
00822 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00823 assure(xsh_polynomial_get_dimension(p) == 1,
00824 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
00825
00826 check_msg( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
00827 "Error evaluating derivative");
00828
00829 cleanup:
00830 return result;
00831 }
00832
00833
00840
00841 polynomial *
00842 xsh_polynomial_add_2d(const polynomial *p1, const polynomial *p2)
00843 {
00844 polynomial *result = NULL;
00845 cpl_polynomial *pol = NULL;
00846
00847 assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00848 assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00849 assure(xsh_polynomial_get_dimension(p1) == 2,
00850 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00851 assure(xsh_polynomial_get_dimension(p2) == 2,
00852 CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863 {
00864 int degree, i, j;
00865
00866 degree = xsh_max_int(xsh_polynomial_get_degree(p1),
00867 xsh_polynomial_get_degree(p2));
00868
00869 pol = cpl_polynomial_new(2);
00870 for (i = 0; i <= degree; i++)
00871 for (j = 0; j <= degree; j++) {
00872 double coeff1, coeff2;
00873 int power[2];
00874
00875
00876 coeff1 = xsh_polynomial_get_coeff_2d(p1, i, j);
00877 coeff2 = xsh_polynomial_get_coeff_2d(p2, i, j);
00878
00879 power[0] = i;
00880 power[1] = j;
00881 cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
00882 }
00883 }
00884
00885 result = xsh_polynomial_new(pol);
00886
00887 cleanup:
00888 xsh_free_polynomial(&pol);
00889 return result;
00890 }
00891
00892
00905
00906 static cpl_error_code
00907 derivative_cpl_polynomial(cpl_polynomial *p, int varno)
00908 {
00909 int dimension, degree;
00910 int i, j;
00911 int power[2];
00912
00913 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00914 dimension = cpl_polynomial_get_dimension(p);
00915 degree = cpl_polynomial_get_degree(p);
00916 assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT,
00917 "Illegal dimension: %d", dimension);
00918 assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
00919 "Illegal variable number: %d", varno);
00920
00921 if (dimension == 1)
00922 {
00923
00924 for(i = 0; i <= degree; i++)
00925 {
00926 double coeff;
00927 power[0] = i+1;
00928
00929
00930 coeff = cpl_polynomial_get_coeff(p, power);
00931
00932 power[0] = i;
00933 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
00934 }
00935 }
00936
00937 if (dimension == 2)
00938 {
00939
00940 for(i = 0; i <= degree; i++)
00941 {
00942 for(j = 0; i + j <= degree; j++)
00943 {
00944 double coeff;
00945 power[varno - 1] = i+1;
00946 power[2 - varno] = j;
00947
00948 coeff = cpl_polynomial_get_coeff(p, power);
00949
00950 power[varno - 1] = i;
00951
00952 cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
00953 }
00954 }
00955 }
00956
00957 cleanup:
00958 return cpl_error_get_code();
00959 }
00960
00961
00971
00972 cpl_error_code
00973 xsh_polynomial_derivative(polynomial *p, int varno)
00974 {
00975 int dimension;
00976
00977 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
00978 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
00979 assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
00980 "Illegal variable number: %d", varno);
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994 p->shift[0] = 0;
00995 p->scale[0] = p->scale[0] / p->scale[varno];
00996
00997 check_msg( derivative_cpl_polynomial(p->pol, varno),
00998 "Error calculating derivative of CPL-polynomial");
00999
01000 cleanup:
01001 return cpl_error_get_code();
01002 }
01003
01004
01005
01014
01015 double
01016 xsh_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
01017 {
01018 polynomial *pp = NULL;
01019 int dimension;
01020 double result = 0;
01021 double factorial;
01022
01023 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01024 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
01025 assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01026 assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
01027 assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);
01028
01029
01030
01031
01032
01033
01034 pp = xsh_polynomial_duplicate(p);
01035
01036 factorial = 1;
01037 while(degree1 > 0)
01038 {
01039 check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
01040
01041 factorial *= degree1;
01042 degree1 -= 1;
01043 }
01044
01045 while(degree2 > 0)
01046 {
01047 check_msg( xsh_polynomial_derivative(pp, 2), "Error calculating derivative");
01048
01049 factorial *= degree2;
01050 degree2 -= 1;
01051 }
01052
01053 check_msg( result = xsh_polynomial_evaluate_2d(pp, 0, 0) / factorial,
01054 "Error evaluating polynomial");
01055
01056 cleanup:
01057 xsh_polynomial_delete(&pp);
01058 return result;
01059 }
01060
01070
01071 double
01072 xsh_polynomial_get_coeff_1d(const polynomial *p, int degree)
01073 {
01074 polynomial *pp = NULL;
01075 int dimension;
01076 double result = 0;
01077 double factorial;
01078
01079 assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01080 check_msg ( dimension = xsh_polynomial_get_dimension(p), "Error reading dimension");
01081 assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
01082 assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
01083
01084
01085
01086
01087
01088
01089 pp = xsh_polynomial_duplicate(p);
01090
01091 factorial = 1;
01092 while(degree > 0)
01093 {
01094 check_msg( xsh_polynomial_derivative(pp, 1), "Error calculating derivative");
01095
01096 factorial *= degree;
01097 degree -= 1;
01098 }
01099
01100 check_msg( result = xsh_polynomial_evaluate_1d(pp, 0) / factorial,
01101 "Error evaluating polynomial");
01102
01103 cleanup:
01104 xsh_polynomial_delete(&pp);
01105 return result;
01106 }
01107
01108
01109
01125
01126 polynomial *
01127 xsh_polynomial_collapse(const polynomial *p, int varno, double value)
01128 {
01129 polynomial *result = NULL;
01130 cpl_polynomial *pol = NULL;
01131 int *power = NULL;
01132
01133 int i, j;
01134 int degree, dimension;
01135
01136 assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
01137 dimension = xsh_polynomial_get_dimension(p);
01138 assure(dimension > 0, CPL_ERROR_ILLEGAL_INPUT,
01139 "Polynomial has non-positive dimension: %d", dimension);
01140 assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
01141 "Don't collapse a 1d polynomial. Evaluate it!");
01142
01143
01144
01145
01146 assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
01147
01148 assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
01149 "Wrong variable number");
01150 value = (value - p->shift[varno]) / p->scale[varno];
01151
01152
01153 degree = cpl_polynomial_get_degree(p->pol);
01154 pol = cpl_polynomial_new(dimension - 1);
01155 power = cpl_malloc(sizeof(int) * dimension);
01156 assure_mem( power );
01157 for (i = 0; i <= degree; i++)
01158 {
01159 double coeff;
01160
01161 power[2-varno] = i;
01162
01163
01164 coeff = 0;
01165 for (j = degree - i; j >= 0; j--)
01166 {
01167 power[varno-1] = j;
01168 coeff += cpl_polynomial_get_coeff(p->pol, power);
01169 if (j > 0) coeff *= value;
01170 }
01171
01172 power[0] = i;
01173 cpl_polynomial_set_coeff(pol, power, coeff);
01174 }
01175
01176
01177 result = xsh_polynomial_new(pol);
01178
01179
01180 j = 0;
01181 for(i = 0; i <= dimension - 1; i++)
01182 {
01183 if (i == varno)
01184 {
01185
01186 j += 2;
01187
01188 }
01189 else
01190 {
01191 result->shift[i] = p->shift[j];
01192 result->scale[i] = p->scale[j];
01193 j += 1;
01194 }
01195 }
01196
01197 assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(),
01198 "Error collapsing polynomial");
01199
01200 cleanup:
01201 cpl_free(power); power = NULL;
01202 xsh_free_polynomial(&pol);
01203 if (cpl_error_get_code() != CPL_ERROR_NONE)
01204 {
01205 xsh_polynomial_delete(&result);
01206 }
01207 return result;
01208 }
01209
01210
01211
01212
01232
01233 polynomial * xsh_polynomial_fit_1d(
01234 const cpl_vector * x_pos,
01235 const cpl_vector * values,
01236 const cpl_vector * sigmas,
01237 int poly_deg,
01238 double * mse)
01239 {
01240 int nc ;
01241 int np ;
01242 cpl_matrix * ma = NULL;
01243 cpl_matrix * mb = NULL;
01244 cpl_matrix * mx = NULL;
01245 const double * x_pos_data ;
01246 const double * values_data ;
01247 const double * sigmas_data = NULL;
01248 double mean_x, mean_z;
01249 polynomial * result = NULL;
01250 cpl_polynomial * out ;
01251 cpl_vector * x_val = NULL;
01252 int i, j ;
01253
01254
01255 assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
01256 assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT,
01257 "Polynomial degree is %d. Must be non-negative", poly_deg);
01258 np = cpl_vector_get_size(x_pos) ;
01259
01260 nc = 1 + poly_deg ;
01261 assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
01262 "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
01263 np, poly_deg, nc);
01264
01265
01266
01267
01268
01269 ma = cpl_matrix_new(np, nc) ;
01270 mb = cpl_matrix_new(np, 1) ;
01271
01272
01273 mean_x = cpl_vector_get_mean(x_pos);
01274 mean_z = cpl_vector_get_mean(values);
01275
01276
01277 x_pos_data = cpl_vector_get_data_const(x_pos) ;
01278 values_data = cpl_vector_get_data_const(values) ;
01279 if (sigmas != NULL)
01280 {
01281 sigmas_data = cpl_vector_get_data_const(sigmas) ;
01282 }
01283
01284 if (sigmas != NULL)
01285 {
01286 for (i=0 ; i<np ; i++)
01287 {
01288
01289 if (sigmas_data[i] == 0)
01290 {
01291 xsh_free_matrix(&ma) ;
01292 xsh_free_matrix(&mb) ;
01293 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01294 "Sigmas must be non-zero");
01295 }
01296 for (j=0 ; j<nc ; j++)
01297 {
01298 cpl_matrix_set(ma, i, j,
01299 xsh_pow_int(x_pos_data[i] - mean_x, j) /
01300 sigmas_data[i]) ;
01301 }
01302
01303 cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
01304 }
01305 }
01306 else
01307 {
01308 for (i=0 ; i<np ; i++)
01309 {
01310 for (j=0 ; j<nc ; j++)
01311 {
01312 cpl_matrix_set(ma, i, j,
01313 xsh_pow_int(x_pos_data[i] - mean_x, j) / 1);
01314 }
01315
01316 cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
01317 }
01318 }
01319
01320
01321 check_msg( mx = cpl_matrix_solve_normal(ma, mb),
01322 "Could not invert matrix");
01323 xsh_free_matrix(&ma);
01324 xsh_free_matrix(&mb);
01325
01326
01327 out = cpl_polynomial_new(1) ;
01328
01329 for (i=0 ; i<nc ; i++) {
01330 cpl_polynomial_set_coeff(out, &i, cpl_matrix_get(mx, i, 0)) ;
01331 }
01332 xsh_free_matrix(&mx);
01333
01334
01335 if (mse != NULL) {
01336 *mse = 0.00 ;
01337 x_val = cpl_vector_new(1) ;
01338 for (i=0 ; i<np ; i++)
01339 {
01340 double residual;
01341 cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
01342
01343 residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
01344 *mse += residual*residual;
01345 }
01346 xsh_free_vector(&x_val) ;
01347
01348 *mse /= (double)np ;
01349 }
01350
01351
01352 result = xsh_polynomial_new(out);
01353 xsh_free_polynomial(&out);
01354
01355 xsh_polynomial_shift(result, 0, mean_z);
01356 xsh_polynomial_shift(result, 1, mean_x);
01357
01358 cleanup:
01359 xsh_free_vector(&x_val);
01360 xsh_free_matrix(&ma);
01361 xsh_free_matrix(&mb);
01362 xsh_free_matrix(&mx);
01363 return result;
01364 }
01365
01366
01367
01411
01412 polynomial *
01413 xsh_polynomial_fit_2d(
01414 const cpl_bivector * xy_pos,
01415 const cpl_vector * values,
01416 const cpl_vector * sigmas,
01417 int poly_deg1,
01418 int poly_deg2,
01419 double * mse,
01420 double * red_chisq,
01421 polynomial ** variance)
01422 {
01423 int nc ;
01424 int degx, degy ;
01425 int * degx_tab ;
01426 int * degy_tab ;
01427 int np ;
01428 cpl_matrix * ma ;
01429 cpl_matrix * mb ;
01430 cpl_matrix * mx ;
01431 cpl_matrix * mat;
01432 cpl_matrix * mat_ma;
01433 cpl_matrix * cov = NULL;
01434 const double * xy_pos_data_x ;
01435 const double * xy_pos_data_y ;
01436 const double * values_data ;
01437 const double * sigmas_data = NULL;
01438 const cpl_vector* xy_pos_x;
01439 const cpl_vector* xy_pos_y;
01440 double mean_x, mean_y, mean_z;
01441 cpl_polynomial * out ;
01442 cpl_polynomial * variance_cpl ;
01443 polynomial * result = NULL;
01444 int * powers ;
01445
01446
01447 assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
01448 assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree1 is %d", poly_deg1);
01449 assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree2 is %d", poly_deg2);
01450 np = cpl_bivector_get_size(xy_pos) ;
01451
01452
01453 assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL,
01454 CPL_ERROR_ILLEGAL_INPUT,
01455 "Cannot calculate variance or chi_sq without knowing");
01456
01457
01458 nc = (1 + poly_deg1)*(1 + poly_deg2) ;
01459
01460 assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX, "%d coefficients. Only %d points", nc, np);
01461
01462
01463
01464
01465 assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT,
01466 "%d coefficients. %d points. Cannot calculate chi square", nc, np);
01467
01468 degx_tab = cpl_malloc(nc * sizeof(int)) ;
01469 assure_mem( degx_tab );
01470
01471 degy_tab = cpl_malloc(nc * sizeof(int)) ;
01472 if (degy_tab == NULL) {
01473 cpl_free(degx_tab);
01474 assure_mem( false );
01475 }
01476
01477 {
01478 int i=0 ;
01479 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01480 for (degx=0 ; degx<=poly_deg1 ; degx++) {
01481 degx_tab[i] = degx ;
01482 degy_tab[i] = degy ;
01483 i++ ;
01484 }
01485 }
01486 }
01487
01488
01489
01490
01491
01492 ma = cpl_matrix_new(np, nc) ;
01493 mb = cpl_matrix_new(np, 1) ;
01494
01495
01496 xy_pos_x = cpl_bivector_get_x_const(xy_pos);
01497 xy_pos_y = cpl_bivector_get_y_const(xy_pos);
01498
01499 mean_x = cpl_vector_get_mean(xy_pos_x);
01500 mean_y = cpl_vector_get_mean(xy_pos_y);
01501 mean_z = cpl_vector_get_mean(values);
01502
01503
01504
01505 xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x) ;
01506 xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y) ;
01507 values_data = cpl_vector_get_data_const(values) ;
01508 if (sigmas != NULL)
01509 {
01510 sigmas_data = cpl_vector_get_data_const(sigmas) ;
01511 }
01512
01513 if (sigmas != NULL)
01514 {
01515 int i;
01516 for (i=0 ; i<np ; i++) {
01517 double *ma_data = cpl_matrix_get_data(ma);
01518 double *mb_data = cpl_matrix_get_data(mb);
01519
01520 int j = 0;
01521 double valy = 1;
01522
01523
01524 if (sigmas_data[i] == 0)
01525 {
01526 xsh_free_matrix(&ma) ;
01527 xsh_free_matrix(&mb) ;
01528 cpl_free(degx_tab) ;
01529 cpl_free(degy_tab) ;
01530 assure(false, CPL_ERROR_DIVISION_BY_ZERO,
01531 "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
01532 }
01533
01534 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01535 double valx = 1;
01536 for (degx=0 ; degx<=poly_deg1 ; degx++) {
01537 ma_data[j + i*nc] = valx * valy / sigmas_data[i];
01538 valx *= (xy_pos_data_x[i] - mean_x);
01539 j++;
01540 }
01541 valy *= (xy_pos_data_y[i] - mean_y);
01542 }
01543
01544
01545
01546 mb_data[0 + i*1] = (values_data[i] - mean_z) / sigmas_data[i];
01547 }
01548 }
01549 else
01550 {
01551 int i;
01552 for (i=0 ; i<np ; i++) {
01553 double *ma_data = cpl_matrix_get_data(ma);
01554 double *mb_data = cpl_matrix_get_data(mb);
01555
01556 double valy = 1;
01557 int j = 0;
01558 for (degy=0 ; degy<=poly_deg2 ; degy++) {
01559 double valx = 1;
01560 for (degx=0 ; degx<=poly_deg1 ; degx++) {
01561 ma_data[j + i*nc] = valx * valy / 1;
01562 valx *= (xy_pos_data_x[i] - mean_x);
01563 j++;
01564 }
01565 valy *= (xy_pos_data_y[i] - mean_y);
01566 }
01567
01568
01569
01570 mb_data[0 + i*1] = (values_data[i] - mean_z) / 1;
01571 }
01572 }
01573
01574
01575
01576 if (variance != NULL)
01577 {
01578 mat = cpl_matrix_transpose_create(ma);
01579 if (mat != NULL)
01580 {
01581 mat_ma = cpl_matrix_product_create(mat, ma);
01582 if (mat_ma != NULL)
01583 {
01584 cov = cpl_matrix_invert_create(mat_ma);
01585
01586
01587
01588
01589 variance_cpl = cpl_polynomial_new(2);
01590 }
01591 }
01592 xsh_free_matrix(&mat);
01593 xsh_free_matrix(&mat_ma);
01594 }
01595
01596
01597 mx = cpl_matrix_solve_normal(ma, mb) ;
01598
01599 xsh_free_matrix(&ma) ;
01600 xsh_free_matrix(&mb) ;
01601 if (mx == NULL) {
01602 cpl_free(degx_tab) ;
01603 cpl_free(degy_tab) ;
01604 xsh_free_matrix(&cov) ;
01605 assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed") ;
01606 }
01607
01608
01609 out = cpl_polynomial_new(2) ;
01610 powers = cpl_malloc(2 * sizeof(int)) ;
01611 if (powers == NULL) {
01612 cpl_free(degx_tab) ;
01613 cpl_free(degy_tab) ;
01614 xsh_free_matrix(&mx) ;
01615 xsh_free_matrix(&cov) ;
01616 xsh_free_polynomial(&out) ;
01617 assure_mem( false );
01618 }
01619
01620 {
01621 int i;
01622 for (i = 0 ; i < nc ; i++)
01623 {
01624 powers[0] = degx_tab[i] ;
01625 powers[1] = degy_tab[i] ;
01626 cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0)) ;
01627
01628
01629 if (variance != NULL &&
01630 cov != NULL && variance_cpl != NULL
01631 )
01632 {
01633 int j;
01634 for (j = 0; j < nc; j++)
01635 {
01636 double coeff;
01637
01638
01639
01640
01641
01642
01643 powers[0] = degx_tab[i] + degx_tab[j] ;
01644 powers[1] = degy_tab[i] + degy_tab[j] ;
01645
01646 coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
01647 cpl_polynomial_set_coeff(variance_cpl, powers,
01648 coeff + cpl_matrix_get(cov, i, j)) ;
01649 }
01650 }
01651 }
01652 }
01653
01654 cpl_free(powers) ;
01655 cpl_free(degx_tab) ;
01656 cpl_free(degy_tab) ;
01657 xsh_free_matrix(&cov) ;
01658 xsh_free_matrix(&mx) ;
01659
01660
01661 result = xsh_polynomial_new(out);
01662 xsh_free_polynomial(&out);
01663 xsh_polynomial_shift(result, 0, mean_z);
01664 xsh_polynomial_shift(result, 1, mean_x);
01665 xsh_polynomial_shift(result, 2, mean_y);
01666
01667
01668 if (variance != NULL)
01669 {
01670 *variance = xsh_polynomial_new(variance_cpl);
01671 xsh_free_polynomial(&variance_cpl);
01672
01673
01674
01675
01676 xsh_polynomial_shift(*variance, 1, mean_x);
01677 xsh_polynomial_shift(*variance, 2, mean_y);
01678
01679
01680
01681 }
01682
01683
01684 if (mse != NULL || red_chisq != NULL)
01685 {
01686 int i;
01687
01688 if (mse != NULL) *mse = 0.00 ;
01689 if (red_chisq != NULL) *red_chisq = 0.00 ;
01690 for (i = 0 ; i < np ; i++)
01691 {
01692 double regress = xsh_polynomial_evaluate_2d(result,
01693 xy_pos_data_x[i],
01694 xy_pos_data_y[i]);
01695
01696 if (mse != NULL)
01697 {
01698 double residual = values_data[i] - regress;
01699 *mse += residual*residual;
01700 }
01701 if (red_chisq != NULL)
01702 {
01703 *red_chisq += xsh_pow_int((values_data[i] - regress) /
01704 sigmas_data[i], 2);
01705 }
01706 }
01707
01708 if (mse != NULL) *mse /= (double) np ;
01709
01710 if (red_chisq != NULL)
01711 {
01712 passure( np > nc, "%d %d", np, nc);
01713 *red_chisq /= (double) (np - nc) ;
01714 }
01715 }
01716
01717 cleanup:
01718 return result ;
01719 }
01720
01721