fMatrix.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * The University of Tokyo
 */
#ifndef __F_MATRIX_H__
#define __F_MATRIX_H__

#include <dims_common.h>
#include <dims_clapack.h>
#if defined(__ia64) || defined(__x68_64)
#include <cmath>
#else
#include <math.h>
#endif

#include <iostream>
using namespace std;

#include <assert.h>

class fVec;
class fMat;

fMat inv_svd(const fMat& mat, int lwork = -1);
fMat p_inv_svd(const fMat& mat, int lwork = -1);
fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork = -1);
fMat lineq_svd(const fMat& A, const fMat& b, int lwork = -1);


class fMat 
{
public:

        fMat() {
                p_data = 0;
                n_col = 0;
                n_row = 0;
                m_info = 0;
                temp = 0.0;
        }

        fMat(int m, int n, double* ini = 0) {
                p_data = 0;
                n_col = 0;
                n_row = 0;
                m_info = 0;
                temp = 0.0;
                resize(m, n);
                if(ini)
                {
                        int k = m * n;
                        for(int i=0; i<k; i++) p_data[i] = ini[i];
                }
        }

        fMat(int m, int n, double ini) {
                p_data = 0;
                n_col = 0;
                n_row = 0;
                m_info = 0;
                temp = 0.0;
                resize(m, n);
                int k = m * n;
                for(int i=0; i<k; i++) p_data[i] = ini;
        }

        fMat(const fMat& ini) {
                p_data = 0;
                n_col = 0;
                n_row = 0;
                m_info = ini.m_info;
                temp = ini.temp;
                resize(ini.row(), ini.col());
                int k = ini.row() * ini.col();
                for(int i=0; i<k; i++) p_data[i] = ini.data()[i];
        }

        fMat(const fVec& ini);
        
        ~fMat() {
                if(p_data) delete[] p_data;
        }


        int resize(int i, int j) {
                if(n_row == i && n_col == j) return 0;
                if(p_data) delete[] p_data;
                p_data = 0;
                n_row = i;
                n_col = j;
                if(n_row > 0 && n_col > 0)
                {
                        int nn = n_row * n_col;
                        p_data = new double[nn];
                        if(!p_data)
                        {
                                n_row = 0;
                                n_col = 0;
                                m_info = 1;
                                return -1;
                        }
                }
                return 0;
        }
        int col() const {
                return n_col;
        }
        int row() const {
                return n_row;
        }
        int info() {
                return m_info;
        }
        void setinfo(int _info) {
                m_info = _info;
        }

        double& operator() (int i, int j) {
                assert(i>=0 && i<n_row && j>=0 && j<n_col);
                return *(p_data + i + j*n_row);
        }
        
        double operator() (int i, int j) const {
                assert(i>=0 && i<n_row && j>=0 && j<n_col);
                return *(p_data + i + j*n_row);
        }

        fMat operator = (const fMat& mat);
        void operator = (double d);
        void set(double* _d) {
                int n_elements = n_row * n_col;
                dims_copy(_d, p_data, n_elements);
        }

        double* data() const {
                return p_data;
        }
        operator double *() {
                return p_data;
        }

        friend ostream& operator << (ostream& ost, const fMat& mat) {
                int i, j;
                ost << "[" << mat.row() << ", " << mat.col() << "]" << endl;
                for(i=0; i<mat.row(); i++)
                {
                        for(j=0; j<mat.col(); j++)
                        {
                                ost << *(mat.data() + i + j*mat.row());
                                if(j != mat.col()-1) ost << "\t";
                        }
                        if(i == mat.row()-1) ost << flush;
                        else ost << endl;
                }
                return ost;
        }
        

        void set_submat(int row_start, int col_start, const fMat& subM);


        void get_submat(int row_start, int col_start, const fMat& allM);

        friend fMat tran(const fMat& mat);

        void tran(const fMat& mat);

        void tran();

        void identity();

        void zero() {
                int ndata = n_row * n_col;
                for(int i=0; i<ndata; i++) p_data[i] = 0.0;
        }
                

