linear_algebra.cc

#include <iostream>

#include <vtestbed/config/real_type.hh>
#include <vtestbed/linalg/linear_algebra.hh>

//#define VTB_DEBUG_LINEAR_ALGEBRA
//#define VTB_DEBUG_EIGEN_VALUES

template <class T>
bool
vtb::linalg::Matrix<T>::is_symmetric() const {
    using blitz::all;
    return all(*this == this->transpose());
}

template <class T>
bool
vtb::linalg::Matrix<T>::is_symmetric(T eps) const {
    using blitz::all;
    using blitz::abs;
    return all(abs(*this - this->transpose()) < eps);
}

template <class T>
bool
vtb::linalg::Matrix<T>::is_positive_definite() const {
    using blitz::Range;
    using blitz::sum;
    using blitz::pow2;
    using std::sqrt;
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    const auto& rhs = *this;
    Matrix<T> L(rhs.shape());
    L = 0;
    const int n = rhs.rows();
    for (int i=0; i<n; ++i) {
        for (int j=0; j<i; ++j) {
            Range r{0,j};
            L(i,j) = (rhs(i,j) - sum(L(i,r)*L(j,r))) / L(j,j);
        }
        T s = sum(pow2(L(i, Range(0,i))));
        if (rhs(i,i) < s) { return false; }
        L(i,i) = sqrt(rhs(i,i) - s);
    }
    return true;
}

template <class T>
bool
vtb::linalg::Matrix<T>::is_toeplitz() const {
    const auto& rhs = *this;
    const int nrows = rhs.rows();
    const int ncols = rhs.cols();
    for (int i=1; i<nrows; ++i) {
        for (int j=1; j<ncols; ++j) {
            if (rhs(i,j) != rhs(i-1, j-1)) { return false; }
        }
    }
    return true;
}

template <class T>
bool
vtb::linalg::Matrix<T>::is_toeplitz(T eps) const {
    using std::abs;
    const auto& rhs = *this;
    const int nrows = rhs.rows();
    const int ncols = rhs.cols();
    for (int i=1; i<nrows; ++i) {
        for (int j=1; j<ncols; ++j) {
            if (abs(rhs(i,j) - rhs(i-1,j-1)) < eps) {
                return false;
            }
        }
    }
    return true;
}

template <class T>
auto
vtb::linalg::Lower_triangular_matrix<T>::forward_substitution(const Vector<T>& b) const -> Vector<T> {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    using blitz::sum;
    auto& L = *this;
    const int n = L.rows();
    Vector<T> y(b.shape());
    y(0) = b(0) / L(0,0);
    for (int i=1; i<n; ++i) {
        blitz::Range r{0,i-1};
        y(i) = (b(i) - sum(L(i,r)*y(r))) / L(i,i);
    }
    return y;
}

template <class T>
auto
vtb::linalg::Lower_triangular_matrix<T>::backward_substitution(const Vector<T>& y) const -> Vector<T> {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    auto& L = *this;
    const int n = L.rows();
    Vector<T> x(y.shape());
    x(n-1) = y(n-1) / L(n-1,n-1);
    for (int i=n-2; i>=0; --i) {
        blitz::Range r{i+1,n-1};
        T sum{0};
        for (int j=i+1; j<n; ++j) {
            sum += conj(L(j,i)) * x(j);
        }
        x(i) = (y(i) - sum) / L(i,i);
    }
    return x;
}

template <class T>
auto
vtb::linalg::Symmetric_matrix<T>::cholesky_indefinite() const -> Lower_triangular_matrix_ldlt<T> {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square() ||
        !this->is_symmetric()) {
        throw std::runtime_error{"bad matrix"};
    }
    #endif
    using namespace blitz;
    Lower_triangular_matrix_ldlt<T> L(this->shape());
    L = 0;
    auto& A = *this;
    const int n = A.rows();
    Vector<T> d(n);
    int j = 0;
    d(j) = A(j,j);
    for (int i=0; i<n; ++i) {
        L(i,j) = A(i,j) / d(j);
    }
    for (j=1; j<n; ++j) {
        Range r{0,j-1};
        d(j) = A(j,j) - sum(pow2(L(j,r))*d(r));
        for (int i=0; i<n; ++i) {
            L(i,j) = (A(i,j) - sum(L(i,r)*d(r)*L(j,r))) / d(j);
        }
    }
    L._diagonal.reference(d);
    return L;
}

