linalg.cc 12.6 KB
Newer Older
1
#include "radixglls/linalg.hh"
2
3
4
5
6
#include <algorithm>  //std::min/max
#include <cmath>      // std::sqrt
#include <limits>     // std::numeric_limits
#include <numeric>    // std::inner_product
#include "radixbug/bug.hh"
7
#include "radixglls/fmangle.hh"
8

9
namespace radix {
10

11
GllsMatrix::GllsMatrix(size_t nrows, size_t ncols, double val)
12
13
14
    : m_data(nrows, VectorDouble(ncols, val))
    , m_nrows(nrows)
    , m_ncols(ncols) {}
Aaron Bevill's avatar
Aaron Bevill committed
15

16
GllsMatrix::GllsMatrix(const VectorVectorDouble &data, size_t ncols)
17
18
    : m_data(data.begin(), data.end())
    , m_nrows(data.size())
19
20
21
22
    , m_ncols(data.size() > 0 ? data[0].size() : ncols) {
  for (size_t i = 0; i < m_nrows; ++i) {
    radix_check(m_data[i].size() == m_ncols);
  }
Aaron Bevill's avatar
Aaron Bevill committed
23
24
}

25
26
GllsMatrix::GllsMatrix(
    std::initializer_list<std::initializer_list<double>> data)
27
28
    : m_data(data.begin(), data.end())
    , m_nrows(m_data.size())
29
30
31
32
    , m_ncols(m_data.size() > 0 ? m_data[0].size() : 0) {
  for (size_t i = 0; i < m_nrows; ++i) {
    radix_check(m_data[i].size() == m_ncols);
  }
Aaron Bevill's avatar
Aaron Bevill committed
33
34
}

35
36
37
38
39
40
GllsMatrix GllsMatrix::from_colmajor(const double data[], size_t nrows,
                                     size_t ncols) {
  GllsMatrix matrix(nrows, ncols);
  for (size_t i = 0; i < nrows; ++i) {
    for (size_t j = 0; j < ncols; ++j) {
      matrix[i][j] = data[i + nrows * j];
Aaron Bevill's avatar
Aaron Bevill committed
41
    }
42
43
  }
  return matrix;
Aaron Bevill's avatar
Aaron Bevill committed
44
45
}

46
47
48
49
50
51
GllsMatrix GllsMatrix::from_colmajor(const VectorDouble &data, size_t nrows,
                                     size_t ncols) {
  GllsMatrix matrix(nrows, ncols);
  for (size_t i = 0; i < nrows; ++i) {
    for (size_t j = 0; j < ncols; ++j) {
      matrix[i][j] = data[i + nrows * j];
Aaron Bevill's avatar
Aaron Bevill committed
52
    }
53
54
  }
  return matrix;
Aaron Bevill's avatar
Aaron Bevill committed
55
56
}

57
58
59
GllsMatrix GllsMatrix::from_identity(size_t n) {
  VectorDouble diag(n, 1.);
  return GllsMatrix::from_diag(diag);
Aaron Bevill's avatar
Aaron Bevill committed
60
61
}

62
63
64
65
66
67
GllsMatrix GllsMatrix::from_diag(const VectorDouble &vec) {
  GllsMatrix matrix(vec.size(), vec.size(), 0.);
  for (size_t i = 0; i < matrix.nrows(); ++i) {
    matrix[i][i] = vec[i];
  }
  return matrix;
68
69
}

70
71
72
73
74
75
GllsMatrix GllsMatrix::from_diag_sq(const VectorDouble &vec) {
  GllsMatrix matrix(vec.size(), vec.size(), 0.);
  for (size_t i = 0; i < matrix.nrows(); ++i) {
    matrix[i][i] = vec[i] * vec[i];
  }
  return matrix;
76
77
}

78
79
80
81
82
83
GllsMatrix GllsMatrix::from_diag_inv(const VectorDouble &vec) {
  GllsMatrix matrix(vec.size(), vec.size(), 0.);
  for (size_t i = 0; i < matrix.nrows(); ++i) {
    matrix[i][i] = 1. / vec[i];
  }
  return matrix;
84
85
}

86
87
88
89
void GllsMatrix::to_colmajor(double data[]) const {
  for (size_t j = 0; j < m_ncols; ++j) {
    for (size_t i = 0; i < m_nrows; ++i) {
      data[i + m_nrows * j] = m_data[i][j];
Aaron Bevill's avatar
Aaron Bevill committed
90
    }
91
  }
Aaron Bevill's avatar
Aaron Bevill committed
92
93
}

94
95
96
97
98
void GllsMatrix::to_colmajor(VectorDouble &data) const {
  data.resize(m_nrows * m_ncols);
  for (size_t j = 0; j < m_ncols; ++j) {
    for (size_t i = 0; i < m_nrows; ++i) {
      data[i + m_nrows * j] = m_data[i][j];
99
    }
100
  }
101
102
}

103
size_t GllsMatrix::nrows() const { return m_nrows; }
Aaron Bevill's avatar
Aaron Bevill committed
104

105
size_t GllsMatrix::ncols() const { return m_ncols; }
Aaron Bevill's avatar
Aaron Bevill committed
106

107
const VectorDouble &GllsMatrix::operator[](size_t i) const { return m_data[i]; }
Aaron Bevill's avatar
Aaron Bevill committed
108

109
VectorDouble &GllsMatrix::operator[](size_t i) { return m_data[i]; }
Aaron Bevill's avatar
Aaron Bevill committed
110

111
112
113
114
115
GllsMatrix GllsMatrix::transpose() const {
  GllsMatrix matrix(m_ncols, m_nrows);
  for (size_t j = 0; j < m_ncols; ++j) {
    for (size_t i = 0; i < m_nrows; ++i) {
      matrix[j][i] = m_data[i][j];
Aaron Bevill's avatar
Aaron Bevill committed
116
    }
117
118
  }
  return matrix;
Aaron Bevill's avatar
Aaron Bevill committed
119
120
}

121
122
123
GllsMatrix GllsMatrix::row_slice(size_t ibegin, size_t iend) const {
  VectorVectorDouble data(m_data.begin() + ibegin, m_data.begin() + iend);
  return GllsMatrix(data, m_ncols);
Aaron Bevill's avatar
Aaron Bevill committed
124
125
}

126
127
128
129
130
131
VectorDouble &GllsMatrix::append_row(const VectorDouble &row) {
  radix_check(row.size() == m_ncols);
  m_data.push_back(row);
  ++m_nrows;
  radix_ensure(m_data.size() == m_nrows);
  return m_data[m_nrows - 1];
Aaron Bevill's avatar
Aaron Bevill committed
132
133
}

134
135
136
137
138
139
140
141
VectorDouble GllsMatrix::operator*(const VectorDouble &b) const {
  radix_check(m_ncols == b.size());  // matrix multiply shape mismatch
  VectorDouble prod(m_nrows);
  for (size_t i = 0; i < m_nrows; ++i) {
    prod[i] =
        std::inner_product(m_data[i].begin(), m_data[i].end(), b.begin(), 0.);
  }
  return prod;
142
143
}

144
145
146
147
148
149
150
151
GllsMatrix GllsMatrix::operator*(const GllsMatrix &B) const {
  radix_check(m_ncols == B.nrows());
  GllsMatrix Btrans = B.transpose();
  GllsMatrix prod(m_nrows, Btrans.nrows());
  for (size_t i = 0; i < m_nrows; ++i) {
    for (size_t k = 0; k < Btrans.nrows(); ++k) {
      prod[i][k] = std::inner_product(m_data[i].begin(), m_data[i].end(),
                                      Btrans[k].begin(), 0.);
152
    }
153
154
  }
  return prod;
155
156
}

157
158
159
160
161
162
bool GllsMatrix::operator==(const GllsMatrix &B) const {
  if (ncols() != B.ncols()) return false;
  if (nrows() != B.nrows()) return false;
  for (size_t r = 0; r < nrows(); ++r) {
    for (size_t c = 0; c < ncols(); ++c) {
      if (m_data[r][c] != B.m_data[r][c]) return false;
163
    }
164
165
  }
  return true;
166
167
}

168
169
170
171
172
173
bool GllsMatrix::is_diagonal() const {
  for (size_t i = 0; i < m_nrows; ++i) {
    for (size_t j = 0; j < m_ncols; ++j) {
      if (m_data[i][j] != 0. && i != j) {
        return false;
      }
174
    }
175
176
  }
  return true;
177
}
178
// translate linux __func__ macro to windows __FUNCTION__ macro
179
180
#ifdef _MSC_VER  // Visual Studio
#define __func__ __FUNCTION__
181
#endif
182
/// Require that Lapack parameter "info" is returned zero
183
184
185
186
187
188
189
190
191
#define lapack_require(info, lapack_fname)                            \
  if (info != 0) {                                                    \
    std::ostringstream stream;                                        \
    stream << "Function " << __func__ << " called Lapack function "   \
           << lapack_fname << ", "                                    \
           << "which returned code " << info << "."                   \
           << "This may mean the problem is ill-posed." << std::endl; \
    throw LapackError(stream.str(), info);                            \
  }
192

193
// LAPACK dgesvd computes the singular value decomposition of a matrix
194
#define ML_DGESVD LAPACK_FUNC(dgesvd, DGESVD)
195
extern "C" void ML_DGESVD(
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
    char *jobu,    // which columns of u are computed ('A' = all columns)
    char *jobvt,   // which rows of vt are computed ('A' = all rows)
    int *m,        // number of rows in the input matrix A
    int *n,        // number of cols "
    double *a,     // the m by n matrix to decompose (colmajor inout)
    int *lda,      // leading dimension of A
    double *s,     // singular values of A, sorted descending (out)
    double *u,     // left singular vectors (colmajor out) not used if jobu=='N'
    int *ldu,      // leading dimension of u
    double *vt,    // right singular vectors (colmajor out)
    int *ldvt,     // leading dimension of vt
    double *work,  // workspace and error data (array inout)
    int *lwork,    // dimension of lwork
    int *info);    // 0 indicates successful exit (out)

void GllsMatrix::svd(GllsMatrix &left_svectors, VectorDouble &svalues,
                     GllsMatrix &right_svectors) const {
  if (m_nrows == 0 || m_ncols == 0) {
    left_svectors = GllsMatrix::from_identity(m_nrows);
    svalues.resize(0);
    right_svectors = GllsMatrix::from_identity(m_ncols);
    return;
  }
  //
  char jobu  = 'A';
  char jobvt = 'A';
  int m      = m_nrows;
  int n      = m_ncols;
  VectorDouble matrix_colmajor(m * n);
  this->to_colmajor(matrix_colmajor);
  int lda = m;
  svalues.resize(std::min(m, n));
  VectorDouble u(m * m);
  int ldu = m;
  VectorDouble vt(n * n);
  int ldvt  = n;
  int lwork = std::min(m * n * (int)sizeof(VectorDouble::value_type),
                       std::numeric_limits<int>::max());
  VectorDouble work(lwork);
  int info;
  //
  ML_DGESVD(&jobu, &jobvt, &m, &n, &(matrix_colmajor[0]), &lda, &(svalues[0]),
            &(u[0]), &ldu, &(vt[0]), &ldvt, &(work[0]), &lwork, &info);
  //
  left_svectors  = GllsMatrix::from_colmajor(u, m, m);
  right_svectors = GllsMatrix::from_colmajor(vt, n, n);
  lapack_require(info, "dgesvd");
243
244
}

245
// LAPACK dsytrf factorizes a double-precision symmetric matrix
246
#define ML_DSYTRF LAPACK_FUNC(dsytrf, DSYTRF)
247
extern "C" void ML_DSYTRF(
248
249
250
251
252
253
254
255
256
257
    char *uplo,    // 'U' or 'L' for shape of triangular decomposition
    int *n,        // a is n by n
    double *a,     // lhs matrix, serialized column-major (array inout)
    int *lda,      // leading dimension of A (for slicing into A)
    int *ipiv,     // pivot array (array out)
    double *work,  // work(1) is the optimal lwork (array out)
    int *lwork,    // size of the work array, suggest >= n
    int *info);    // 0 indicates successful exit (out)

FactorizedMatrix::FactorizedMatrix(const GllsMatrix &matrix)
258
259
260
    : GllsMatrix(matrix)
    , m_factorized(m_nrows * m_nrows)
    , m_ipiv(m_nrows) {
261
262
263
264
265
266
  // check and copy the matrix
  radix_check(m_nrows > 0);
  radix_check(m_nrows == m_ncols);
  for (size_t i = 0; i < m_nrows; ++i) {
    for (size_t j = 0; j < m_nrows; ++j) {
      m_factorized[i + m_nrows * j] = m_data[i][j];
267
    }
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
  }
  // factorize the matrix
  char uplo    = 'U';
  int n        = int(m_nrows);
  int lda      = m_nrows;
  int lwork    = m_nrows * 64;
  double *work = new double[lwork];
  int info;
  //
  ML_DSYTRF(&uplo, &n, &(m_factorized[0]), &lda, &(m_ipiv[0]), work, &lwork,
            &info);
  //
  radix_line("Optimal lwork(dsytrf) is " << work[0] << " with n=" << n);
  delete[] work;
  lapack_require(info, "dsytrf");
283
284
}

285
// LAPACK dsytri inverts a factorized matrix
286
#define ML_DSYTRI LAPACK_FUNC(dsytri, DSYTRI)
287
extern "C" void ML_DSYTRI(
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
    char *uplo,    // 'U' or 'L' for shape of triangular decomposition
    int *n,        // a is n by n
    double *a,     // lhs matrix, serialized column-major (array inout)
    int *lda,      // leading dimension of A (for slicing into A)
    int *ipiv,     // pivot array (array out)
    double *work,  // work(1) is the optimal lwork (array out)
    int *info);    // 0 indicates successful exit (out)

GllsMatrix FactorizedMatrix::inverse() const {
  VectorDouble inverse_colmajor = m_factorized;
  char uplo                     = 'U';
  int n                         = int(m_nrows);
  int lda                       = m_nrows;
  VectorInt ipiv(m_ipiv.begin(), m_ipiv.end());
  double *work = new double[m_nrows];
  // initialize work area to zero
  radix_block(for (size_t i = 0; i < m_nrows; ++i) work[i] = 0.0);
  int info;
  //
  ML_DSYTRI(&uplo, &n, &(inverse_colmajor[0]), &lda, &(ipiv[0]), work, &info);
  //
  radix_line("Optimal lwork(dsytri) is " << work[0] << " with n=" << n);
  delete[] work;
  lapack_require(info, "dsytri");
  // de-serialize the result
  GllsMatrix inverse =
      GllsMatrix::from_colmajor(inverse_colmajor, m_nrows, m_nrows);
  // unfold the result
  for (size_t i = 0; i < m_nrows; ++i) {
    for (size_t j = i; j < m_nrows; ++j) {
      inverse[j][i] = inverse[i][j];
319
    }
320
321
322
  }
  //
  return inverse;
323
324
}

325
// LAPACK dsytrs solves a factorized matrix
326
#define ML_DSYTRS LAPACK_FUNC(dsytrs, DSYTRS)
327
extern "C" void ML_DSYTRS(
328
329
330
331
332
333
334
    char *uplo,  // 'U' or 'L' for shape of triangular decomposition
    int *n,      // a is n by n
    int *nrhs,   // number of right sides (can solve for multiple values of b)
    double *a,   // lhs matrix, serialized column-major (array inout)
    int *lda,    // leading dimension of A (for slicing into A)
    int *ipiv,   // pivot array (array out)
    double *b,   // rhs vector(s), serialized with each rhs vector contiguous
335
    // (array inout)
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
    int *ldb,    // leading dimension of b (for slicing into b)
    int *info);  // 0 indicates successful exit (out)

void FactorizedMatrix::solve(VectorDouble &xb) const {
  char uplo               = 'U';
  int n                   = int(m_nrows);
  int nrhs                = 1;
  VectorDouble factorized = m_factorized;
  int lda                 = m_nrows;
  VectorInt ipiv          = m_ipiv;
  int ldb                 = m_nrows;
  int info;
  //
  radix_check(xb.size() == size_t(m_nrows));
  //
  ML_DSYTRS(&uplo, &n, &nrhs, &(factorized[0]), &lda, &(ipiv[0]), &(xb[0]),
            &ldb, &info);
  //
  lapack_require(info, "dsytrs");
355
356
}

357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
void FactorizedMatrix::solve(GllsMatrix &xb) const {
  radix_check(xb.nrows() == m_nrows);
  char uplo               = 'U';
  int n                   = int(m_nrows);
  int nrhs                = int(xb.ncols());
  VectorDouble factorized = m_factorized;
  int lda                 = m_nrows;
  VectorInt ipiv          = m_ipiv;
  int ldb                 = m_nrows;
  int info;
  //
  // serialize xb col-major
  VectorDouble xb_colmajor(n * nrhs);
  xb.to_colmajor(xb_colmajor);
  //
  ML_DSYTRS(&uplo, &n, &nrhs, &(factorized[0]), &lda, &(ipiv[0]),
            &(xb_colmajor[0]), &ldb, &info);
  //
  // de-serialize xb (row-major)
  xb = GllsMatrix::from_colmajor(xb_colmajor, xb.nrows(), xb.ncols());
  lapack_require(info, "dsytrs");
378
379
}

380
381
382
VectorDouble f2vd(double arr[], int size) {
  VectorDouble vd(arr, arr + size_t(size));
  return vd;
383
384
}

385
386
387
388
void vd2f(const VectorDouble &vd, double arr[]) {
  for (size_t i = 0; i < vd.size(); ++i) {
    arr[i] = vd[i];
  }
389
390
}

391
}  // namespace radix