        friend fMat inv(const fMat& mat);
        friend fMat p_inv(const fMat& mat);
        friend fMat sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k);
        friend fMat lineq(const fMat& A, const fMat& b);
        friend fMat lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b);

        int inv(const fMat&);
        int p_inv(const fMat&);
        int sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k);
        int lineq(const fMat& A, const fMat& b);
        int lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b);


        friend fMat inv_svd(const fMat& mat, int lwork);
        friend fMat p_inv_svd(const fMat& mat, int lwork);
        friend fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork);
        friend fMat lineq_svd(const fMat& A, const fMat& b, int lwork);

        int inv_svd(const fMat&, int lwork = -1);
        int p_inv_svd(const fMat&, int lwork = -1);
        int sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork = -1);
        int lineq_svd(const fMat& A, const fMat& b, int lwork = -1);

        int inv_posv(const fMat&);
        int lineq_posv(const fMat& A, const fMat& b);
        int inv_porfs(const fMat&);
        int lineq_porfs(const fMat& A, const fMat& b);

        friend fMat operator - (const fMat& mat);
        void operator += (const fMat& mat);
        void operator -= (const fMat& mat);
        void operator *= (double d);
        void operator /= (double d);

        friend fMat operator + (const fMat& mat1, const fMat& mat2);
        friend fMat operator - (const fMat& mat1, const fMat& mat2);
        friend fMat operator * (const fMat& mat1, const fMat& mat2);
        friend fVec operator * (const fMat& mat, const fVec& vec);
        friend fMat operator * (double d, const fMat& mat);
        friend fMat operator * (const fMat& mat, double d);
        friend fMat operator / (const fMat& mat, double d);
        void set(const fMat& mat);
        void neg(const fMat& mat);
        void add(const fMat& mat1, const fMat& mat2);
        void add(const fMat& mat);
        void sub(const fMat& mat1, const fMat& mat2);
        void mul(const fMat& mat1, const fMat& mat2);
        void mul(double d, const fMat& mat);
        void mul(const fMat& mat, double d);
        void mul_tran(const fMat& mat1, int trans_first);
        void div(const fMat& mat, double d);

        void dmul(const fMat& mat1, const fMat& mat2);
        void ddiv(const fMat& mat1, const fMat& mat2);


        int svd(fMat& U, fVec& Sigma, fMat& VT);


        void symmetric(char t = 'U');

        double det(void);

        friend fMat eigs(const fMat& mat, double *w);
        friend fMat eigs2(const fMat& mat, double *w);
        

        int eig(fVec& wr, fVec& wi);


        int eig(fVec& wr, fVec& wi, fMat& v);
        

        int eig(fVec& wr, fVec& wi, fMat& vr, fMat& vi);


        friend int inv_enlarge(const fMat& m12, const fMat& m21, const fMat& m22, const fMat& P, fMat& X, fMat& y, fMat& z, fMat& w);


        friend int inv_shrink(const fMat& P, const fMat& q, const fMat& r, const fMat& s, fMat& X);


        friend int inv_row_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y);
        

        friend int inv_col_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y);

protected:
        double* p_data;
        double temp;
        int m_info;
        int n_col;
        int n_row;
};

class fVec
        : public fMat
{
public:
        fVec() : fMat() {
        }
        fVec(int m, double* ini = 0) : fMat(m, 1, ini) {
        }
        fVec(int m, double ini) : fMat(m, 1, ini) {
        }
        fVec(const fVec& ini) : fMat(ini) {
        }
        ~fVec() {
        }

        int size() const {
                return n_row;
        }
        void resize(int i) {
                if(n_row == i) return;
                if(p_data) delete[] p_data;
                p_data = 0;
                n_row = i;
                n_col = 1;
                if(n_row > 0)
                {
                        p_data = new double[n_row];
                }
        }

        void get_subvec(int start, const fVec& allV);
        void set_subvec(int start, const fVec& subV);

        void operator = (double d);
        fVec operator = (const fVec& vec);

        void set(double* _d) {
                dims_copy(_d, p_data, n_row);
        }

        double& operator () (int i) {
                assert(i >= 0 && i < n_row);
                return *(p_data + i);
        }
        double operator () (int i) const {
                assert(i >= 0 && i < n_row);
                return *(p_data + i);
        }

        friend double length(const fVec& v) {
                return sqrt(v*v);
        }
        double length() const {
                return sqrt((*this) * (*this));
        }
        friend fVec unit(const fVec& v) {
                fVec ret(v.n_col);
                double len = v.length();
                ret = v / len;
                return ret;
        }
        void unit() {
                double len = length();
                (*this) /= len;
        }
        double sum() {
                double ret = 0.0;
                for(int i=0; i<n_row; i++) ret += p_data[i];
                return ret;
        }
        void zero() {
                for(int i=0; i<n_row; i++) p_data[i] = 0;
        }

        int max_index();
        double max_value();
        int min_index();
        double min_value();

        void abs();

        int lineq(const fMat& A, const fVec& b);
        int lineq_svd(const fMat& A, const fVec& b, int lwork = -1);
        int lineq_posv(const fMat& A, const fVec& b);
        int lineq_porfs(const fMat& A, const fVec& b);
        int lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fVec& b);

        friend fVec operator - (const fVec& vec);
        void operator += (const fVec& vec);
        void operator -= (const fVec& vec);
        void operator *= (double d);
        void operator /= (double d);

        friend fVec operator + (const fVec& vec1, const fVec& vec2);
        friend fVec operator - (const fVec& vec1, const fVec& vec2);
        friend fVec operator / (const fVec& vec, double d);
        friend fVec operator * (double d, const fVec& vec);
        friend fVec operator * (const fVec& vec, double d);
        friend double operator * (const fVec& vec1, const fVec& vec2);
        void set(const fVec& vec);
        void neg(const fVec& vec);
        void add(const fVec& vec1, const fVec& vec2);
        void add(const fVec& vec);
        void sub(const fVec& vec1, const fVec& vec2);
        void div(const fVec& vec, double d);
        void mul(const fVec& vec, double d);
        void mul(double d, const fVec& vec);
        void mul(const fMat& mat, const fVec& vec);
        void mul(const fVec& vec, const fMat& mat);
protected:
};

#endif