GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/ell/genmat.c Lines: 112 189 59.3 %
Date: 2017-05-26 Branches: 77 134 57.5 %

Line Branch Exec Source
1
/*
2
  Teem: Tools to process and visualize scientific data and images             .
3
  Copyright (C) 2013, 2012, 2011, 2010, 2009  University of Chicago
4
  Copyright (C) 2008, 2007, 2006, 2005  Gordon Kindlmann
5
  Copyright (C) 2004, 2003, 2002, 2001, 2000, 1999, 1998  University of Utah
6
7
  This library is free software; you can redistribute it and/or
8
  modify it under the terms of the GNU Lesser General Public License
9
  (LGPL) as published by the Free Software Foundation; either
10
  version 2.1 of the License, or (at your option) any later version.
11
  The terms of redistributing and/or modifying this software also
12
  include exceptions to the LGPL that facilitate static linking.
13
14
  This library is distributed in the hope that it will be useful,
15
  but WITHOUT ANY WARRANTY; without even the implied warranty of
16
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17
  Lesser General Public License for more details.
18
19
  You should have received a copy of the GNU Lesser General Public License
20
  along with this library; if not, write to Free Software Foundation, Inc.,
21
  51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
22
*/
23
24
25
#include "ell.h"
26
27
int
28
ell_Nm_check(Nrrd *mat, int doNrrdCheck) {
29
  static const char me[]="ell_Nm_check";
30
31
448
  if (doNrrdCheck) {
32
    if (nrrdCheck(mat)) {
33
      biffMovef(ELL, NRRD, "%s: basic nrrd validity check failed", me);
34
      return 1;
35
    }
36
  } else {
37
224
    if (!mat) {
38
      biffAddf(ELL, "%s: got NULL pointer", me);
39
      return 1;
40
    }
41
  }
42
224
  if (!( 2 == mat->dim )) {
43
    biffAddf(ELL, "%s: nrrd must be 2-D (not %d-D)", me, mat->dim);
44
    return 1;
45
  }
46
224
  if (!( nrrdTypeDouble == mat->type )) {
47
    biffAddf(ELL, "%s: nrrd must be type %s (not %s)", me,
48
             airEnumStr(nrrdType, nrrdTypeDouble),
49
             airEnumStr(nrrdType, mat->type));
50
    return 1;
51
  }
52
53
224
  return 0;
54
224
}
55
56
/*
57
******** ell_Nm_tran
58
**
59
**     M             N
60
** N [trn]  <--  M [mat]
61
*/
62
int
63
ell_Nm_tran(Nrrd *ntrn, Nrrd *nmat) {
64
  static const char me[]="ell_Nm_tran";
65
  double *mat, *trn;
66
  size_t MM, NN, mm, nn;
67
68

96
  if (!( ntrn && !ell_Nm_check(nmat, AIR_FALSE) )) {
69
    biffAddf(ELL, "%s: NULL or invalid args", me);
70
    return 1;
71
  }
72
32
  if (ntrn == nmat) {
73
    biffAddf(ELL, "%s: sorry, can't work in-place yet", me);
74
    return 1;
75
  }
76
  /*
77
  if (nrrdAxesSwap(ntrn, nmat, 0, 1)) {
78
    biffMovef(ELL, NRRD, "%s: trouble", me);
79
    return 1;
80
    }
81
  */
82
32
  NN = nmat->axis[0].size;
83
32
  MM = nmat->axis[1].size;
84
32
  if (nrrdMaybeAlloc_va(ntrn, nrrdTypeDouble, 2,
85
                        MM, NN)) {
86
    biffMovef(ELL, NRRD, "%s: trouble", me);
87
    return 1;
88
  }
89
32
  mat = AIR_CAST(double *, nmat->data);
90
32
  trn = AIR_CAST(double *, ntrn->data);
91
448
  for (nn=0; nn<NN; nn++) {
92
4224
    for (mm=0; mm<MM; mm++) {
93
1920
      trn[mm + MM*nn] = mat[nn + NN*mm];
94
    }
95
  }
96
97
32
  return 0;
98
32
}
99
100
/*
101
******** ell_Nm_mul
102
**
103
** Currently, only useful for matrix-matrix multiplication
104
**
105
** matrix-matrix:      M       N
106
**                  L [A] . M [B]
107
*/
108
int
109
ell_Nm_mul(Nrrd *nAB, Nrrd *nA, Nrrd *nB) {
110
  static const char me[]="ell_Nm_mul";
111
  double *A, *B, *AB, tmp;
112
  size_t LL, MM, NN, ll, mm, nn;
113
128
  char stmp[4][AIR_STRLEN_SMALL];
114
115

192
  if (!( nAB && !ell_Nm_check(nA, AIR_FALSE)
116
128
         && !ell_Nm_check(nB, AIR_FALSE) )) {
117
    biffAddf(ELL, "%s: NULL or invalid args", me);
118
    return 1;
119
  }
120

128
  if (nAB == nA || nAB == nB) {
121
    biffAddf(ELL, "%s: can't do in-place multiplication", me);
122
    return 1;
123
  }
124
64
  LL = nA->axis[1].size;
125
64
  MM = nA->axis[0].size;
126
64
  NN = nB->axis[0].size;
127
64
  if (MM != nB->axis[1].size) {
128
    biffAddf(ELL, "%s: size mismatch: %s-by-%s times %s-by-%s", me,
129
             airSprintSize_t(stmp[0], LL),
130
             airSprintSize_t(stmp[1], MM),
131
             airSprintSize_t(stmp[2], nB->axis[1].size),
132
             airSprintSize_t(stmp[3], NN));
133
    return 1;
134
  }
135
64
  if (nrrdMaybeAlloc_va(nAB, nrrdTypeDouble, 2,
136
                        NN, LL)) {
137
    biffMovef(ELL, NRRD, "%s: trouble", me);
138
    return 1;
139
  }
140
64
  A = (double*)(nA->data);
141
64
  B = (double*)(nB->data);
142
64
  AB = (double*)(nAB->data);
143
896
  for (ll=0; ll<LL; ll++) {
144
6912
    for (nn=0; nn<NN; nn++) {
145
      tmp = 0;
146
52224
      for (mm=0; mm<MM; mm++) {
147
23040
        tmp += A[mm + MM*ll]*B[nn + NN*mm];
148
      }
149
3072
      AB[ll*NN + nn] = tmp;
150
    }
151
  }
152
153
64
  return 0;
154
64
}
155
156
/*
157
** _ell_LU_decomp()
158
**
159
** in-place LU decomposition
160
*/
161
int
162
_ell_LU_decomp(double *aa, size_t *indx, size_t NN)  {
163
  static const char me[]="_ell_LU_decomp";
164
  int ret=0;
165
  size_t ii, imax=0, jj, kk;
166
  double big, sum, tmp;
167
  double *vv;
168
169
64
  if (!( vv = (double*)calloc(NN, sizeof(double)) )) {
170
    biffAddf(ELL, "%s: couldn't allocate vv[]!", me);
171
    ret = 1; goto seeya;
172
  }
173
174
  /* find vv[i]: max of abs of everything in column i */
175
448
  for (ii=0; ii<NN; ii++) {
176
    big = 0.0;
177
2688
    for (jj=0; jj<NN; jj++) {
178
1152
      if ((tmp=AIR_ABS(aa[ii*NN + jj])) > big) {
179
        big = tmp;
180
448
      }
181
    }
182
192
    if (!big) {
183
      char stmp[AIR_STRLEN_SMALL];
184
      biffAddf(ELL, "%s: singular matrix since column %s all zero", me,
185
               airSprintSize_t(stmp, ii));
186
      ret = 1; goto seeya;
187
    }
188
192
    vv[ii] = big;
189
  }
190
191
448
  for (jj=0; jj<NN; jj++) {
192
    /* for aa[ii][jj] in lower triangle (below diagonal), subtract from
193
       aa[ii][jj] the dot product of all elements to its left with elements
194
       above it (starting at the top) */
195
1344
    for (ii=0; ii<jj; ii++) {
196
480
      sum = aa[ii*NN + jj];
197
2240
      for (kk=0; kk<ii; kk++) {
198
640
        sum -= aa[ii*NN + kk]*aa[kk*NN + jj];
199
      }
200
480
      aa[ii*NN + jj] = sum;
201
    }
202
203
    /* for aa[ii][jj] in upper triangle (including diagonal), subtract from
204
       aa[ii][jj] the dot product of all elements above it with elements to
205
       its left (starting from the left) */
206
    big = 0.0;
207
1728
    for (ii=jj; ii<NN; ii++) {
208
672
      sum = aa[ii*NN + jj];
209
3584
      for (kk=0; kk<jj; kk++) {
210
1120
        sum -= aa[ii*NN + kk]*aa[kk*NN + jj];
211
      }
212
672
      aa[ii*NN + jj] = sum;
213
      /* imax column is one in which abs(aa[i][j])/vv[i] */
214
672
      if ((tmp = AIR_ABS(sum)/vv[ii]) >= big) {
215
        big = tmp;
216
        imax = ii;
217
192
      }
218
    }
219
220
    /* unless we're on the imax column, swap this column the with imax column,
221
       and permute vv[] accordingly */
222
192
    if (jj != imax) {
223
      /* could record parity # of permutes here */
224
      for (kk=0; kk<NN; kk++) {
225
        tmp = aa[imax*NN + kk];
226
        aa[imax*NN + kk] = aa[jj*NN + kk];
227
        aa[jj*NN + kk] = tmp;
228
      }
229
      tmp = vv[imax];
230
      vv[imax] = vv[jj];
231
      vv[jj] = tmp;
232
    }
233
234
192
    indx[jj] = imax;
235
236
192
    if (aa[jj*NN + jj] == 0.0) {
237
      aa[jj*NN + jj] = ELL_EPS;
238
    }
239
240
    /* divide everything right of a[jj][jj] by a[jj][jj] */
241
192
    if (jj != NN) {
242
192
      tmp = 1.0/aa[jj*NN + jj];
243
1344
      for (ii=jj+1; ii<NN; ii++) {
244
480
        aa[ii*NN + jj] *= tmp;
245
      }
246
    }
247
  }
248
 seeya:
249
32
  airFree(vv);
250
32
  return ret;
251
32
}
252
253
/*
254
** _ell_LU_back_sub
255
**
256
** given the matrix and index array from _ellLUDecomp generated from
257
** some matrix M, solves for x in the linear equation Mx = b, and
258
** puts the result back into b
259
*/
260
void
261
_ell_LU_back_sub(double *aa, size_t *indx, double *bb, size_t NN) {
262
  size_t ii, jj;
263
  double sum;
264
265
  /* Forward substitution, with lower triangular matrix */
266
2880
  for (ii=0; ii<NN; ii++) {
267
1152
    sum = bb[indx[ii]];
268
1152
    bb[indx[ii]] = bb[ii];
269
8064
    for (jj=0; jj<ii; jj++) {
270
2880
      sum -= aa[ii*NN + jj]*bb[jj];
271
    }
272
1152
    bb[ii] = sum;
273
  }
274
275
  /* Backward substitution, with upper triangular matrix */
276
2688
  for (ii=NN; ii>0; ii--) {
277
1152
    sum = bb[ii-1];
278
8064
    for (jj=ii; jj<NN; jj++) {
279
2880
      sum -= aa[(ii-1)*NN + jj]*bb[jj];
280
    }
281
1152
    bb[ii-1] = sum / aa[(ii-1)*NN + (ii-1)];
282
  }
283
  return;
284
192
}
285
286
/*
287
** _ell_inv
288
**
289
** Invert NNxNN matrix based on LU-decomposition
290
**
291
** The given matrix is copied, turned into its LU-decomposition, and
292
** then repeated backsubstitution is used to get successive columns of
293
** the inverse.
294
*/
295
int
296
_ell_inv(double *inv, double *_mat, size_t NN) {
297
  static const char me[]="_ell_inv";
298
  size_t ii, jj, *indx=NULL;
299
  double *col=NULL, *mat=NULL;
300
  int ret=0;
301
302

96
  if (!( (col = (double*)calloc(NN, sizeof(double))) &&
303
32
         (mat = (double*)calloc(NN*NN, sizeof(double))) &&
304
32
         (indx = (size_t*)calloc(NN, sizeof(size_t))) )) {
305
    biffAddf(ELL, "%s: couldn't allocate all buffers", me);
306
    ret = 1; goto seeya;
307
  }
308
309
32
  memcpy(mat, _mat, NN*NN*sizeof(double));
310
311
32
  if (_ell_LU_decomp(mat, indx, NN)) {
312
    biffAddf(ELL, "%s: trouble", me);
313
    ret = 1; goto seeya;
314
  }
315
316
448
  for (jj=0; jj<NN; jj++) {
317
192
    memset(col, 0, NN*sizeof(double));
318
192
    col[jj] = 1.0;
319
192
    _ell_LU_back_sub(mat, indx, col, NN);
320
    /* set column jj of inv to result of backsub */
321
2688
    for (ii=0; ii<NN; ii++) {
322
1152
      inv[ii*NN + jj] = col[ii];
323
    }
324
  }
325
 seeya:
326
32
  airFree(col); airFree(mat); airFree(indx);
327
32
  return ret;
328
}
329
330
/*
331
******** ell_Nm_inv
332
**
333
** computes the inverse of given matrix in nmat, and puts the
334
** inverse in the (maybe allocated) ninv.  Does not touch the
335
** values in nmat.
336
*/
337
int
338
ell_Nm_inv(Nrrd *ninv, Nrrd *nmat) {
339
  static const char me[]="ell_Nm_inv";
340
  double *mat, *inv;
341
  size_t NN;
342
343

96
  if (!( ninv && !ell_Nm_check(nmat, AIR_FALSE) )) {
344
    biffAddf(ELL, "%s: NULL or invalid args", me);
345
    return 1;
346
  }
347
348
32
  NN = nmat->axis[0].size;
349
32
  if (!( NN == nmat->axis[1].size )) {
350
    char stmp[2][AIR_STRLEN_SMALL];
351
    biffAddf(ELL, "%s: need a square matrix, not %s-by-%s", me,
352
             airSprintSize_t(stmp[0], nmat->axis[1].size),
353
             airSprintSize_t(stmp[1], NN));
354
    return 1;
355
  }
356
32
  if (nrrdMaybeAlloc_va(ninv, nrrdTypeDouble, 2,
357
                        NN, NN)) {
358
    biffMovef(ELL, NRRD, "%s: trouble", me);
359
    return 1;
360
  }
361
32
  inv = (double*)(ninv->data);
362
32
  mat = (double*)(nmat->data);
363
32
  if (_ell_inv(inv, mat, NN)) {
364
    biffAddf(ELL, "%s: trouble", me);
365
    return 1;
366
  }
367
368
32
  return 0;
369
32
}
370
371
/*
372
******** ell_Nm_pseudo_inv()
373
**
374
** determines the pseudoinverse of the given matrix M by using the formula
375
** P = (M^T * M)^(-1) * M^T
376
**
377
** I'll get an SVD-based solution working later, since that gives a more
378
** general solution
379
*/
380
int
381
ell_Nm_pseudo_inv(Nrrd *ninv, Nrrd *nA) {
382
  static const char me[]="ell_Nm_pseudo_inv";
383
  Nrrd *nAt, *nAtA, *nAtAi;
384
  int ret=0;
385
386

96
  if (!( ninv && !ell_Nm_check(nA, AIR_FALSE) )) {
387
    biffAddf(ELL, "%s: NULL or invalid args", me);
388
    return 1;
389
  }
390
32
  nAt = nrrdNew();
391
32
  nAtA = nrrdNew();
392
32
  nAtAi = nrrdNew();
393
64
  if (ell_Nm_tran(nAt, nA)
394
64
      || ell_Nm_mul(nAtA, nAt, nA)
395
64
      || ell_Nm_inv(nAtAi, nAtA)
396
64
      || ell_Nm_mul(ninv, nAtAi, nAt)) {
397
    biffAddf(ELL, "%s: trouble", me);
398
    ret = 1; goto seeya;
399
  }
400
401
 seeya:
402
32
  nrrdNuke(nAt); nrrdNuke(nAtA); nrrdNuke(nAtAi);
403
32
  return ret;
404
32
}
405
406
/*
407
******** ell_Nm_wght_pseudo_inv()
408
**
409
** determines a weighted least squares solution via
410
** P = (A^T * W * A)^(-1) * A^T * W
411
*/
412
int
413
ell_Nm_wght_pseudo_inv(Nrrd *ninv, Nrrd *nA, Nrrd *nW) {
414
  static const char me[]="ell_Nm_wght_pseudo_inv";
415
  Nrrd *nAt, *nAtW, *nAtWA, *nAtWAi;
416
  int ret=0;
417
418
  if (!( ninv && !ell_Nm_check(nA, AIR_FALSE)
419
         && !ell_Nm_check(nW, AIR_FALSE) )) {
420
    biffAddf(ELL, "%s: NULL or invalid args", me);
421
    return 1;
422
  }
423
  nAt = nrrdNew();
424
  nAtW = nrrdNew();
425
  nAtWA = nrrdNew();
426
  nAtWAi = nrrdNew();
427
  if (ell_Nm_tran(nAt, nA)
428
      || ell_Nm_mul(nAtW, nAt, nW)
429
      || ell_Nm_mul(nAtWA, nAtW, nA)
430
      || ell_Nm_inv(nAtWAi, nAtWA)
431
      || ell_Nm_mul(ninv, nAtWAi, nAtW)) {
432
    biffAddf(ELL, "%s: trouble", me);
433
    ret = 1; goto seeya;
434
  }
435
436
 seeya:
437
  nrrdNuke(nAt); nrrdNuke(nAtW); nrrdNuke(nAtWA); nrrdNuke(nAtWAi);
438
  return ret;
439
}
440