template <class T>
auto
vtb::linalg::Symmetric_matrix<T>::eigen_values(T eps, int nsweeps) const -> Vector<T> {
    using std::abs;
    using std::sqrt;
    Symmetric_matrix<T> a(this->copy());
    const int n = a.rows();
    Vector<T> result(n);
    result = 0;
    // The number of elements under the main diagonal.
    int nelements = n*(n - 1) / 2;
    int niterations = nelements*nsweeps;
    for (int it=0; it<niterations; ++it) {
        int pivot_i = 0;
        int pivot_j = 1;
        T pivot = abs(a(0,1));
        for (int i=0; i<n; ++i) {
            for (int j=0; j<n; ++j) {
                if (i == j) {
                    continue;
                }
                T new_pivot = abs(a(i,j));
                if (new_pivot > pivot) {
                    pivot = new_pivot;
                    pivot_i = i;
                    pivot_j = j;
                }
            }
        }
        if (abs(pivot) < eps) {
            break;
        }
        int i = pivot_i;
        int j = pivot_j;
        T w = (a(j,j) - a(i,i)) / (2*a(i,j));
        T sqrt_w = sqrt(1 + w*w);
        T t1 = -w + sqrt_w, t2 = -w - sqrt_w;
        T t = t1;
        if (t1 == 0 || (t2 != 0 && abs(t2) < abs(t1))) {
            t = t2;
        }
        T r = sqrt(1 + t*t);
        T cos = 1 / r, sin = t / r;
        T tau = sin / (1 + cos);
        T a_ij = a(i,j);
        a(i,j) = (cos*cos - sin*sin)*a(i,j) + cos*sin*(a(i,i) - a(j,j));
        a(j,i) = a(i,j);
        a(i,i) -= t*a_ij;
        a(j,j) += t*a_ij;
        for (int k=0; k<n; ++k) {
            if (k == i || k == j) {
                continue;
            }
            T sa_ik = sin*a(i,k);
            T sa_jk = sin*a(j,k);
            a(i,k) -= sa_jk + tau*sa_ik;
            a(j,k) += sa_ik - tau*sa_jk;
            a(k,i) = a(i,k);
            a(k,j) = a(j,k);
        }
        #if defined(VTB_DEBUG_EIGEN_VALUES)
        std::clog
            << std::setw(20) << __func__
            << std::setw(20) << pivot
            << std::setw(20) << w
            << std::setw(20) << t
            << std::setw(20) << cos
            << std::setw(20) << sin
            << std::endl;
        #endif
    }
    for (int i=0; i<n; ++i) {
        result(i) = a(i,i);
    }
    return result;
}

template <class T>
auto
vtb::linalg::Positive_definite_matrix<T>::cholesky() const -> Lower_triangular_matrix<T> {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square() || !this->is_symmetric()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    using blitz::Range;
    using blitz::sum;
    using blitz::pow2;
    using std::sqrt;
    Lower_triangular_matrix<T> L(this->shape());
    L = 0;
    auto& rhs = *this;
    const int n = rhs.rows();
    for (int i=0; i<n; ++i) {
        for (int j=0; j<i; ++j) {
            Range r{0,j};
            L(i,j) = (rhs(i,j) - sum(L(i,r)*L(j,r))) / L(j,j);
        }
        T s = rhs(i,i) - sum(pow2(L(i, Range(0,i))));
        if (s <= 0) {
            std::clog << "i=" << i << std::endl;
            std::clog << "s=" << s << std::endl;
            throw Invalid_matrix("not positive definite");
        }
        L(i,i) = sqrt(s);
    }
    return L;
}

template <class T>
auto
vtb::linalg::Lower_upper_triangular_matrix<T>::inverse() const -> Square_matrix<T> {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    Square_matrix<T> result(this->shape());
    auto& rhs = *this;
    const int n = rhs.rows();
    for (int j=0; j<n; ++j) {
        for (int i=0; i<n; ++i) {
            result(i,j) = (_permutations(i) == j) ? 1 : 0;
            for (int k=0; k<i; ++k) {
                result(i,j) -= rhs(i,k) * result(k,j);
            }
        }
        for (int i=n-1; i>=0; --i) {
            for (int k=i+1; k<n; ++k) {
                result(i,j) -= rhs(i,k) * result(k,j);
            }
            result(i,j) /= rhs(i,i);
            if (!std::isfinite(result(i,j))) {
                throw std::invalid_argument{"inverse: bad matrix"};
            }
        }
    }
    return result;
}

template <class T>
auto
vtb::linalg::Lower_upper_triangular_matrix<T>::solve(const Vector<T>& b) const -> Vector<T> {
    const auto& A = *this;
    const auto& p = this->_permutations;
    const int n = this->rows();
    Vector<T> x(b.shape());
    for (int i=0; i<n; ++i) {
        x(i) = b(p(i));
    }
    for (int i=0; i<n; ++i) {
        for (int k=0; k<i; ++k) {
            x(i) -= A(i,k)*x(k);
        }
    }
    for (int i=n-1; i>=0; --i) {
        for (int k=i+1; k<n; ++k) {
            x(i) -= A(i,k)*x(k);
        }
        x(i) /= A(i,i);
        if (!std::isfinite(x(i))) {
            throw std::invalid_argument{"solve: bad matrix"};
        }
    }
    return x;
}

