SINFONI Pipeline Reference Manual  2.5.2
sinfo_matrix.c
1 /*
2  * This file is part of the ESO SINFONI Pipeline
3  * Copyright (C) 2004,2005 European Southern Observatory
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, 51 Franklin St, Fifth Floor, Boston, MA 02111-1307 USA
18  */
19 /*----------------------------------------------------------------------------
20 
21  File name : sinfo_matrix.c
22  Author : Nicolas Devillard
23  Created on : 1994
24  Description : basic 2d sinfo_eclipse_matrix handling routines
25 
26  ---------------------------------------------------------------------------*/
27 /*
28  $Id: sinfo_matrix.c,v 1.4 2012-03-02 08:42:20 amodigli Exp $
29  $Author: amodigli $
30  $Date: 2012-03-02 08:42:20 $
31  $Revision: 1.4 $
32  */
33 #ifdef HAVE_CONFIG_H
34 # include <config.h>
35 #endif
36 /*----------------------------------------------------------------------------
37  Includes
38  ---------------------------------------------------------------------------*/
39 #include "sinfo_matrix.h"
47 /*----------------------------------------------------------------------------
48  Macros
49  ---------------------------------------------------------------------------*/
50 #define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)
51 /*----------------------------------------------------------------------------
52  Private function prototypes
53  ---------------------------------------------------------------------------*/
54 static int
55 gauss_pivot(double *ptra, double *ptrc, int n);
56 /*----------------------------------------------------------------------------
57  Function codes
58  ---------------------------------------------------------------------------*/
59 
71 Matrix
72 sinfo_create_mx(int nr, int nc)
73 {
74  Matrix b;
75  b = (Matrix) cpl_calloc(1, sizeof(sinfo_eclipse_matrix));
76  b->m = (double*) cpl_calloc(nr * nc, sizeof(double));
77  b->nr = nr;
78  b->nc = nc;
79  return b;
80 }
81 
91 Matrix
92 sinfo_copy_mx(Matrix a)
93 {
94  Matrix b = sinfo_create_mx(a->nr, a->nc);
95  if (b != NULL ) {
96  register int s = a->nr * a->nc;
97  register double *mm = b->m + s;
98  register double *am = a->m + s;
99  while (s--)
100  *--mm = *--am;
101  }
102  return b;
103 }
104 
114 void
115 sinfo_close_mx(Matrix a)
116 {
117  if (a == NULL )
118  return;
119  if (a->m != NULL )
120  cpl_free(a->m);
121  cpl_free(a);
122  return;
123 }
124 
135 Matrix
136 sinfo_mul_mx(Matrix a, Matrix b)
137 {
138  Matrix c, d;
139  int n1 = a->nr, n2 = a->nc, n3 = b->nc;
140  register double *a0;
141  register double *c0;
142  register double *d0;
143  register int i, j, k;
144 
145  if (n2 != b->nr)
146  return NULL ;
147  c = sinfo_create_mx(n1, n3);
148  d = sinfo_transp_mx(b);
149 
150  for (i = 0, c0 = c->m; i < n1; i++)
151  for (j = 0, d0 = d->m; j < n3; j++, c0++)
152  for (k = 0, *c0 = 0, a0 = a->m + i * n2; k < n2; k++)
153  *c0 += *a0++ * *d0++;
154  sinfo_close_mx(d);
155  return c;
156 }
157 
169 Matrix
170 sinfo_invert_mx(Matrix aa)
171 {
172  Matrix bb;
173  int test = 1;
174 
175  if (aa->nr != aa->nc)
176  return NULL ;
177  bb = sinfo_create_mx(aa->nr, aa->nc);
178 
179  if (aa->nr == 1) {
180  double det;
181  register double ted;
182  det = *(aa->m);
183  if (dtiny(det))
184  test = 0;
185  ted = 1. / det;
186  *(bb->m) = ted;
187  }
188  else if (aa->nr == 2) {
189  double det;
190  register double ted;
191  register double *mm = aa->m;
192  double a = *(mm++), b = *(mm++);
193  double c = *(mm++), d = *(mm);
194  det = a * d - b * c;
195  if (dtiny(det))
196  test = 0;
197  ted = 1. / det;
198  mm = bb->m;
199  *(mm++) = d * ted, *(mm++) = -b * ted;
200  *(mm++) = -c * ted, *(mm) = a * ted;
201  }
202  else if (aa->nr == 3) {
203  double det;
204  register double ted;
205  register double *mm = aa->m;
206  double a = *(mm++), b = *(mm++), c = *(mm++);
207  double d = *(mm++), e = *(mm++), f = *(mm++);
208  double g = *(mm++), h = *(mm++), i = *(mm);
209  det = a * e * i - a * h * f - b * d * i + b * g * f + c * d * h
210  - c * g * e;
211  if (dtiny(det))
212  test = 0;
213  ted = 1. / det;
214  mm = bb->m;
215  *(mm++) = (e * i - f * h) * ted, *(mm++) = (c * h - b * i) * ted, *(mm++) =
216  (b * f - e * c) * ted;
217 
218  *(mm++) = (f * g - d * i) * ted, *(mm++) = (a * i - g * c) * ted, *(mm++) =
219  (d * c - a * f) * ted;
220 
221  *(mm++) = (d * h - g * e) * ted, *(mm++) = (g * b - a * h) * ted, *(mm) =
222  (a * e - d * b) * ted;
223  }
224  else {
225  Matrix temp = sinfo_copy_mx(aa);
226  if (gauss_pivot(temp->m, bb->m, aa->nr) == 0)
227  test = 0;
228  sinfo_close_mx(temp);
229  }
230  if (test == 0) {
231  sinfo_msg_error("not invertible, aborting inversion");
232  return NULL ;
233  }
234  return bb;
235 }
236 
246 Matrix
247 sinfo_transp_mx(Matrix a)
248 {
249  register int nc = a->nc, nr = a->nr;
250  register double *a0;
251  register double *b0;
252  register int i, j;
253  Matrix b = sinfo_create_mx(nc, nr);
254 
255  if (b == (Matrix) NULL )
256  return b;
257  for (i = 0, b0 = b->m; i < nc; i++)
258  for (j = 0, a0 = a->m + i; j < nr; j++, a0 += nc, b0++)
259  *b0 = *a0;
260  return b;
261 }
262 
275 static int
276 gauss_pivot(double *ptra, double *ptrc, int n)
277 /* c(n,n) = a(n,n)^-1 */
278 {
279 #define SINFO_ABS(a) (((a) > 0) ? (a) : -(a))
280 
281  register int i, j, k, l;
282  int maj;
283  double max, r, t;
284  double *ptrb;
285 
286  ptrb = (double *) cpl_calloc(n * n, sizeof(double));
287  for (i = 0; i < n; i++)
288  ptrb[i * n + i] = 1.0;
289 
290  for (i = 1; i <= n; i++) {
291  /* Search max in current column */
292  max = SINFO_ABS(*(ptra + n*i-n));
293  maj = i;
294  for (j = i; j <= n; j++)
295  if (SINFO_ABS(*(ptra+n*j+i-n-1)) > max) {
296  maj = j;
297  max = SINFO_ABS(*(ptra+n*j+i-n-1));
298  }
299 
300  /* swap lines i and maj */
301  if (maj != i) {
302  for (j = i; j <= n; j++) {
303  r = *(ptra + n * maj + j - n - 1);
304  *(ptra + n * maj + j - n - 1) = *(ptra + n * i + j - n - 1);
305  *(ptra + n * i + j - n - 1) = r;
306  }
307  for (l = 0; l < n; l++) {
308  r = *(ptrb + l * n + maj - 1);
309  *(ptrb + l * n + maj - 1) = *(ptrb + l * n + i - 1);
310  *(ptrb + l * n + i - 1) = r;
311  }
312  }
313 
314  /* Subtract line by line */
315  for (j = i + 1; j <= n; j++) {
316  t = (*(ptra + (n + 1) * i - n - 1));
317  if (dtiny(t))
318  return (0);
319  r = (*(ptra + n * j + i - n - 1)) / t;
320  for (l = 0; l < n; l++)
321  *(ptrb + l * n + j - 1) -= r * (*(ptrb + l * n + i - 1));
322  for (k = i; k <= n; k++)
323  *(ptra + n * j + k - n - 1) -= r
324  * (*(ptra + n * i + k - n - 1));
325  }
326  }
327 
328  /* Triangular system resolution */
329  for (l = 0; l < n; l++)
330  for (i = n; i >= 1; i--) {
331  t = (*(ptra + (n + 1) * i - n - 1));
332  if (dtiny(t))
333  return (0);
334  *(ptrc + l + (i - 1) * n) = (*(ptrb + l * n + i - 1)) / t;
335  if (i > 1)
336  for (j = i - 1; j > 0; j--)
337  *(ptrb + l * n + j - 1) -= (*(ptra + n * j + i - n - 1))
338  * (*(ptrc + l + (i - 1) * n));
339  }
340  cpl_free(ptrb);
341  return (1);
342 }
343 
363 Matrix
364 sinfo_least_sq_mx(Matrix A, Matrix B)
365 {
366  Matrix m1, m2, m3, m4, m5;
367 
368  m1 = sinfo_transp_mx(A);
369  m2 = sinfo_mul_mx(A, m1);
370  m3 = sinfo_invert_mx(m2);
371  m4 = sinfo_mul_mx(B, m1);
372  m5 = sinfo_mul_mx(m4, m3);
373 
374  sinfo_close_mx(m1);
375  sinfo_close_mx(m2);
376  sinfo_close_mx(m3);
377  sinfo_close_mx(m4);
378 
379  return m5;
380 }
381 
394 void
395 sinfo_print_mx(Matrix M, const char * name)
396 {
397  int i, j;
398 
399  fprintf(stdout, "# sinfo_eclipse_matrix %s is [%d x %d]\n", name, M->nr,
400  M->nc);
401  for (j = 0; j < M->nr; j++) {
402  for (i = 0; i < M->nc; i++) {
403  fprintf(stdout, "%g\t", M->m[i + j * M->nc]);
404  }
405  fprintf(stdout, "\n");
406  }
407  fprintf(stdout, "\n");
408 }
409