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 #include <math.h>
00033
00034 #include <cxmessages.h>
00035 #include <cxstring.h>
00036
00037 #include <cpl_msg.h>
00038 #include <cpl_error.h>
00039
00040 #include "gimatrix.h"
00041
00042
00051 inline static void
00052 _giraffe_swap(cxdouble *a, cxdouble *b)
00053 {
00054 register cxdouble tmp = *a;
00055
00056 *a = *b;
00057 *b = tmp;
00058
00059 return;
00060
00061 }
00062
00063
00064 inline static cxbool
00065 _giraffe_tiny(cxdouble a)
00066 {
00067 return a < 0. ? (a > -1.e-30) : (a < 1.e-30);
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 static cxint
00087 _giraffe_matrix_gausspiv(cxdouble *ptra, cxdouble *ptrc, cxint n)
00088
00089 {
00090
00091 register cxint i;
00092 register cxint j;
00093 register cxint k;
00094 register cxint l;
00095
00096 cxint maj;
00097
00098 cxdouble max;
00099 cxdouble r;
00100 cxdouble t;
00101 cxdouble *ptrb;
00102
00103
00104 ptrb = (cxdouble *)cx_calloc(n * n, sizeof(cxdouble));
00105
00106 for(i = 0; i < n; i++) {
00107 ptrb[i * n + i] = 1.0;
00108 }
00109
00110 for (i = 1; i <= n; i++) {
00111
00112
00113 max = CX_ABS(*(ptra + n * i - n));
00114 maj = i;
00115
00116 for (j = i; j <= n; j++) {
00117 if (CX_ABS(*(ptra + n * j + i - n - 1)) > max) {
00118 maj = j;
00119 max = CX_ABS(*(ptra + n * j + i - n - 1));
00120 }
00121 }
00122
00123
00124 if (maj != i) {
00125 for (j = i;j <= n;j++) {
00126 r = *(ptra + n * maj + j - n - 1);
00127 *(ptra + n * maj + j - n - 1) = *(ptra + n * i + j - n - 1);
00128 *(ptra + n * i + j - n - 1) = r;
00129 }
00130
00131 for(l = 0; l < n; l++) {
00132 r = *(ptrb + l * n + maj - 1);
00133 *(ptrb + l * n + maj - 1) = *(ptrb + l * n + i - 1);
00134 *(ptrb + l * n + i - 1) = r;
00135 }
00136 }
00137
00138
00139 for (j = i + 1; j <= n; j++) {
00140 t = (*(ptra + (n + 1) * i - n - 1));
00141 if (_giraffe_tiny(t) == TRUE) {
00142 return 0;
00143 }
00144 r = (*(ptra + n * j + i - n - 1)) / t;
00145 for(l = 0; l < n; l++) {
00146 *(ptrb + l * n + j - 1) -= r * (*(ptrb + l * n + i - 1));
00147 }
00148 for (k = i; k <= n; k++) {
00149 *(ptra + n * j + k - n - 1) -=
00150 r * (*(ptra + n * i + k - n - 1));
00151 }
00152 }
00153 }
00154
00155
00156 for(l = 0; l < n; l++) {
00157 for (i = n; i >= 1; i--) {
00158 t = (*(ptra + (n + 1) * i - n - 1));
00159 if (_giraffe_tiny(t) == TRUE) {
00160 return 0;
00161 }
00162 *(ptrc + l + (i - 1) * n) = (*(ptrb + l * n + i - 1)) / t;
00163 if (i > 1) {
00164 for (j = i - 1;j > 0;j--) {
00165 *(ptrb + l * n + j - 1) -=
00166 (*(ptra + n * j + i - n - 1)) *
00167 (*(ptrc + l + (i - 1) * n));
00168 }
00169 }
00170 }
00171 }
00172 cx_free(ptrb);
00173
00174 return 1;
00175 }
00176
00177
00178 static cpl_matrix *
00179 _giraffe_matrix_inverse(cpl_matrix *aa)
00180 {
00181 cxint test = 1;
00182 cxint aa_ncol = 0;
00183 cxint aa_nrow = 0;
00184
00185 cxdouble *pd_temp = NULL;
00186 cxdouble *pd_bb = NULL;
00187
00188 cpl_matrix *bb = NULL;
00189 cpl_matrix *temp = NULL;
00190
00191 aa_ncol = cpl_matrix_get_ncol(aa);
00192 aa_nrow = cpl_matrix_get_nrow(aa);
00193
00194 if(aa_nrow != aa_ncol) {
00195 return NULL;
00196 }
00197
00198 bb = cpl_matrix_new(aa_nrow, aa_ncol);
00199
00200 temp = cpl_matrix_duplicate(aa);
00201
00202 pd_temp = cpl_matrix_get_data(temp);
00203 pd_bb = cpl_matrix_get_data(bb);
00204
00205 if (_giraffe_matrix_gausspiv(pd_temp, pd_bb, aa_nrow) == 0) {
00206 test = 0;
00207 }
00208
00209 cpl_matrix_delete(temp);
00210
00211 if (test == 0) {
00212 cpl_matrix_delete(bb);
00213 return NULL;
00214 }
00215
00216 return bb;
00217 }
00218
00219
00236 cxdouble
00237 giraffe_matrix_sigma_mean(const cpl_matrix *matrix, cxdouble mean)
00238 {
00239
00240 cxulong size = 0;
00241 cxulong size2 = 0;
00242
00243 const cxdouble *pt = NULL;
00244
00245 cxdouble diff = 0.;
00246 cxdouble sigma = 0.;
00247
00248
00249 cx_assert(matrix != NULL);
00250
00251 size = cpl_matrix_get_ncol(matrix) * cpl_matrix_get_nrow(matrix);
00252 size2 = size - 1;
00253
00254 pt = cpl_matrix_get_data_const(matrix);
00255
00256 while (size--) {
00257 diff = *pt++ - mean;
00258 sigma += diff * diff;
00259 }
00260
00261 return sqrt(sigma / (cxdouble)size2);
00262
00263 }
00264
00265
00282 cxdouble
00283 giraffe_matrix_sigma_fit(const cpl_matrix *matrix,
00284 const cpl_matrix *matrix_fit)
00285 {
00286
00287 cxint ancol;
00288 cxint anrow;
00289 cxint fncol;
00290 cxint fnrow;
00291
00292 cxulong size;
00293 cxulong size2;
00294
00295 const cxdouble *pta = NULL;
00296 const cxdouble *ptf = NULL;
00297
00298 cxdouble diff = 0.;
00299 cxdouble sigma = 0.;
00300
00301
00302 cx_assert(matrix != NULL);
00303 cx_assert(matrix_fit != NULL);
00304
00305 ancol = cpl_matrix_get_ncol(matrix);
00306 anrow = cpl_matrix_get_nrow(matrix);
00307 fncol = cpl_matrix_get_ncol(matrix_fit);
00308 fnrow = cpl_matrix_get_nrow(matrix_fit);
00309
00310 if ((ancol * anrow) != (fncol * fnrow)) {
00311 return 0.0;
00312 }
00313
00314 size = ancol * anrow;
00315 size2 = size - 1;
00316
00317 pta = cpl_matrix_get_data_const(matrix);
00318 ptf = cpl_matrix_get_data_const(matrix_fit);
00319
00320 while (size--) {
00321 diff = *pta++ - *ptf++;
00322 sigma += diff * diff;
00323 }
00324
00325 return sqrt(sigma / (cxdouble) size2);
00326
00327 }
00328
00329
00344 cpl_image *
00345 giraffe_matrix_create_image(const cpl_matrix *matrix)
00346 {
00347
00348 cpl_image *image = NULL;
00349
00350
00351 if (matrix) {
00352 cxint nx = cpl_matrix_get_ncol(matrix);
00353 cxint ny = cpl_matrix_get_nrow(matrix);
00354
00355
00356 image = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
00357
00358 if (image) {
00359 cxsize sz = nx * ny;
00360 cxdouble *pixels = cpl_image_get_data_double(image);
00361
00362 memcpy(pixels, cpl_matrix_get_data_const(matrix),
00363 sz * sizeof(cxdouble));
00364 }
00365 }
00366
00367 return image;
00368
00369 }
00370
00371 #define PIX_STACK_SIZE 50
00372
00387 cxint
00388 giraffe_matrix_sort(cpl_matrix *mA)
00389 {
00390 register cxint i;
00391 register cxint ir;
00392 register cxint j;
00393 register cxint j_stack;
00394 register cxint k;
00395 register cxint l;
00396
00397 register cxdouble a;
00398 register cxdouble *pix_arr = NULL;
00399
00400 cxint i_stack[PIX_STACK_SIZE] ;
00401
00402
00403 pix_arr = cpl_matrix_get_data(mA);
00404 ir = cpl_matrix_get_nrow(mA) * cpl_matrix_get_ncol(mA);
00405
00406 l = 1 ;
00407 j_stack = 0 ;
00408 for (;;) {
00409 if (ir - l < 7) {
00410 for (j = l + 1 ; j <= ir ; j++) {
00411 a = pix_arr[j - 1];
00412 for (i = j - 1 ; i >= 1 ; i--) {
00413 if (pix_arr[i - 1] <= a) {
00414 break;
00415 }
00416 pix_arr[i] = pix_arr[i - 1];
00417 }
00418 pix_arr[i] = a;
00419 }
00420 if (j_stack == 0) {
00421 break;
00422 }
00423 ir = i_stack[j_stack-- - 1];
00424 l = i_stack[j_stack-- - 1];
00425 }
00426 else {
00427 k = (l + ir) >> 1;
00428 _giraffe_swap(&pix_arr[k - 1], &pix_arr[l]);
00429 if (pix_arr[l] > pix_arr[ir - 1]) {
00430 _giraffe_swap(&pix_arr[l], &pix_arr[ir - 1]);
00431 }
00432 if (pix_arr[l - 1] > pix_arr[ir - 1]) {
00433 _giraffe_swap(&pix_arr[l - 1], &pix_arr[ir - 1]);
00434 }
00435 if (pix_arr[l] > pix_arr[l - 1]) {
00436 _giraffe_swap(&pix_arr[l], &pix_arr[l - 1]);
00437 }
00438 i = l + 1;
00439 j = ir;
00440 a = pix_arr[l - 1];
00441 for (;;) {
00442 do {
00443 i++;
00444 } while (pix_arr[i - 1] < a);
00445
00446 do {
00447 j--;
00448 } while (pix_arr[j - 1] > a);
00449
00450 if (j < i) {
00451 break;
00452 }
00453 _giraffe_swap(&pix_arr[i - 1], &pix_arr[j - 1]);
00454 }
00455 pix_arr[l - 1] = pix_arr[j - 1];
00456 pix_arr[j - 1] = a;
00457 j_stack += 2;
00458 if (j_stack > PIX_STACK_SIZE) {
00459
00460 return -1 ;
00461 }
00462 if (ir - i + 1 >= j - l) {
00463 i_stack[j_stack - 1] = ir;
00464 i_stack[j_stack - 2] = i;
00465 ir = j - 1;
00466 }
00467 else {
00468 i_stack[j_stack - 1] = j - 1;
00469 i_stack[j_stack - 2] = l;
00470 l = i;
00471 }
00472 }
00473 }
00474
00475 return 0;
00476
00477 }
00478
00479 #undef PIX_STACK_SIZE
00480
00481
00510 cpl_matrix *
00511 giraffe_matrix_leastsq(const cpl_matrix* mA, const cpl_matrix* mB)
00512 {
00513
00514 cpl_matrix* m1 = NULL;
00515 cpl_matrix* m2 = NULL;
00516 cpl_matrix* m3 = NULL;
00517 cpl_matrix* mX = NULL;
00518
00519
00520 cx_assert(mA != NULL);
00521 cx_assert(mB != NULL);
00522 cx_assert(cpl_matrix_get_ncol(mA) == cpl_matrix_get_ncol(mB));
00523
00524 m1 = cpl_matrix_transpose_create(mA);
00525 m2 = cpl_matrix_product_create(mA, m1);
00526 m3 = cpl_matrix_invert_create(m2);
00527
00528 if (m3 == NULL) {
00529 cpl_matrix_delete(m2);
00530 m2 = NULL;
00531
00532 cpl_matrix_delete(m1);
00533 m1 = NULL;
00534
00535 return NULL;
00536 }
00537
00538 cpl_matrix_delete(m2);
00539
00540 m2 = cpl_matrix_product_create(mB, m1);
00541
00542 cpl_matrix_delete(m1);
00543 m1 = NULL;
00544
00545 mX = cpl_matrix_product_create(m2, m3);
00546
00547 cpl_matrix_delete(m2);
00548 m2 = NULL;
00549
00550 cpl_matrix_delete(m3);
00551 m3 = NULL;
00552
00553 return mX;
00554
00555 }
00556
00557
00586 cpl_matrix*
00587 giraffe_matrix_solve_cholesky(const cpl_matrix* A, const cpl_matrix* b,
00588 const cpl_matrix* Cb, cpl_matrix* Cx)
00589 {
00590
00591 const char* const _id = "giraffe_matrix_solve_cholesky";
00592
00593 cxint m = 0;
00594 cxint n = 0;
00595
00596 cpl_matrix* AT = NULL;
00597 cpl_matrix* ATC = NULL;
00598 cpl_matrix* ATCA = NULL;
00599 cpl_matrix* ATCb = NULL;
00600 cpl_matrix* C = NULL;
00601 cpl_matrix* X = NULL;
00602 cpl_matrix* x = NULL;
00603
00604 cpl_error_code status = CPL_ERROR_NONE;
00605
00606
00607 if ((A == NULL) || (b == NULL)) {
00608
00609 cpl_error_set(_id, CPL_ERROR_NULL_INPUT);
00610 return NULL;
00611
00612 }
00613
00614 m = cpl_matrix_get_nrow(A);
00615 n = cpl_matrix_get_ncol(A);
00616
00617 if ((cpl_matrix_get_nrow(b) != m) || (cpl_matrix_get_ncol(b) != 1)) {
00618
00619 cpl_error_set(_id, CPL_ERROR_INCOMPATIBLE_INPUT);
00620 return NULL;
00621
00622 }
00623
00624 if (Cb != NULL) {
00625
00626 if ((cpl_matrix_get_nrow(Cb) != m) || (cpl_matrix_get_ncol(Cb) != m)) {
00627 cpl_error_set(_id, CPL_ERROR_INCOMPATIBLE_INPUT);
00628 return NULL;
00629 }
00630
00631 }
00632
00633 if (Cx != NULL) {
00634
00635 if ((cpl_matrix_get_nrow(Cx) != n) || (cpl_matrix_get_ncol(Cx) != n)) {
00636 cpl_error_set(_id, CPL_ERROR_ILLEGAL_INPUT);
00637 return NULL;
00638 }
00639
00640 }
00641
00642
00643 if (Cb != NULL) {
00644
00645
00646
00647
00648
00649
00650 if (cpl_matrix_is_diagonal(Cb, CX_MINDOUBLE) == TRUE) {
00651
00652 register cxint i = 0;
00653
00654 C = cpl_matrix_new(m, m);
00655
00656 for (i = 0; i < m; ++i) {
00657
00658 register cxdouble value = cpl_matrix_get(Cb, i, i);
00659
00660 if (value <= CX_MINDOUBLE) {
00661
00662 cpl_matrix_delete(C);
00663 C = NULL;
00664
00665 break;
00666 }
00667
00668 cpl_matrix_set(C, i, i, 1. / value);
00669
00670 }
00671
00672 }
00673 else {
00674 C = cpl_matrix_invert_create(Cb);
00675 }
00676
00677 if (C == NULL) {
00678 cpl_error_set(_id, CPL_ERROR_SINGULAR_MATRIX);
00679 return NULL;
00680 }
00681
00682 }
00683 else {
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696 C = cpl_matrix_new(m, m);
00697 cpl_matrix_fill_diagonal(C, 1., 0);
00698
00699 }
00700
00701
00702 AT = cpl_matrix_transpose_create(A);
00703 ATC = cpl_matrix_product_create(AT, C);
00704
00705 cpl_matrix_delete(AT);
00706 AT = NULL;
00707
00708 cpl_matrix_delete(C);
00709 C = NULL;
00710
00711
00712 ATCA = cpl_matrix_product_create(ATC, A);
00713 ATCb = cpl_matrix_product_create(ATC, b);
00714
00715 cpl_matrix_delete(ATC);
00716 ATC = NULL;
00717
00718
00719
00720
00721
00722
00723 status = cpl_matrix_decomp_chol(ATCA);
00724
00725 if (status != CPL_ERROR_NONE) {
00726
00727 cpl_matrix_delete(ATCA);
00728 ATCA = NULL;
00729
00730 cpl_matrix_delete(ATCb);
00731 ATCb = NULL;
00732
00733 return NULL;
00734
00735 }
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748 X = cpl_matrix_new(n, n + 1);
00749
00750 cpl_matrix_fill_diagonal(X, 1., 0);
00751 cpl_matrix_copy(X, ATCb, 0, n);
00752
00753 cpl_matrix_delete(ATCb);
00754 ATCb = NULL;
00755
00756
00757 status = cpl_matrix_solve_chol(ATCA, X);
00758
00759 cpl_matrix_delete(ATCA);
00760 ATCA = NULL;
00761
00762 if (status != CPL_ERROR_NONE) {
00763 cpl_matrix_delete(X);
00764 X = NULL;
00765 }
00766
00767
00768
00769
00770
00771
00772
00773 x = cpl_matrix_extract_column(X, n);
00774
00775 if (Cx != NULL) {
00776 cpl_matrix_copy(Cx, X, 0, 0);
00777 }
00778
00779 cpl_matrix_delete(X);
00780 X = NULL;
00781
00782 return x;
00783
00784 }
00785
00786
00787
00788
00789
00803 cxint
00804 giraffe_matrix_clear(cpl_matrix *matrix)
00805 {
00806 cxint nr_matrix;
00807 cxint nc_matrix;
00808
00809 cxdouble *pd_matrix = NULL;
00810
00811 cx_assert(matrix != NULL);
00812
00813 pd_matrix = cpl_matrix_get_data(matrix);
00814 nc_matrix = cpl_matrix_get_ncol(matrix);
00815 nr_matrix = cpl_matrix_get_nrow(matrix);
00816
00817 memset(pd_matrix, 0, nr_matrix * nc_matrix * sizeof(cxdouble));
00818
00819 return 0;
00820
00821 }
00822
00823
00843 void
00844 giraffe_matrix_dump(const cpl_matrix *matrix, cxint max_rows)
00845 {
00846
00847 cxint i;
00848 cxint j;
00849 cxint k;
00850 cxint nc;
00851 cxint nr;
00852 cxint ncw;
00853
00854 const cxdouble *pd_m = NULL;
00855
00856 cx_string *buffer = NULL;
00857 cx_string *tmp = NULL;
00858
00859 if (matrix == NULL) {
00860 return;
00861 }
00862
00863 pd_m = cpl_matrix_get_data_const(matrix);
00864
00865 nr = cpl_matrix_get_nrow(matrix);
00866 nc = cpl_matrix_get_ncol(matrix);
00867
00868 if (nr > max_rows) {
00869 nr = max_rows;
00870 }
00871
00872 buffer = cx_string_new();
00873 tmp = cx_string_new();
00874
00875
00876 for (i = 0; i < nc; i++) {
00877 ncw = cx_string_sprintf(tmp, " %d", i);
00878 cx_string_append(buffer, cx_string_get(tmp));
00879 }
00880
00881 cpl_msg_debug("", cx_string_get(buffer));
00882
00883
00884 for (k = 0, i = 0; i < nr; i++) {
00885 ncw = cx_string_sprintf(buffer," %d", i);
00886 for (j = 0; j < nc; j++, k++) {
00887 ncw = cx_string_sprintf(tmp, " %+18.12f", pd_m[k]);
00888 cx_string_append(buffer, cx_string_get(tmp));
00889 }
00890
00891 cpl_msg_debug("", cx_string_get(buffer));
00892 }
00893
00894 cx_string_delete(tmp);
00895 cx_string_delete(buffer);
00896
00897 return;
00898
00899 }