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 #ifdef HAVE_CONFIG_H
00028 # include <config.h>
00029 #endif
00030
00031 #include <math.h>
00032
00033 #include <cxmemory.h>
00034 #include <cxmessages.h>
00035
00036 #include "gimath.h"
00037
00038
00048 inline static cxdouble
00049 _giraffe_chebyshev2d_eval(cxdouble ax, cxdouble bx, cxdouble ay, cxdouble by,
00050 const cpl_matrix* coeffs, cxdouble x, cxdouble y)
00051 {
00052
00053 cxint i, k;
00054 cxint xorder;
00055 cxint yorder;
00056
00057 const cxdouble* _coeffs = NULL;
00058
00059 cxdouble x_n = (2.0 * x - ax - bx) / (bx - ax);
00060 cxdouble y_n = (2.0 * y - ay - by) / (by - ay);
00061 cxdouble cx0 = 1.0;
00062 cxdouble cx1 = x_n;
00063 cxdouble sum = 0.;
00064
00065
00066 cx_assert(coeffs != NULL);
00067
00068 xorder = cpl_matrix_get_nrow(coeffs);
00069 yorder = cpl_matrix_get_ncol(coeffs);
00070
00071 _coeffs = cpl_matrix_get_data_const(coeffs);
00072 cx_assert(_coeffs != NULL);
00073
00074 for (i = 0, k = 0; i < xorder; i++) {
00075
00076 register cxint j;
00077
00078 register cxdouble cx2 = 0.;
00079 register cxdouble cy0 = 1.0;
00080 register cxdouble cy1 = y_n;
00081
00082
00083 if (i < 2) {
00084 cx2 = cx0;
00085 }
00086 else {
00087 cx2 = 2.0 * cx1 * x_n - cx0;
00088 }
00089
00090 for (j = 0; j < yorder; j++) {
00091
00092 cxdouble cy2 = 0.;
00093
00094 if (j < 2) {
00095 cy2 = cy0;
00096 }
00097 else {
00098 cy2 = 2.0 * cy1 * y_n - cy0;
00099 }
00100
00101 sum += cx2 * cy2 * _coeffs[k++];
00102
00103 cy0 = cy1;
00104 cy1 = cy2;
00105
00106 }
00107
00108 cx0 = cx1;
00109 cx1 = cx2;
00110
00111 }
00112
00113 return sum;
00114
00115 }
00116
00117
00118 cxdouble
00119 giraffe_interpolate_linear(cxdouble x, cxdouble x_0, cxdouble y_0,
00120 cxdouble x_1, cxdouble y_1)
00121 {
00122
00123 register cxdouble t = (x - x_0) / (x_1 - x_0);
00124
00125 return (1. - t) * y_0 + t * y_1;
00126
00127 }
00128
00129
00144 cpl_matrix *
00145 giraffe_chebyshev_base1d(cxdouble start, cxdouble size, cxint order,
00146 cpl_matrix *m_x)
00147 {
00148
00149 register cxint32 i, j, x_nrow, t_ncol;
00150
00151 register cxdouble xn, dsize2, d2size;
00152
00153 cxdouble *pmx, *pmt;
00154
00155 cpl_matrix *m_t = NULL;
00156
00157
00158
00159
00160
00161
00162 dsize2 = size / 2.0;
00163 d2size = 2.0 / size;
00164
00165 x_nrow = cpl_matrix_get_nrow(m_x);
00166
00167 m_t = cpl_matrix_new(order, x_nrow);
00168
00169 if (m_t == NULL) {
00170 return NULL;
00171 }
00172
00173 t_ncol = x_nrow;
00174
00175 pmx = cpl_matrix_get_data(m_x);
00176 pmt = cpl_matrix_get_data(m_t);
00177
00178
00179
00180
00181
00182
00183 for (j = 0; j < t_ncol; j++) {
00184
00185
00186
00187
00188
00189
00190 xn = (pmx[j] - start - dsize2) * d2size;
00191
00192
00193
00194
00195
00196 pmt[j] = 1.0;
00197
00198 if (order < 2) {
00199 continue;
00200 }
00201
00202
00203
00204
00205
00206 pmt[j+t_ncol] = xn;
00207
00208 for (i = 2; i < order; i++) {
00209
00210
00211
00212
00213
00214
00215 pmt[j+i*t_ncol] = 2.0 * xn * pmt[j+(i-1)*t_ncol] -
00216 pmt[j+(i-2)*t_ncol];
00217
00218 }
00219 }
00220
00221 return m_t;
00222
00223 }
00224
00225
00246 cpl_matrix *
00247 giraffe_chebyshev_base2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
00248 cxdouble ysize, cxint xorder, cxint yorder,
00249 cpl_matrix *m_x, cpl_matrix *m_y)
00250 {
00251
00252 register cxint32 i, j, k, l;
00253
00254 cxint32 x_nrow, y_nrow, t_nrow, t_ncol;
00255
00256 register cxdouble dxsize2, d2xsize;
00257 register cxdouble dysize2, d2ysize;
00258 register cxdouble x_n, y_n;
00259 register cxdouble bx0, bx1, bx2, by0, by1, by2;
00260
00261 cxdouble *pmx, *pmy, *pmt;
00262
00263 cpl_matrix *m_t = NULL;
00264
00265
00266
00267
00268
00269
00270 dxsize2 = xsize / 2.0;
00271 d2xsize = 2.0 / xsize;
00272 dysize2 = ysize / 2.0;
00273 d2ysize = 2.0 / ysize;
00274
00275 x_nrow = cpl_matrix_get_nrow(m_x);
00276 y_nrow = cpl_matrix_get_nrow(m_y);
00277
00278 if (x_nrow != y_nrow) {
00279 return NULL;
00280 }
00281
00282 m_t = cpl_matrix_new(xorder * yorder, x_nrow);
00283
00284 if (m_t == NULL) {
00285 return NULL;
00286 }
00287
00288 t_nrow = cpl_matrix_get_nrow(m_t);
00289 t_ncol = cpl_matrix_get_ncol(m_t);
00290
00291 pmx = cpl_matrix_get_data(m_x);
00292 pmy = cpl_matrix_get_data(m_y);
00293 pmt = cpl_matrix_get_data(m_t);
00294
00295
00296
00297
00298
00299
00300 for (j = 0; j < t_ncol; j++) {
00301
00302
00303
00304
00305
00306
00307 x_n = (pmx[j] - xstart - dxsize2) * d2xsize;
00308 y_n = (pmy[j] - ystart - dysize2) * d2ysize;
00309
00310 bx0 = 1.0;
00311 bx1 = x_n;
00312
00313 for (l = 0,i = 0; i < xorder; i++) {
00314
00315 if (i < 2) {
00316 bx2 = bx0;
00317 }
00318 else {
00319 bx2 = 2.0 * bx1 * x_n - bx0;
00320 }
00321
00322 by0 = 1.0;
00323 by1 = y_n;
00324
00325 for (k = 0; k < yorder; k++) {
00326
00327 if (k < 2) {
00328 by2 = by0;
00329 }
00330 else {
00331 by2 = 2.0 * by1 * y_n - by0;
00332 }
00333
00334 pmt[j + (l++) * t_ncol] = bx2 * by2;
00335
00336 by0 = by1;
00337 by1 = by2;
00338
00339 }
00340
00341 bx0 = bx1;
00342 bx1 = bx2;
00343
00344 }
00345 }
00346
00347 return m_t;
00348
00349 }
00350
00351
00370 cpl_matrix *
00371 giraffe_chebyshev_base2dt(cxdouble xstart, cxdouble ystart, cxdouble xsize,
00372 cxdouble ysize, cxint xorder, cxint yorder,
00373 cpl_matrix *m_x, cpl_matrix *m_y)
00374 {
00375
00376 register cxint32 i, j, k, l;
00377
00378 cxint32 x_nrow, y_nrow, t_nrow, t_ncol;
00379
00380 register cxdouble dxsize2, d2xsize;
00381 register cxdouble dysize2, d2ysize;
00382 register cxdouble x_n, y_n;
00383 register cxdouble bx0, bx1, bx2, by0, by1, by2;
00384
00385 cxdouble *pmx, *pmy, *pmt;
00386
00387 cpl_matrix *m_t = NULL;
00388
00389
00390
00391
00392
00393
00394 dxsize2 = xsize / 2.0;
00395 d2xsize = 2.0 / xsize;
00396 dysize2 = ysize / 2.0;
00397 d2ysize = 2.0 / ysize;
00398
00399 x_nrow = cpl_matrix_get_nrow(m_x);
00400 y_nrow = cpl_matrix_get_nrow(m_y);
00401
00402 if (x_nrow != y_nrow) {
00403 return NULL;
00404 }
00405
00406 m_t = cpl_matrix_new(x_nrow, xorder * yorder);
00407
00408 if (m_t == NULL) {
00409 return NULL;
00410 }
00411
00412 t_nrow = cpl_matrix_get_nrow(m_t);
00413 t_ncol = cpl_matrix_get_ncol(m_t);
00414
00415 pmx = cpl_matrix_get_data(m_x);
00416 pmy = cpl_matrix_get_data(m_y);
00417 pmt = cpl_matrix_get_data(m_t);
00418
00419
00420
00421
00422
00423
00424 for (j = 0; j < t_nrow; j++) {
00425
00426
00427
00428
00429
00430
00431 x_n = (pmx[j] - xstart - dxsize2) * d2xsize;
00432 y_n = (pmy[j] - ystart - dysize2) * d2ysize;
00433
00434 bx0 = 1.0;
00435 bx1 = x_n;
00436
00437 for (l = 0, i = 0; i < xorder; i++) {
00438
00439 if (i < 2) {
00440 bx2 = bx0;
00441 }
00442 else {
00443 bx2 = 2.0 * bx1 * x_n - bx0;
00444 }
00445
00446 by0 = 1.0;
00447 by1 = y_n;
00448
00449 for (k = 0; k < yorder; k++) {
00450
00451 if (k < 2) {
00452 by2 = by0;
00453 }
00454 else {
00455 by2 = 2.0 * by1 * y_n - by0;
00456 }
00457
00458 pmt[j * t_ncol + (l++)] = bx2 * by2;
00459
00460 by0 = by1;
00461 by1 = by2;
00462
00463 }
00464
00465 bx0 = bx1;
00466 bx1 = bx2;
00467
00468 }
00469 }
00470
00471 return m_t;
00472 }
00473
00474
00489 cpl_matrix *
00490 giraffe_chebyshev_fit1d(cxdouble start, cxdouble size, cpl_matrix *m_c,
00491 cpl_matrix *m_x)
00492 {
00493
00494 register cxint32 i, j, jj, order;
00495
00496 cxint32 x_nrow, c_nrow, c_ncol, t_nrow, t_ncol;
00497
00498 register cxdouble xn;
00499 register cxdouble dsize2, d2size;
00500 register cxdouble *f0 = NULL;
00501 register cxdouble *t0 = NULL;
00502 register cxdouble *c0 = NULL;
00503
00504 cxdouble *pmc, *pmx, *pmt, *pmf;
00505
00506 cpl_matrix *m_t;
00507 cpl_matrix *m_f;
00508
00509
00510
00511
00512
00513
00514 dsize2 = size / 2.0;
00515 d2size = 2.0 / size;
00516
00517 c_nrow = cpl_matrix_get_nrow(m_c);
00518 c_ncol = cpl_matrix_get_ncol(m_c);
00519 x_nrow = cpl_matrix_get_nrow(m_x);
00520 order = c_nrow;
00521
00522 m_t = cpl_matrix_new(x_nrow, order);
00523
00524 if (m_t == NULL) {
00525 return NULL;
00526 }
00527
00528 m_f = cpl_matrix_new(c_nrow, x_nrow);
00529
00530 if (m_f == NULL) {
00531 return NULL;
00532 }
00533
00534 t_nrow = cpl_matrix_get_nrow(m_t);
00535 t_ncol = cpl_matrix_get_ncol(m_t);
00536
00537 pmc = cpl_matrix_get_data(m_c);
00538 pmx = cpl_matrix_get_data(m_x);
00539 pmt = cpl_matrix_get_data(m_t);
00540 pmf = cpl_matrix_get_data(m_f);
00541
00542
00543
00544
00545
00546
00547 for (j = 0; j < t_nrow; j++) {
00548
00549
00550
00551
00552
00553
00554 xn = (pmx[j] - start - dsize2) * d2size;
00555
00556
00557
00558
00559
00560 jj = j * t_ncol;
00561
00562 pmt[jj] = 1.0;
00563
00564 if (order < 2) {
00565 continue;
00566 }
00567
00568
00569
00570
00571
00572 pmt[jj+1] = xn;
00573
00574 for (i = 2; i < order; i++) {
00575
00576
00577
00578
00579
00580
00581 pmt[jj + i] = 2.0 * xn * pmt[jj + (i - 1)] - pmt[jj + (i - 2)];
00582
00583 }
00584 }
00585
00586 for (i = 0, f0 = pmf ; i < c_nrow; i++) {
00587 for (j = 0, t0 = pmt; j < t_nrow; j++, f0++) {
00588 for (jj = 0, *f0 = 0, c0 = pmc + i * c_ncol; jj < c_ncol; jj++) {
00589 *f0 += *c0++ * *t0++;
00590 }
00591 }
00592 }
00593
00594 cpl_matrix_delete(m_t);
00595
00596 return m_f;
00597
00598 }
00599
00600
00618 cpl_matrix *
00619 giraffe_chebyshev_fit2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
00620 cxdouble ysize, const cpl_matrix* m_c,
00621 const cpl_matrix* m_x, const cpl_matrix* m_y)
00622 {
00623
00624 cxint i;
00625 cxint nx;
00626
00627 const cxdouble* _x = NULL;
00628 const cxdouble* _y = NULL;
00629
00630 cxdouble bx = xstart + xsize;
00631 cxdouble by = ystart + ysize;
00632
00633 cpl_matrix *f = NULL;
00634
00635
00636 if (m_c == NULL || m_x == NULL || m_y == NULL) {
00637 return NULL;
00638 }
00639
00640 nx = cpl_matrix_get_nrow(m_x);
00641
00642 if (nx != cpl_matrix_get_nrow(m_y)) {
00643 return NULL;
00644 }
00645
00646 f = cpl_matrix_new(nx, 1);
00647
00648 if (f == NULL) {
00649 return NULL;
00650 }
00651
00652 _x = cpl_matrix_get_data_const(m_x);
00653 _y = cpl_matrix_get_data_const(m_y);
00654
00655
00656 for (i = 0; i < nx; i++) {
00657
00658 cxdouble sum = _giraffe_chebyshev2d_eval(xstart, bx, ystart, by,
00659 m_c, _x[i], _y[i]);
00660 cpl_matrix_set(f, i, 0, sum);
00661
00662 }
00663
00664 return f;
00665
00666 }
00667
00668
00669 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
00670
00698 cxint
00699 giraffe_gauss_jordan(
00700 cpl_matrix *mA,
00701 cxint n,
00702 cpl_matrix *mB,
00703 cxint m
00704 ) {
00705
00706 cxint *indxc, *indxr, *ipiv;
00707 register cxint i, icol = 0, irow = 0, j, jj, k, l, ll;
00708 register cxdouble big, dum, pivinv, swap;
00709
00710 cxdouble *pd_mA = NULL, *pd_mB = NULL;
00711 cxint nr_mA, nr_mB;
00712
00713 pd_mA = cpl_matrix_get_data(mA);
00714 pd_mB = cpl_matrix_get_data(mB);
00715 nr_mA = cpl_matrix_get_nrow(mA);
00716 nr_mB = cpl_matrix_get_nrow(mB);
00717
00718 indxc = (cxint *) cx_calloc(n, sizeof(cxint));
00719 indxr = (cxint *) cx_calloc(n, sizeof(cxint));
00720 ipiv = (cxint *) cx_calloc(n, sizeof(cxint));
00721
00722 for (i = 0; i < n; i++) {
00723
00724 big = 0.0;
00725 for (j = 0; j < n; j++) {
00726
00727 jj = j * nr_mA;
00728 if (ipiv[j] != 1) {
00729 for (k = 0; k < n; k++) {
00730 if (ipiv[k] == 0) {
00731 if (fabs(pd_mA[jj + k]) >= big) {
00732 big = fabs(pd_mA[jj + k]);
00733 irow = j;
00734 icol = k;
00735 }
00736 } else if (ipiv[k] > 1) {
00737 cx_free((cxptr) ipiv);
00738 cx_free((cxptr) indxr);
00739 cx_free((cxptr) indxc);
00740
00741 return -1;
00742 }
00743 }
00744 }
00745 }
00746
00747
00748
00749
00750
00751
00752 ++(ipiv[icol]);
00753
00754 if (irow != icol) {
00755 for (l = 0; l < n; l++) {
00756 SWAP(pd_mA[irow * nr_mA + l], pd_mA[icol * nr_mA + l])
00757 }
00758 for (l = 0; l < m; l++) {
00759 SWAP(pd_mB[irow * nr_mB + l], pd_mB[icol * nr_mB + l])
00760 }
00761 }
00762
00763 indxr[i] = irow;
00764 indxc[i] = icol;
00765
00766 if (pd_mA[icol * nr_mA + icol] == 0.0) {
00767 cx_free((cxptr) ipiv);
00768 cx_free((cxptr) indxr);
00769 cx_free((cxptr) indxc);
00770
00771 return -2;
00772 }
00773
00774
00775 pivinv = 1.0 / pd_mA[icol * nr_mA + icol];
00776 pd_mA[icol * nr_mA + icol] = 1.0;
00777
00778 for (l = 0; l < n; l++) {
00779 pd_mA[icol * nr_mA + l] *= pivinv;
00780 }
00781
00782 for (l = 0; l < m; l++) {
00783 pd_mB[icol * nr_mB + l] *= pivinv;
00784 }
00785
00786 for (ll = 0; ll < n; ll++) {
00787
00788 if (ll != icol) {
00789 dum = pd_mA[ll * nr_mA + icol];
00790 pd_mA[ll * nr_mA + icol] = 0.0;
00791
00792 for (l = 0; l < n; l++) {
00793 pd_mA[ll * nr_mA + l] -= pd_mA[icol * nr_mA + l] * dum;
00794 }
00795
00796 for (l = 0; l < m; l++) {
00797 pd_mB[ll * nr_mB + l] -= pd_mB[icol * nr_mB + l] * dum;
00798 }
00799 }
00800 }
00801 }
00802
00803 cx_free((cxptr) ipiv);
00804
00805
00806 for (l = (n-1); l >= 0; l--) {
00807 if (indxr[l] != indxc[l]) {
00808 for (k = 0; k < n; k++) {
00809 SWAP(pd_mA[k * nr_mA + indxr[l]], pd_mA[k * nr_mA + indxc[l]]);
00810 }
00811 }
00812 }
00813 cx_free((cxptr)indxr);
00814 cx_free((cxptr)indxc);
00815
00816 return 0;
00817
00818 }
00819
00820 #undef SWAP
00821
00848 void
00849 giraffe_compute_image_coordinates(
00850 cxlong nx,
00851 cxlong ny,
00852 cpl_matrix *mXi,
00853 cpl_matrix *mYi
00854 ) {
00855
00856 register cxlong i, j, k;
00857 register cxdouble *pd_mXi = NULL, *pd_mYi = NULL;
00858
00859 if ((mXi != NULL) && (mYi != NULL)) {
00860
00861 pd_mXi = cpl_matrix_get_data(mXi);
00862 pd_mYi = cpl_matrix_get_data(mYi);
00863
00864 for (j = 0; j < nx; j++) {
00865 for (k = 0; k < ny; k++) {
00866 i = k + j * ny;
00867 pd_mXi[i] = (cxdouble) j;
00868 pd_mYi[i] = (cxdouble) k;
00869 }
00870 }
00871 } else if (mXi != NULL) {
00872
00873 pd_mXi = cpl_matrix_get_data(mXi);
00874
00875 for (j = 0; j < nx; j++) {
00876 for (k = 0; k < ny; k++) {
00877 i = k + j * ny;
00878 pd_mXi[i] = (cxdouble) j;
00879 }
00880 }
00881 } else if (mYi != NULL) {
00882
00883 pd_mYi = cpl_matrix_get_data(mYi);
00884
00885 for (j = 0; j < nx; j++) {
00886 for (k = 0; k < ny; k++) {
00887 i = k + j * ny;
00888 pd_mYi[i] = (cxdouble) k;
00889 }
00890 }
00891 }
00892
00893 return;
00894
00895 }
00896