gimatrix.c
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
00039 #include "gimatrix.h"
00040
00041
00050 inline static void
00051 _giraffe_swap(cxdouble *a, cxdouble *b)
00052 {
00053 register cxdouble tmp = *a;
00054
00055 *a = *b;
00056 *b = tmp;
00057
00058 return;
00059
00060 }
00061
00062
00063 inline static cxbool
00064 _giraffe_tiny(cxdouble a)
00065 {
00066 return a < 0. ? (a > -1.e-30) : (a < 1.e-30);
00067 }
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 static cxint
00086 _giraffe_matrix_gausspiv(cxdouble *ptra, cxdouble *ptrc, cxint n)
00087
00088 {
00089
00090 register cxint i;
00091 register cxint j;
00092 register cxint k;
00093 register cxint l;
00094
00095 cxint maj;
00096
00097 cxdouble max;
00098 cxdouble r;
00099 cxdouble t;
00100 cxdouble *ptrb;
00101
00102
00103 ptrb = (cxdouble *)cx_calloc(n * n, sizeof(cxdouble));
00104
00105 for(i = 0; i < n; i++) {
00106 ptrb[i * n + i] = 1.0;
00107 }
00108
00109 for (i = 1; i <= n; i++) {
00110
00111
00112 max = CX_ABS(*(ptra + n * i - n));
00113 maj = i;
00114
00115 for (j = i; j <= n; j++) {
00116 if (CX_ABS(*(ptra + n * j + i - n - 1)) > max) {
00117 maj = j;
00118 max = CX_ABS(*(ptra + n * j + i - n - 1));
00119 }
00120 }
00121
00122
00123 if (maj != i) {
00124 for (j = i;j <= n;j++) {
00125 r = *(ptra + n * maj + j - n - 1);
00126 *(ptra + n * maj + j - n - 1) = *(ptra + n * i + j - n - 1);
00127 *(ptra + n * i + j - n - 1) = r;
00128 }
00129
00130 for(l = 0; l < n; l++) {
00131 r = *(ptrb + l * n + maj - 1);
00132 *(ptrb + l * n + maj - 1) = *(ptrb + l * n + i - 1);
00133 *(ptrb + l * n + i - 1) = r;
00134 }
00135 }
00136
00137
00138 for (j = i + 1; j <= n; j++) {
00139 t = (*(ptra + (n + 1) * i - n - 1));
00140 if (_giraffe_tiny(t) == TRUE) {
00141 return 0;
00142 }
00143 r = (*(ptra + n * j + i - n - 1)) / t;
00144 for(l = 0; l < n; l++) {
00145 *(ptrb + l * n + j - 1) -= r * (*(ptrb + l * n + i - 1));
00146 }
00147 for (k = i; k <= n; k++) {
00148 *(ptra + n * j + k - n - 1) -=
00149 r * (*(ptra + n * i + k - n - 1));
00150 }
00151 }
00152 }
00153
00154
00155 for(l = 0; l < n; l++) {
00156 for (i = n; i >= 1; i--) {
00157 t = (*(ptra + (n + 1) * i - n - 1));
00158 if (_giraffe_tiny(t) == TRUE) {
00159 return 0;
00160 }
00161 *(ptrc + l + (i - 1) * n) = (*(ptrb + l * n + i - 1)) / t;
00162 if (i > 1) {
00163 for (j = i - 1;j > 0;j--) {
00164 *(ptrb + l * n + j - 1) -=
00165 (*(ptra + n * j + i - n - 1)) *
00166 (*(ptrc + l + (i - 1) * n));
00167 }
00168 }
00169 }
00170 }
00171 cx_free(ptrb);
00172
00173 return 1;
00174 }
00175
00176
00177 static cpl_matrix *
00178 _giraffe_matrix_inverse(cpl_matrix *aa)
00179 {
00180 cxint test = 1;
00181 cxint aa_ncol = 0;
00182 cxint aa_nrow = 0;
00183
00184 cxdouble *pd_temp = NULL;
00185 cxdouble *pd_bb = NULL;
00186
00187 cpl_matrix *bb = NULL;
00188 cpl_matrix *temp = NULL;
00189
00190 aa_ncol = cpl_matrix_get_ncol(aa);
00191 aa_nrow = cpl_matrix_get_nrow(aa);
00192
00193 if(aa_nrow != aa_ncol) {
00194 return NULL;
00195 }
00196
00197 bb = cpl_matrix_new(aa_nrow, aa_ncol);
00198
00199 temp = cpl_matrix_duplicate(aa);
00200
00201 pd_temp = cpl_matrix_get_data(temp);
00202 pd_bb = cpl_matrix_get_data(bb);
00203
00204 if (_giraffe_matrix_gausspiv(pd_temp, pd_bb, aa_nrow) == 0) {
00205 test = 0;
00206 }
00207
00208 cpl_matrix_delete(temp);
00209
00210 if (test == 0) {
00211 cpl_matrix_delete(bb);
00212 return NULL;
00213 }
00214
00215 return bb;
00216 }
00217
00218
00235 cxdouble
00236 giraffe_matrix_sigma_mean(const cpl_matrix *matrix, cxdouble mean)
00237 {
00238
00239 cxulong size = 0;
00240 cxulong size2 = 0;
00241
00242 const cxdouble *pt = NULL;
00243
00244 cxdouble diff = 0.;
00245 cxdouble sigma = 0.;
00246
00247
00248 cx_assert(matrix != NULL);
00249
00250 size = cpl_matrix_get_ncol(matrix) * cpl_matrix_get_nrow(matrix);
00251 size2 = size - 1;
00252
00253 pt = cpl_matrix_get_data_const(matrix);
00254
00255 while (size--) {
00256 diff = *pt++ - mean;
00257 sigma += diff * diff;
00258 }
00259
00260 return sqrt(sigma / (cxdouble)size2);
00261
00262 }
00263
00264
00281 cxdouble
00282 giraffe_matrix_sigma_fit(const cpl_matrix *matrix,
00283 const cpl_matrix *matrix_fit)
00284 {
00285
00286 cxint ancol;
00287 cxint anrow;
00288 cxint fncol;
00289 cxint fnrow;
00290
00291 cxulong size;
00292 cxulong size2;
00293
00294 const cxdouble *pta = NULL;
00295 const cxdouble *ptf = NULL;
00296
00297 cxdouble diff = 0.;
00298 cxdouble sigma = 0.;
00299
00300
00301 cx_assert(matrix != NULL);
00302 cx_assert(matrix_fit != NULL);
00303
00304 ancol = cpl_matrix_get_ncol(matrix);
00305 anrow = cpl_matrix_get_nrow(matrix);
00306 fncol = cpl_matrix_get_ncol(matrix_fit);
00307 fnrow = cpl_matrix_get_nrow(matrix_fit);
00308
00309 if ((ancol * anrow) != (fncol * fnrow)) {
00310 return 0.0;
00311 }
00312
00313 size = ancol * anrow;
00314 size2 = size - 1;
00315
00316 pta = cpl_matrix_get_data_const(matrix);
00317 ptf = cpl_matrix_get_data_const(matrix_fit);
00318
00319 while (size--) {
00320 diff = *pta++ - *ptf++;
00321 sigma += diff * diff;
00322 }
00323
00324 return sqrt(sigma / (cxdouble) size2);
00325
00326 }
00327
00328
00343 cpl_image *
00344 giraffe_matrix_create_image(const cpl_matrix *matrix)
00345 {
00346
00347 cpl_image *image = NULL;
00348
00349
00350 if (matrix) {
00351 cxint nx = cpl_matrix_get_ncol(matrix);
00352 cxint ny = cpl_matrix_get_nrow(matrix);
00353
00354
00355 image = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);
00356
00357 if (image) {
00358 cxsize sz = nx * ny;
00359 cxdouble *pixels = cpl_image_get_data_double(image);
00360
00361 memcpy(pixels, cpl_matrix_get_data_const(matrix),
00362 sz * sizeof(cxdouble));
00363 }
00364 }
00365
00366 return image;
00367
00368 }
00369
00370 #define PIX_STACK_SIZE 50
00371
00386 cxint
00387 giraffe_matrix_sort(cpl_matrix *mA)
00388 {
00389 register cxint i;
00390 register cxint ir;
00391 register cxint j;
00392 register cxint j_stack;
00393 register cxint k;
00394 register cxint l;
00395
00396 register cxdouble a;
00397 register cxdouble *pix_arr = NULL;
00398
00399 cxint i_stack[PIX_STACK_SIZE] ;
00400
00401
00402 pix_arr = cpl_matrix_get_data(mA);
00403 ir = cpl_matrix_get_nrow(mA) * cpl_matrix_get_ncol(mA);
00404
00405 l = 1 ;
00406 j_stack = 0 ;
00407 for (;;) {
00408 if (ir - l < 7) {
00409 for (j = l + 1 ; j <= ir ; j++) {
00410 a = pix_arr[j - 1];
00411 for (i = j - 1 ; i >= 1 ; i--) {
00412 if (pix_arr[i - 1] <= a) {
00413 break;
00414 }
00415 pix_arr[i] = pix_arr[i - 1];
00416 }
00417 pix_arr[i] = a;
00418 }
00419 if (j_stack == 0) {
00420 break;
00421 }
00422 ir = i_stack[j_stack-- - 1];
00423 l = i_stack[j_stack-- - 1];
00424 }
00425 else {
00426 k = (l + ir) >> 1;
00427 _giraffe_swap(&pix_arr[k - 1], &pix_arr[l]);
00428 if (pix_arr[l] > pix_arr[ir - 1]) {
00429 _giraffe_swap(&pix_arr[l], &pix_arr[ir - 1]);
00430 }
00431 if (pix_arr[l - 1] > pix_arr[ir - 1]) {
00432 _giraffe_swap(&pix_arr[l - 1], &pix_arr[ir - 1]);
00433 }
00434 if (pix_arr[l] > pix_arr[l - 1]) {
00435 _giraffe_swap(&pix_arr[l], &pix_arr[l - 1]);
00436 }
00437 i = l + 1;
00438 j = ir;
00439 a = pix_arr[l - 1];
00440 for (;;) {
00441 do {
00442 i++;
00443 } while (pix_arr[i - 1] < a);
00444
00445 do {
00446 j--;
00447 } while (pix_arr[j - 1] > a);
00448
00449 if (j < i) {
00450 break;
00451 }
00452 _giraffe_swap(&pix_arr[i - 1], &pix_arr[j - 1]);
00453 }
00454 pix_arr[l - 1] = pix_arr[j - 1];
00455 pix_arr[j - 1] = a;
00456 j_stack += 2;
00457 if (j_stack > PIX_STACK_SIZE) {
00458
00459 return -1 ;
00460 }
00461 if (ir - i + 1 >= j - l) {
00462 i_stack[j_stack - 1] = ir;
00463 i_stack[j_stack - 2] = i;
00464 ir = j - 1;
00465 }
00466 else {
00467 i_stack[j_stack - 1] = j - 1;
00468 i_stack[j_stack - 2] = l;
00469 l = i;
00470 }
00471 }
00472 }
00473
00474 return 0;
00475
00476 }
00477
00478 #undef PIX_STACK_SIZE
00479
00480
00509 cpl_matrix *
00510 giraffe_matrix_leastsq(const cpl_matrix* mA, const cpl_matrix* mB)
00511 {
00512
00513 cpl_matrix* m1 = NULL;
00514 cpl_matrix* m2 = NULL;
00515 cpl_matrix* m3 = NULL;
00516 cpl_matrix* mX = NULL;
00517
00518
00519 cx_assert(mA != NULL);
00520 cx_assert(mB != NULL);
00521 cx_assert(cpl_matrix_get_ncol(mA) == cpl_matrix_get_ncol(mB));
00522
00523 m1 = cpl_matrix_transpose_create(mA);
00524 m2 = cpl_matrix_product_create(mA, m1);
00525 m3 = cpl_matrix_invert_create(m2);
00526
00527 if (m3 == NULL) {
00528 cpl_matrix_delete(m2);
00529 m2 = NULL;
00530
00531 cpl_matrix_delete(m1);
00532 m1 = NULL;
00533
00534 return NULL;
00535 }
00536
00537 cpl_matrix_delete(m2);
00538
00539 m2 = cpl_matrix_product_create(mB, m1);
00540
00541 cpl_matrix_delete(m1);
00542 m1 = NULL;
00543
00544 mX = cpl_matrix_product_create(m2, m3);
00545
00546 cpl_matrix_delete(m2);
00547 m2 = NULL;
00548
00549 cpl_matrix_delete(m3);
00550 m3 = NULL;
00551
00552 return mX;
00553
00554 }
00555
00556
00570 cxint
00571 giraffe_matrix_clear(cpl_matrix *matrix)
00572 {
00573 cxint nr_matrix;
00574 cxint nc_matrix;
00575
00576 cxdouble *pd_matrix = NULL;
00577
00578 cx_assert(matrix != NULL);
00579
00580 pd_matrix = cpl_matrix_get_data(matrix);
00581 nc_matrix = cpl_matrix_get_ncol(matrix);
00582 nr_matrix = cpl_matrix_get_nrow(matrix);
00583
00584 memset(pd_matrix, 0, nr_matrix * nc_matrix * sizeof(cxdouble));
00585
00586 return 0;
00587
00588 }
00589
00590
00610 void
00611 giraffe_matrix_dump(const cpl_matrix *matrix, cxint max_rows)
00612 {
00613
00614 cxint i;
00615 cxint j;
00616 cxint k;
00617 cxint nc;
00618 cxint nr;
00619 cxint ncw;
00620
00621 const cxdouble *pd_m = NULL;
00622
00623 cx_string *buffer = NULL;
00624 cx_string *tmp = NULL;
00625
00626 if (matrix == NULL) {
00627 return;
00628 }
00629
00630 pd_m = cpl_matrix_get_data_const(matrix);
00631
00632 nr = cpl_matrix_get_nrow(matrix);
00633 nc = cpl_matrix_get_ncol(matrix);
00634
00635 if (nr > max_rows) {
00636 nr = max_rows;
00637 }
00638
00639 buffer = cx_string_new();
00640 tmp = cx_string_new();
00641
00642
00643 for (i = 0; i < nc; i++) {
00644 ncw = cx_string_sprintf(tmp, " %d", i);
00645 cx_string_append(buffer, cx_string_get(tmp));
00646 }
00647
00648 cpl_msg_debug("", cx_string_get(buffer));
00649
00650
00651 for (k = 0, i = 0; i < nr; i++) {
00652 ncw = cx_string_sprintf(buffer," %d", i);
00653 for (j = 0; j < nc; j++, k++) {
00654 ncw = cx_string_sprintf(tmp, " %+18.12f", pd_m[k]);
00655 cx_string_append(buffer, cx_string_get(tmp));
00656 }
00657
00658 cpl_msg_debug("", cx_string_get(buffer));
00659 }
00660
00661 cx_string_delete(tmp);
00662 cx_string_delete(buffer);
00663
00664 return;
00665
00666 }