qr.h

#pragma once
 
#include "mat.h"
#include "det.h"
#include "functions.h"
#include "up_tri_mat.h"
 
#include <limits>
#include <algorithm>
 
namespace cgv {
namespace math {
 
//qr decomposition returns false if matrix is singular
template<typename T>
bool qr(const mat<T> &a, mat<T> &q, up_tri_mat<T> &r) 
{
    mat<T> rr = a;
    
    bool sing =false;
    unsigned n = a.nrows();
    unsigned m = a.ncols();
    assert(n == m);
    q.resize(n,n);
    r.resize(n);
    unsigned i,j,k;
    vec<T> c(n), d(n);
    T scale,sigma,sum,tau;
    for (k=0;k<n-1;k++) 
    {
        scale=0.0;
        for (i=k;i<n;i++) 
            scale=std::max((T)scale,(T)std::abs(rr(i,k)));
        if (scale == 0.0) 
        {
            sing=true;
            c[k]=d[k]=0.0;
        } else {
            for (i=k;i<n;i++) rr(i,k) /= scale;
            for (sum=0.0,i=k;i<n;i++) sum += rr(i,k)*rr(i,k);
            sigma=sign((T)sqrt(sum),(T)rr(k,k));
            rr(k,k) += sigma;
            c[k]=sigma*rr(k,k);
            d[k] = -scale*sigma;
            for (j=k+1;j<n;j++) {
                for (sum=0.0,i=k;i<n;i++) 
                    sum += rr(i,k)*rr(i,j);
                tau=sum/c[k];
                for (i=k;i<n;i++) 
                    rr(i,j) -= tau*rr(i,k);
            }
        }
    }
    d[n-1]=rr(n-1,n-1);
    if (d[n-1] == 0.0) 
        sing=true;
    for (i=0;i<n;i++) {
        for (j=0;j<n;j++) q(i,j)=0.0;
        q(i,i)=1.0;
    }
    for (k=0;k<n-1;k++) 
    {
        if (c[k] != 0.0) 
        {
            for (j=0;j<n;j++) 
            {
                sum=0.0;
                for (i=k;i<n;i++)
                    sum += rr(i,k)*q(j,i);
                sum /= c[k];
                for (i=k;i<n;i++)
                    q(j,i) -= sum*rr(i,k);
            }
        }
    }
    for (i=0;i<n;i++) 
    {
        rr(i,i)=d[i];
        for (j=0;j<i;j++) rr(i,j)=0.0;
    }
    for(unsigned i = 0; i < n;i++)
        for(unsigned j = i; j < n; j++)
            r(i,j)=rr(i,j);
    return !sing;
}
 
 
//qr decomposition by modified gram schmidt 
template<typename T>
bool qr_mgs(const mat<T> &a, mat<T> &q, up_tri_mat<T> &r) 
{
    unsigned n = a.nrows();
    unsigned m = a.ncols();
    assert(n == m);
    q.resize(n,n);
    r.resize(n);
 
    for(unsigned j = 0; j < m; j++)
    {   
        vec<T> v = a.col(j);
        for(unsigned i = 0; i < j; i++)
        {
            r(i,j) = dot(q.col(i),v);
            v= v- r(i,j)*q.col(i);
        }
        r(j,j) = length(v);
        if(std::abs(r(j,j)) == 0)
            return false;
        q.set_col(j, v/r(j,j));
        
    }
    return true;
}
 
 
 
 
 
 
 
// rq decomposition using tricky column and row permutations
template <typename T>
bool rq(const mat<T> &a, up_tri_mat<T> &r, mat<T> &q)
{
    unsigned N = a.nrows();
    r.resize(N);
    q.resize(N,N);
    mat<T> p=transpose(a);
    p.fliplr();
    p.flipud();
    if(!qr(p,q,r)) return false;
    mat<T> rr =r;
    rr.transpose();
    rr.fliplr();
    rr.flipud();
    
    q.transpose();
    q.fliplr();
    q.flipud();
    
    if (det(q)<0)
    {
 
        rr.set_col(0,-rr.col(0));
        q.set_row(0,-q.row(0));
        
    }
    
    for(unsigned i = 0; i< r.nrows();i++)
        for(unsigned j = i; j< r.ncols();j++)
            r(i,j)=rr(i,j);
 
    return true;
}
 
 
 
 
 
 
 
}
 
}