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 t_nrow = cpl_matrix_get_nrow(m_t);
00521 t_ncol = cpl_matrix_get_ncol(m_t);
00522 order = c_nrow;
00523
00524 m_t = cpl_matrix_new(x_nrow, order);
00525
00526 if (m_t == NULL) {
00527 return NULL;
00528 }
00529
00530 m_f = cpl_matrix_new(c_nrow, x_nrow);
00531
00532 if (m_f == NULL) {
00533 return NULL;
00534 }
00535
00536 pmc = cpl_matrix_get_data(m_c);
00537 pmx = cpl_matrix_get_data(m_x);
00538 pmt = cpl_matrix_get_data(m_t);
00539 pmf = cpl_matrix_get_data(m_f);
00540
00541
00542
00543
00544
00545
00546 for (j = 0; j < t_nrow; j++) {
00547
00548
00549
00550
00551
00552
00553 xn = (pmx[j] - start - dsize2) * d2size;
00554
00555
00556
00557
00558
00559 jj = j * t_ncol;
00560
00561 pmt[jj] = 1.0;
00562
00563 if (order < 2) {
00564 continue;
00565 }
00566
00567
00568
00569
00570
00571 pmt[jj+1] = xn;
00572
00573 for (i = 2; i < order; i++) {
00574
00575
00576
00577
00578
00579
00580 pmt[jj + i] = 2.0 * xn * pmt[jj + (i - 1)] - pmt[jj + (i - 2)];
00581
00582 }
00583 }
00584
00585 for (i = 0, f0 = pmf ; i < c_nrow; i++) {
00586 for (j = 0, t0 = pmt; j < t_nrow; j++, f0++) {
00587 for (jj = 0, *f0 = 0, c0 = pmc + i * c_ncol; jj < c_ncol; jj++) {
00588 *f0 += *c0++ * *t0++;
00589 }
00590 }
00591 }
00592
00593 cpl_matrix_delete(m_t);
00594
00595 return m_f;
00596
00597 }
00598
00599
00617 #if 0
00618 cpl_matrix *
00619 giraffe_chebyshev_fit2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
00620 cxdouble ysize, cpl_matrix *m_c, cpl_matrix *m_x,
00621 cpl_matrix *m_y)
00622 {
00623
00624 register cxint32 i, j, k, l;
00625
00626 cxint32 xorder, yorder;
00627 cxint32 x_nrow, y_nrow, f_nrow, c_nrow, c_ncol;
00628
00629 register cxdouble xn, yn;
00630 register cxdouble dxsize2, dysize2;
00631 register cxdouble d2xsize, d2ysize;
00632 register cxdouble bx0, bx1, bx2, by0, by1, by2;
00633
00634 cxdouble *pmc, *pmx, *pmy, *pmf;
00635
00636 cpl_matrix *m_f = NULL;
00637
00638
00639
00640
00641
00642
00643 c_nrow = cpl_matrix_get_nrow(m_c);
00644 c_ncol = cpl_matrix_get_ncol(m_c);
00645
00646 xorder = c_nrow;
00647 yorder = c_ncol;
00648
00649 dxsize2 = xsize / 2.0;
00650 d2xsize = 2.0 / xsize;
00651 dysize2 = ysize / 2.0;
00652 d2ysize = 2.0 / ysize;
00653
00654 x_nrow = cpl_matrix_get_nrow(m_x);
00655 y_nrow = cpl_matrix_get_nrow(m_y);
00656
00657 if (x_nrow != y_nrow) {
00658 return NULL;
00659 }
00660
00661 m_f = cpl_matrix_new(x_nrow, 1);
00662
00663 if (m_f == NULL) {
00664 return NULL;
00665 }
00666
00667 f_nrow = cpl_matrix_get_nrow(m_f);
00668
00669 pmx = cpl_matrix_get_data(m_x);
00670 pmy = cpl_matrix_get_data(m_y);
00671 pmf = cpl_matrix_get_data(m_f);
00672 pmc = cpl_matrix_get_data(m_c);
00673
00674
00675
00676
00677
00678
00679 for (j = 0; j < f_nrow; j++) {
00680
00681
00682
00683
00684
00685
00686 xn = (pmx[j] - xstart - dxsize2) * d2xsize;
00687 yn = (pmy[j] - ystart - dysize2) * d2ysize;
00688
00689 bx0 = 1.0;
00690 bx1 = xn;
00691
00692 pmf[j] = 0.0;
00693
00694 for (l = 0, i = 0; i < xorder; i++) {
00695
00696 if (i < 2) {
00697 bx2 = bx0;
00698 }
00699 else {
00700 bx2 = 2.0 * bx1 * xn - bx0;
00701 }
00702
00703 by0 = 1.0;
00704 by1 = yn;
00705 for (k = 0; k < yorder; k++) {
00706
00707 if (k < 2) {
00708 by2 = by0;
00709 }
00710 else {
00711 by2 = 2.0 * by1 * yn - by0;
00712 }
00713
00714 pmf[j] += (bx2 * by2 * pmc[l++]);
00715
00716 by0 = by1;
00717 by1 = by2;
00718 }
00719 bx0 = bx1;
00720 bx1 = bx2;
00721 }
00722 }
00723
00724 return m_f;
00725
00726 }
00727 #else
00728 cpl_matrix *
00729 giraffe_chebyshev_fit2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
00730 cxdouble ysize, const cpl_matrix* m_c,
00731 const cpl_matrix* m_x, const cpl_matrix* m_y)
00732 {
00733
00734 cxint i;
00735 cxint nx;
00736
00737 const cxdouble* _x = NULL;
00738 const cxdouble* _y = NULL;
00739
00740 cxdouble bx = xstart + xsize;
00741 cxdouble by = ystart + ysize;
00742
00743 cpl_matrix *f = NULL;
00744
00745
00746 if (m_c == NULL || m_x == NULL || m_y == NULL) {
00747 return NULL;
00748 }
00749
00750 nx = cpl_matrix_get_nrow(m_x);
00751
00752 if (nx != cpl_matrix_get_nrow(m_y)) {
00753 return NULL;
00754 }
00755
00756 f = cpl_matrix_new(nx, 1);
00757
00758 if (f == NULL) {
00759 return NULL;
00760 }
00761
00762 _x = cpl_matrix_get_data_const(m_x);
00763 _y = cpl_matrix_get_data_const(m_y);
00764
00765
00766 for (i = 0; i < nx; i++) {
00767
00768 cxdouble sum = _giraffe_chebyshev2d_eval(xstart, bx, ystart, by,
00769 m_c, _x[i], _y[i]);
00770 cpl_matrix_set(f, i, 0, sum);
00771
00772 }
00773
00774 return f;
00775
00776 }
00777 #endif
00778
00779
00780 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
00781
00809 cxint
00810 giraffe_gauss_jordan(
00811 cpl_matrix *mA,
00812 cxint n,
00813 cpl_matrix *mB,
00814 cxint m
00815 ) {
00816
00817 cxint *indxc, *indxr, *ipiv;
00818 register cxint i, icol = 0, irow = 0, j, jj, k, l, ll;
00819 register cxdouble big, dum, pivinv, swap;
00820
00821 cxdouble *pd_mA = NULL, *pd_mB = NULL;
00822 cxint nr_mA, nr_mB;
00823
00824 pd_mA = cpl_matrix_get_data(mA);
00825 pd_mB = cpl_matrix_get_data(mB);
00826 nr_mA = cpl_matrix_get_nrow(mA);
00827 nr_mB = cpl_matrix_get_nrow(mB);
00828
00829 indxc = (cxint *) cx_calloc(n, sizeof(cxint));
00830 indxr = (cxint *) cx_calloc(n, sizeof(cxint));
00831 ipiv = (cxint *) cx_calloc(n, sizeof(cxint));
00832
00833 for (i = 0; i < n; i++) {
00834
00835 big = 0.0;
00836 for (j = 0; j < n; j++) {
00837
00838 jj = j * nr_mA;
00839 if (ipiv[j] != 1) {
00840 for (k = 0; k < n; k++) {
00841 if (ipiv[k] == 0) {
00842 if (fabs(pd_mA[jj + k]) >= big) {
00843 big = fabs(pd_mA[jj + k]);
00844 irow = j;
00845 icol = k;
00846 }
00847 } else if (ipiv[k] > 1) {
00848 cx_free((cxptr) ipiv);
00849 cx_free((cxptr) indxr);
00850 cx_free((cxptr) indxc);
00851
00852 return -1;
00853 }
00854 }
00855 }
00856 }
00857
00858
00859
00860
00861
00862
00863 ++(ipiv[icol]);
00864
00865 if (irow != icol) {
00866 for (l = 0; l < n; l++) {
00867 SWAP(pd_mA[irow * nr_mA + l], pd_mA[icol * nr_mA + l])
00868 }
00869 for (l = 0; l < m; l++) {
00870 SWAP(pd_mB[irow * nr_mB + l], pd_mB[icol * nr_mB + l])
00871 }
00872 }
00873
00874 indxr[i] = irow;
00875 indxc[i] = icol;
00876
00877 if (pd_mA[icol * nr_mA + icol] == 0.0) {
00878 cx_free((cxptr) ipiv);
00879 cx_free((cxptr) indxr);
00880 cx_free((cxptr) indxc);
00881
00882 return -2;
00883 }
00884
00885
00886 pivinv = 1.0 / pd_mA[icol * nr_mA + icol];
00887 pd_mA[icol * nr_mA + icol] = 1.0;
00888
00889 for (l = 0; l < n; l++) {
00890 pd_mA[icol * nr_mA + l] *= pivinv;
00891 }
00892
00893 for (l = 0; l < m; l++) {
00894 pd_mB[icol * nr_mB + l] *= pivinv;
00895 }
00896
00897 for (ll = 0; ll < n; ll++) {
00898
00899 if (ll != icol) {
00900 dum = pd_mA[ll * nr_mA + icol];
00901 pd_mA[ll * nr_mA + icol] = 0.0;
00902
00903 for (l = 0; l < n; l++) {
00904 pd_mA[ll * nr_mA + l] -= pd_mA[icol * nr_mA + l] * dum;
00905 }
00906
00907 for (l = 0; l < m; l++) {
00908 pd_mB[ll * nr_mB + l] -= pd_mB[icol * nr_mB + l] * dum;
00909 }
00910 }
00911 }
00912 }
00913
00914 cx_free((cxptr) ipiv);
00915
00916
00917 for (l = (n-1); l >= 0; l--) {
00918 if (indxr[l] != indxc[l]) {
00919 for (k = 0; k < n; k++) {
00920 SWAP(pd_mA[k * nr_mA + indxr[l]], pd_mA[k * nr_mA + indxc[l]]);
00921 }
00922 }
00923 }
00924 cx_free((cxptr)indxr);
00925 cx_free((cxptr)indxc);
00926
00927 return 0;
00928
00929 }
00930
00931 #undef SWAP
00932
00959 void
00960 giraffe_compute_image_coordinates(
00961 cxlong nx,
00962 cxlong ny,
00963 cpl_matrix *mXi,
00964 cpl_matrix *mYi
00965 ) {
00966
00967 register cxlong i, j, k;
00968 register cxdouble *pd_mXi = NULL, *pd_mYi = NULL;
00969
00970 if ((mXi != NULL) && (mYi != NULL)) {
00971
00972 pd_mXi = cpl_matrix_get_data(mXi);
00973 pd_mYi = cpl_matrix_get_data(mYi);
00974
00975 for (j = 0; j < nx; j++) {
00976 for (k = 0; k < ny; k++) {
00977 i = k + j * ny;
00978 pd_mXi[i] = (cxdouble) j;
00979 pd_mYi[i] = (cxdouble) k;
00980 }
00981 }
00982 } else if (mXi != NULL) {
00983
00984 pd_mXi = cpl_matrix_get_data(mXi);
00985
00986 for (j = 0; j < nx; j++) {
00987 for (k = 0; k < ny; k++) {
00988 i = k + j * ny;
00989 pd_mXi[i] = (cxdouble) j;
00990 }
00991 }
00992 } else if (mYi != NULL) {
00993
00994 pd_mYi = cpl_matrix_get_data(mYi);
00995
00996 for (j = 0; j < nx; j++) {
00997 for (k = 0; k < ny; k++) {
00998 i = k + j * ny;
00999 pd_mYi[i] = (cxdouble) k;
01000 }
01001 }
01002 }
01003
01004 return;
01005
01006 }
01007