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