bidiagonal_system_solver.h

Go to the documentation of this file.

#ifndef TATOOINE_BIDIAGONAL_SYSTEM_QR_SOLVER_H
#define TATOOINE_BIDIAGONAL_SYSTEM_QR_SOLVER_H
//==============================================================================
#include <cassert>
#include <cmath>
#include <cstdlib>
//==============================================================================
namespace tatooine {
//==============================================================================
// General solvers
//-------------------------------------------------------------------------------
template <typename T>
bool bdsvu(int _n, int _nrhs, const T* _d, const T* _du, T* _b, int _ldb) {
  // assert(_n > 1);
  assert(_nrhs > 0);
  assert(_ldb >= _n || _nrhs == 1);
 
  if (_d[_n - 1] == T(0)) return false;  // shall we handle inf?
 
  for (int j = 0; j < _nrhs; ++j)
    _b[(_n - 1) + j * _ldb] /= _d[_n - 1];
 
  for (int i = _n - 2; i >= 0; --i) {
    if (_d[i] == T(0)) return false;
    for (int j = 0; j < _nrhs; ++j)
      _b[i + j * _ldb] =
          (_b[i + j * _ldb] - _du[i] * _b[(i + 1) + j * _ldb]) / _d[i];
  }
  return true;
}
//------------------------------------------------------------------------------
template <typename T>
bool bdsvl(int _n, int _nrhs, const T* _dl, const T* _d, T* _b, int _ldb) {
  // assert(_n > 1);
  assert(_nrhs > 0);
  assert(_ldb >= _n || _nrhs == 1);
 
  if (_d[0] == T(0)) return false;  // shall we handle inf?
 
  for (int j = 0; j < _nrhs; ++j)
    _b[0 + j * _ldb] /= _d[0];
 
  for (int i = 1; i < _n; ++i) {
    if (_d[i] == T(0)) return false;
    for (int j = 0; j < _nrhs; ++j)
      _b[i + j * _ldb] =
          (_b[i + j * _ldb] - _dl[i - 1] * _b[(i - 1) + j * _ldb]) / _d[i];
  }
  return true;
}
//-------------------------------------------------------------------------------
// Special solvers tailored to problem
//-------------------------------------------------------------------------------
template <typename T>
bool solve_blockwise(int _n, const T* _d, const T* _du, T* _b) {
  // assert(_n > 1);
 
  int m = _n - 1;
 
  // get s
  T s(1);
  {
    if (_d[m - 1] == T(0)) return false;
 
    T x = _du[m - 1] / _d[m - 1];
    s -= x;
    for (int i = m - 2; i >= 0; --i) {
      x *= -_du[i] / _d[i];
      s -= x;
    }
  }
 
  // get y
  assert(_n < 2048 && "limit  temporary buffer size using alloca");
  T* b = (T*)alloca(_n * sizeof(T));
  memcpy(b, _b, _n * sizeof(T));
 
  bool regular = bdsvl(m, 1, _d, _du, b, 1);
  assert(regular);
 
  T y(b[m]);
  for (int i = 0; i < m; ++i)
    y -= b[i];
  y /= s;
 
  _b[m - 1] -= _du[m - 1] * y;
 
  regular = bdsvu(m, 1, _d, _du, _b, 1);
  assert(regular);
 
  _b[m] = y;
 
  return true;
}
//-------------------------------------------------------------------------------
template <typename T>
void _planerot(T _c, T _s, T* _x) {
  T x   = _x[0];
  T y   = _x[1];
  _x[0] = _c * x - _s * y;
  _x[1] = _s * x + _c * y;
}
//------------------------------------------------------------------------------
 
#ifdef DOXYGEN_SKIP
#ifndef _Q_IN_NULL
#define _Q_IN_NULL
#endif
#endif
template <typename T>
bool solve_qr(int _n, T* _d, T* _du, T* _b, T* _null) {
  assert(_n < 2048 && "limit  temporary buffer size using alloca");
#ifndef _Q_IN_NULL
  T* q = (T*)alloca(2 * _n * sizeof(T));  // store givens rotations
#else
  T* qc = (T*)alloca(_n * sizeof(T));  // store cosines of givens rotations
#endif
 
  //
  // 1. Compute givens rotations and construct R.
  //
 
  for (int j = 0; j < _n; ++j) {
    T c, s;
 
    if (_du[j] == T(0)) {
      c = 1;
      s = 0;
    } else {
      if (fabs(_du[j]) > fabs(_d[j])) {  // Do it the super-stable way!
        T tau = _d[j] / _du[j];          // (avoid overflows)
        s     = 1 / std::sqrt(T(1) + tau * tau);
        c     = s * tau;
      } else {
        T tau = _du[j] / _d[j];
        c     = 1 / std::sqrt(T(1) + tau * tau);
        s     = c * tau;
      }
    }
 
    _d[j] = c * _d[j] + s * _du[j];
 
    if (j + 1 < _n) {
      T a       = _d[j + 1];
      _du[j]    = s * a;  // first component...
      _d[j + 1] = c * a;  // ... is zero
    }
#ifndef _Q_IN_NULL
    q[2 * j]     = c;
    q[2 * j + 1] = s;
#else
    qc[j]    = c;
    _null[j] = s;  // store sine in _null
#endif
  }
  _du[_n - 1] = T(0);
 
  // Now we have Q' as givens rotations in q and
  //  R in _d (diagonal), _du (superdiagonal).
 
  //
  // 2. Solve y=R'\_b
  //
 
  if (!bdsvl(_n, 1, _du, _d, _b, 1)) return false;
  _b[_n] = T(0);
 
  //
  // 3. Compute xln=Q*y and _null=Q(:,end).
  //
 
  _null[_n] = T(1);
 
  for (int j = _n - 1; j >= 0; --j) {
#ifndef _Q_IN_NULL
    T c = q[2 * j];
    T s = q[2 * j + 1];
#else
    T c      = qc[j];
    T s      = _null[j];
#endif
 
    _null[j] = T(0);
    _planerot(c, s, _null + j);
    _planerot(c, s, _b + j);
  }
 
  return true;
}
//==============================================================================
}  // namespace tatooine
//==============================================================================
#endif