template <class T>
auto
vtb::linalg::Lower_upper_triangular_matrix<T>::lower_upper_triangular_self() -> Lower_upper_triangular_matrix<T>& {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    using std::abs;
    using std::isfinite;
    using std::swap;
    using blitz::abs;
    using blitz::max;
    using blitz::Range;
    const auto& _ = Range::all();
    auto& rhs = *this;
    const int n = rhs.rows();
    auto& p = this->_permutations;
    p.resize(n);
    for (int i=0; i<n; ++i) {
        p(i) = i;
    }
    int npermutations = 0;
    Vector<T> tmp(n);
    for (int i=0; i<n; ++i) {
        int pivot = 0;
        T elem{0};
        // find max element
        for (int j=i; j<n; ++j) {
            const T new_elem = abs(rhs(j,i));
            if (elem < new_elem) {
                elem = new_elem;
                pivot = j;
            }
        }
        // swap rows
        if (pivot != i) {
            swap(p(i), p(pivot));
            tmp = rhs(pivot,_);
            rhs(pivot,_) = rhs(i,_);
            rhs(i,_) = tmp;
            ++npermutations;
        }
        for (int j=i+1; j<n; ++j) {
            const T x = rhs(i,i);
            rhs(j,i) /= x;
            if (!isfinite(rhs(j,i))) {
                throw std::invalid_argument{
                    "lower_upper_triangular_self: bad matrix"
                };
            }
            for (int k=i+1; k<n; ++k) {
                rhs(j,k) -= rhs(j,i)*rhs(i,k);
            }
        }
    }
    this->_npermutations = npermutations;
    return *this;
}

template <class T>
T
vtb::linalg::Lower_upper_triangular_matrix<T>::determinant() const {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (!this->is_square()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    using namespace blitz;
    firstIndex i;
    auto& rhs = *this;
    T result = product(rhs(i,i));
    if (this->_npermutations%2 == 1) {
        result = -result;
    }
    return result;
}

template <class T>
auto
vtb::linalg::product(const Matrix<T>& lhs, const Vector<T>& rhs) -> Vector<T>
 {
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (lhs.extent(1) != rhs.extent(0)) {
        std::cerr << lhs.shape() << std::endl;
        std::cerr << rhs.shape() << std::endl;
        throw std::runtime_error{"product: bad shape"};
    }
    #endif
    const blitz::Range& _ = blitz::Range::all();
    const int nrows = lhs.rows();
    Vector<T> result(nrows);
    for (int i=0; i<nrows; ++i) {
        result(i) = blitz::dot(lhs(i,_), rhs);
    }
    return result;
}

template <class T>
auto
vtb::linalg::product(const Matrix<T>& lhs, const Matrix<T>& rhs) -> Matrix<T>
 {
    const int nrows = lhs.rows();
    const int ncols = rhs.cols();
    #if defined(VTB_DEBUG_LINEAR_ALGEBRA)
    if (lhs.cols() != rhs.rows()) {
        throw std::runtime_error{"bad shape"};
    }
    #endif
    const blitz::Range& _ = blitz::Range::all();
    Matrix<T> result(nrows, ncols);
    for (int i=0; i<nrows; ++i) {
        for (int j=0; j<ncols; ++j) {
            result(i,j) = blitz::dot(lhs(i,_), rhs(_,j));
        }
    }
    return result;
}

template class vtb::linalg::Matrix<VTB_REAL_TYPE>;
template class vtb::linalg::Lower_triangular_matrix<VTB_REAL_TYPE>;
template class vtb::linalg::Symmetric_matrix<VTB_REAL_TYPE>;
template class vtb::linalg::Lower_upper_triangular_matrix<VTB_REAL_TYPE>;
template class vtb::linalg::Positive_definite_matrix<VTB_REAL_TYPE>;

template auto
vtb::linalg::product(
    const Matrix<VTB_REAL_TYPE>& lhs,
    const Matrix<VTB_REAL_TYPE>& rhs
) -> Matrix<VTB_REAL_TYPE>;

template auto
vtb::linalg::product(
    const Matrix<VTB_REAL_TYPE>& lhs,
    const Vector<VTB_REAL_TYPE>& rhs
) -> Vector<VTB_REAL_TYPE>;

#if defined(VTB_REAL_TYPE_FLOAT)
template class vtb::linalg::Lower_triangular_matrix<double>;
template class vtb::linalg::Positive_definite_matrix<double>;
template class vtb::linalg::Lower_upper_triangular_matrix<double>;
template class vtb::linalg::Symmetric_matrix<double>;
template auto
vtb::linalg::product(
    const Matrix<double>& lhs,
    const Matrix<double>& rhs
) -> Matrix<double>;
template auto
vtb::linalg::product(
    const Matrix<double>& lhs,
    const Vector<double>& rhs
) -> Vector<double>;
#endif