gauss-jordan消元

線形代数を復習して見たが、夜は退屈でコードに書いただけだ.
matrixはboostのmatrixを使った
正しいかどうか分かりませんが、簡単なテストしかしていません.
 

#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
using namespace boost::numeric::ublas;

template<class T>
void swap_elem(T& a , T& b)
{
    T tmp;
    tmp = a;a=b;b=tmp;
}
void swap_row(matrix<double> &mat ,int t ,int  k)
{
    double tmp;
    int n = mat.size2();
    for(int i = 0 ; i < n ; i++){
    swap_elem(mat(t,i) , mat(k,i));
    }
    
}

void gauss_jordan(matrix<double> &mat)
{
    int m = mat.size1();
    int n = mat.size2();
    for(int i = 0 , j = 0 ; i < m && j < n ; )
    {
    int k = i;
    for(; k < m ; k ++)
    {
        if( mat(k , j) !=0 ) break;
    }
    if (k == m){
        j ++;
    }else{
        if(k != i) swap_row(mat , k , i);
        double tmp = 0;

        tmp = 1.0/mat(i,j);
        
        for(k = j ; k < n ; k++)
        mat(i,k) = mat(i,k) * tmp;

        for(k = 0 ; k < m ; k++)
        {
        if(k == i) continue;
        if(mat(k , j) == 0) continue;
        
        tmp = -mat(k , j);
        for(int t = j ; t < n ; t++)
            mat(k,t) = mat(k,t) + mat(i,t) * tmp;
        }
        i++ ; j++;
    }
    }
}
int main()
{
/*    matrix<double> mat(3,3);
    mat(0,0)=1;mat(0,1)=1;mat(0,2)=0;
    mat(1,0)=1;mat(1,1)=-1;mat(1,2)=1;
    mat(2,0)=4;mat(2,1)=2;mat(2,2)=1;
*/
    matrix<double> mat(3,4);
    mat(0,0)=2;mat(0,1)=2;mat(0,2)=-2;mat(0,3)=5;
    mat(1,0)=7;mat(1,1)=7;mat(1,2)=1;mat(1,3)=10;
    mat(2,0)=5;mat(2,1)=5;mat(2,2)=-1;mat(2,3)=5;
//    mat(0,1) = 1;mat(0,0)=1;
//   mat(1,1) = 2;mat(1,0)=2;
//    swap_row(mat , 0 , 1);
    gauss_jordan(mat);
    cout<< mat <<endl;
    return 0;
}