/*
 * MatrixOperations.h
 *
 *  Created on: Aug 16, 2010
 *      Author: Dula Nad
 *
 *   This file contains some matrix functions
 */

#ifndef MATRIXOPERATIONS_H_
#define MATRIXOPERATIONS_H_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

using boost::numeric::ublas::c_matrix;
using boost::numeric::ublas::c_vector;
using boost::numeric::ublas::zero_matrix;
using boost::numeric::ublas::matrix;
using boost::numeric::ublas::vector;

namespace LABUST
{
  namespace SIMULATION
  {
    /**
     * This function computes the skew-symmetric matrix of 3rd order vector.
     * It works with lower order vectors. Higher order vectors are reduced to 3rd order.
     * We use <typename IN> in order to allow for template expressions.
     * uBlas uses template expressions primarily to mess with my head.
     *
     * \param a Vector of size 3
     * \return 3x3 skew symmetric matrix from vector "a"
     */
    template<typename IN>
    c_matrix<double,3,3> skewSymm3(const IN& a)
    {
      c_matrix<double,3,3> retVal = zero_matrix<double>(3,3);

      if (a.size()>=3)
      {
        retVal(0,0) = retVal(1,1) = retVal(2,2) = 0;
        retVal(0,1) -= retVal(1,0) = a(2);
        retVal(2,0) -= retVal(0,2) = a(1);
        retVal(1,2) -= retVal(2,1) = a(0);
      };

      return retVal;
    };

    /** Matrix inversion routine.
     * Uses lu_factorize and lu_substitute in uBLAS to invert a matrix.
     * Templates used to enable a wider variety of matrix types.
     *
     * \param input Matrix to be inverted
     * \param inverse Matrix into which we should put the inverse
     */
    template<typename T, typename OUT, typename IN>
    bool inv (const IN& input, OUT& inverse)
    {
       using namespace boost::numeric::ublas;
       typedef permutation_matrix<std::size_t> pmatrix;
       // create a working copy of the input
       OUT A(input);
       // create a permutation matrix for the LU-factorization
       pmatrix pm(A.size1());
       // perform LU-factorization
       int res = lu_factorize(A,pm);
       if( res != 0 ) return false;
       // create identity matrix of "inverse"
       inverse.assign(identity_matrix<T>(A.size1()));
       // backsubstitute to get the inverse
       lu_substitute(A, pm, inverse);
       return true;
    }
  }
}

#endif /* MATRIXOPERATIONS_H_ */
