LU.h

// -*- c++ -*-

// Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk),
// Ed Rosten (er258@cam.ac.uk)

//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:
//1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//2. Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND OTHER CONTRIBUTORS ``AS IS''
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR OTHER CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

#ifndef TOON_INCLUDE_LU_H
#define TOON_INCLUDE_LU_H

#include <iostream>

#include <TooN/TooN.h>
#include <TooN/lapack.h>


namespace TooN {
template <int Size=-1, class Precision=double>
class LU {
    public:

    template<int S1, int S2, class Base>
    LU(const Matrix<S1,S2,Precision, Base>& m)
    :my_lu(m.num_rows(),m.num_cols()),my_IPIV(m.num_rows()){
        compute(m);
    }
    
    template<int S1, int S2, class Base>
    void compute(const Matrix<S1,S2,Precision,Base>& m){
        //check for consistency with Size
        SizeMismatch<Size, S1>::test(my_lu.num_rows(),m.num_rows());
        SizeMismatch<Size, S2>::test(my_lu.num_rows(),m.num_cols());
    
        //Make a local copy. This is guaranteed contiguous
        my_lu=m;
        FortranInteger lda = m.num_rows();
        FortranInteger M = m.num_rows();
        FortranInteger N = m.num_rows();

        getrf_(&M,&N,&my_lu[0][0],&lda,&my_IPIV[0],&my_info);

        if(my_info < 0){
            std::cerr << "error in LU, INFO was " << my_info << std::endl;
        }
    }

    template <int Rows, int NRHS, class Base>
    Matrix<Size,NRHS,Precision> backsub(const Matrix<Rows,NRHS,Precision,Base>& rhs){
        //Check the number of rows is OK.
        SizeMismatch<Size, Rows>::test(my_lu.num_rows(), rhs.num_rows());
    
        Matrix<Size, NRHS, Precision> result(rhs);

        FortranInteger M=rhs.num_cols();
        FortranInteger N=my_lu.num_rows();
        double alpha=1;
        FortranInteger lda=my_lu.num_rows();
        FortranInteger ldb=rhs.num_cols();
        trsm_("R","U","N","N",&M,&N,&alpha,&my_lu[0][0],&lda,&result[0][0],&ldb);
        trsm_("R","L","N","U",&M,&N,&alpha,&my_lu[0][0],&lda,&result[0][0],&ldb);

        // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
        for(int i=N-1; i>=0; i--){
            const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
            for(int j=0; j<NRHS; j++){
                Precision temp = result[i][j];
                result[i][j] = result[swaprow][j];
                result[swaprow][j] = temp;
            }
        }
        return result;
    }

    template <int Rows, class Base>
    Vector<Size,Precision> backsub(const Vector<Rows,Precision,Base>& rhs){
        //Check the number of rows is OK.
        SizeMismatch<Size, Rows>::test(my_lu.num_rows(), rhs.size());
    
        Vector<Size, Precision> result(rhs);

        FortranInteger M=1;
        FortranInteger N=my_lu.num_rows();
        double alpha=1;
        FortranInteger lda=my_lu.num_rows();
        FortranInteger ldb=1;
        trsm_("R","U","N","N",&M,&N,&alpha,&my_lu[0][0],&lda,&result[0],&ldb);
        trsm_("R","L","N","U",&M,&N,&alpha,&my_lu[0][0],&lda,&result[0],&ldb);

        // now do the row swapping (lapack dlaswp.f only shuffles fortran rows = Rowmajor cols)
        for(int i=N-1; i>=0; i--){
            const int swaprow = my_IPIV[i]-1; // fortran arrays start at 1
            Precision temp = result[i];
            result[i] = result[swaprow];
            result[swaprow] = temp;
        }
        return result;
    }

    Matrix<Size,Size,Precision> get_inverse(){
        Matrix<Size,Size,Precision> Inverse(my_lu);
        FortranInteger N = my_lu.num_rows();
        FortranInteger lda=my_lu.num_rows();
        FortranInteger lwork=-1;
        Precision size;
        getri_(&N, &Inverse[0][0], &lda, &my_IPIV[0], &size, &lwork, &my_info);
        lwork=FortranInteger(size);
        Precision* WORK = new Precision[lwork];
        getri_(&N, &Inverse[0][0], &lda, &my_IPIV[0], WORK, &lwork, &my_info);
        delete [] WORK;
        return Inverse;
    }

    const Matrix<Size,Size,Precision>& get_lu()const {return my_lu;}
    
    private:
    inline int get_sign() const {
        int result=1;
        for(int i=0; i<my_lu.num_rows()-1; i++){
            if(my_IPIV[i] > i+1){
                result=-result;
            }
        }
        return result;
    }
    public:

    inline Precision determinant() const {
        Precision result = get_sign();
        for (int i=0; i<my_lu.num_rows(); i++){
            result*=my_lu(i,i);
        }
        return result;
    }
    
    int get_info() const { return my_info; }

 private:

    Matrix<Size,Size,Precision> my_lu;
    FortranInteger my_info;
    Vector<Size, FortranInteger> my_IPIV;   //Convenient static-or-dynamic array of ints :-)

};
}
    

#endif