MC_matrix.cpp

Go to the documentation of this file.

#include <MC_matrix.hpp>

#include <cmath>
#include <MC_int_vector.hpp>
#include <MC_v3d.hpp>
#include <MC_v4d.hpp>
#include <MC_v3d_vector.hpp>
#include <MC_string_helper.hpp>



namespace mesh_conv
{



MC_matrix::MC_matrix(){current_size[0]=0;current_size[1]=0;resize(0);}

MC_matrix::MC_matrix(const int& n,const int& m)
{current_size[0]=0;current_size[1]=0;resize(n,m);}
MC_matrix::MC_matrix(const int& n)
{current_size[0]=0;current_size[1]=0;resize(n);}
MC_matrix::MC_matrix(const MC_v3d& v)
{
    current_size[0]=0;current_size[1]=0;
    resize(3,1);
    for(int k=0;k<3;k++)
        (*this)(k,0)=v[k];
}
MC_matrix::MC_matrix(const MC_v3d_vector& v)
{
    current_size[0]=0;current_size[1]=0;
    int N=v.size();
    resize(3,N);
    for(int k=0;k<N;k++)
        for(int k_dim=0;k_dim<3;k_dim++)
            (*this)(k_dim,k)=v[k][k_dim];
}
MC_matrix::MC_matrix(const MC_v3d& v1,const MC_v3d& v2)
{
    current_size[0]=0;current_size[1]=0;
    resize(3,2);
    for(int k_dim=0;k_dim<3;k_dim++)
        (*this)(k_dim,0)=v1[k_dim];
    for(int k_dim=0;k_dim<3;k_dim++)
        (*this)(k_dim,1)=v2[k_dim];
}
MC_matrix::MC_matrix(const MC_v3d& v1,const MC_v3d& v2,const MC_v3d& v3)
{
    current_size[0]=0;current_size[1]=0;
    resize(3,3);
    for(int k_dim=0;k_dim<3;k_dim++)
        (*this)(k_dim,0)=v1[k_dim];
    for(int k_dim=0;k_dim<3;k_dim++)
        (*this)(k_dim,1)=v2[k_dim];
    for(int k_dim=0;k_dim<3;k_dim++)
        (*this)(k_dim,2)=v3[k_dim];
}

MC_matrix::MC_matrix(const MC_matrix& _M)
{M=_M.M;for(int k_dim=0;k_dim<2;k_dim++){current_size[k_dim]=_M.current_size[k_dim];}}

MC_matrix::~MC_matrix()
{clear();}

MC_matrix& MC_matrix::clear()
{
  M.resize(0);
  current_size[0]=0;
  current_size[1]=0;
  return *this;
}


void MC_matrix::check_integrity() const
{
  if(int(M.size())!=current_size[0]*current_size[1])
    {std::cout<<"Error in check_integrity in MC_matrix::check_integrity(), size of M is "<<M.size()<<", and size recorded is ("<<current_size[0]<<","<<current_size[1]<<")"<<std::endl;exit(-1);}
}


int MC_matrix::size_1() const {return current_size[0];}
int MC_matrix::size_2() const {return current_size[1];}
MC_int_vector MC_matrix::size() const
{return MC_int_vector(current_size[0],current_size[1]);}
int MC_matrix::size_vec() const
{
  if(current_size[1]==1)
    return current_size[0];
  else if(current_size[0]==1)
    return current_size[1];

  std::cout<<"Error in MC_matrix::size_vec(), matrix is not a vector (size=("<<size()<<"))"<<std::endl;
  exit(-1);
}

double& MC_matrix::operator()(const int& k1,const int& k2)
{
  if(k1<0 || k2<0)
    {std::cout<<"Error in MC_matrix::operator("<<k1<<","<<k2<<"), size=("<<size()<<")"<<std::endl;exit(-1);}

  // resize if needed
  if(k1>=current_size[0] || k2>=current_size[1])
    resize(std::max(k1+1,current_size[0]),std::max(k2+1,current_size[1]));

  int index=k1+current_size[0]*k2;
  if(index>=M.size()){std::cout<<"Error in MC_matrix::operator("<<k1<<","<<k2<<"), index="<<index<<" and size of M is "<<M.size()<<std::endl;exit(-1);}

  return M[index];
}

const double& MC_matrix::operator()(const int& k1,const int& k2) const
{
  if(k1<0 || k1>=current_size[0] || k2<0 || k2>=current_size[1])
    {std::cout<<"Error in MC_matrix::operator("<<k1<<","<<k2<<"), size=("<<size()<<")"<<std::endl;exit(-1);}

  int index=k1+current_size[0]*k2;
  if(index>=M.size()){std::cout<<"Error in MC_matrix::operator("<<k1<<","<<k2<<"), index="<<index<<" and size of M is "<<M.size()<<std::endl;exit(-1);}

  return M[index];
}


double& MC_matrix::operator()(const int& k_index)
{
  
  if(k_index<0)
    {std::cout<<"Error in MC_matrix::operator("<<k_index<<"), size=("<<M.size()<<")"<<std::endl;exit(-1);}

  if(k_index>=M.size())
    {
      if(size_2()==1)
        resize(k_index+1,1);
      else if(size_1()==1 || size_1()==0 || size_2()==0)
        resize(1,k_index+1);
      else
        {std::cout<<"Error in MC_matrix::operator("<<k_index<<"), size=("<<M.size()<<")"<<std::endl;exit(-1);}
    }
  return M[k_index];
}

const double& MC_matrix::operator()(const int& k_index) const
{
  if(k_index<0 || k_index>=M.size())
    {std::cout<<"Error in MC_matrix::operator("<<k_index<<"), size=("<<M.size()<<")"<<std::endl;exit(-1);}
  return M[k_index];
}

MC_matrix& MC_matrix::resize(const MC_int_vector& new_size)
{

    // do not change anything
    if( (new_size[0]==current_size[0]) && ((new_size[1]==current_size[1]) || (new_size[1]==-1&&new_size[0]==current_size[1])))
        return *this;

    MC_int_vector temp_new_size=new_size;
    if(new_size[1]==-1)
            temp_new_size[1]=temp_new_size[0];

    // copy
    MC_double_vector new_matrix=MC_double_vector::zeros(std::max(temp_new_size[0],current_size[0])*std::max(temp_new_size[1],current_size[1]));

    int L1=std::min(temp_new_size[0],current_size[0]);
    int L2=std::min(temp_new_size[1],current_size[1]);
    for(int k_1=0;k_1<L1;k_1++)
        for(int k_2=0;k_2<L2;k_2++)
            new_matrix[k_1+k_2*temp_new_size[0]] = M[k_1+k_2*current_size[0]];


    // record it
    M=new_matrix;
    current_size[0]=temp_new_size[0];
    current_size[1]=temp_new_size[1];


    return *this;

}
MC_matrix& MC_matrix::resize(const int& new_size_1,const int& new_size_2)
{resize(MC_int_vector(new_size_1,new_size_2));return *this;}

MC_matrix& MC_matrix::resize_1(const int& new_size_1)
{resize(new_size_1,size_2());return *this;}
MC_matrix& MC_matrix::resize_2(const int& new_size_2)
{resize(size_1(),new_size_2);return *this;}

std::ostream& operator<<(std::ostream& stream, const MC_matrix& _M)
{
  int k_1=0,k_2=0;
  int N_1=_M.size_1(),N_2=_M.size_2();
  for(k_1=0;k_1<N_1;k_1++)
    {
      for(k_2=0;k_2<N_2;k_2++)
        stream<<_M(k_1,k_2)<<" ";
      stream<<std::endl;
    }
  return stream;
}





//
//MC_matrix MC_matrix::submatrix(const char* string_index_x,const char* string_index_y) const
//{return submatrix(std::string(string_index_x),std::string(string_index_y));}
//
//MC_matrix MC_matrix::submatrix(const std::string& string_index_x,const std::string& string_index_y) const
//{
//
//  int start=0,end=0;
//  double increment=0.0;
//
//  analyse_string(string_index_x,0,&start,&increment,&end);
//  int_vector index_x = int_vector::linspace(start,increment,end);
//
//  analyse_string(string_index_y,1,&start,&increment,&end);
//  int_vector index_y = int_vector::linspace(start,increment,end);
//
//  return submatrix(index_x,index_y);
//}


MC_matrix MC_matrix::operator()(const MC_int_vector& index_1,const MC_int_vector& index_2) const
{
    MC_matrix temp_matrix;

    int k_index_1=0,N_index_1=index_1.size();
    int k_index_2=0,N_index_2=index_2.size();
    int current_x=0,current_y=0;
    for(k_index_1=0;k_index_1<N_index_1;k_index_1++)
    {
        current_x = index_1[k_index_1];
        for(k_index_2=0;k_index_2<N_index_2;k_index_2++)
        {
            current_y = index_2[k_index_2];
            temp_matrix(k_index_1,k_index_2) = (*this)(current_x,current_y);
        }
    }

    return temp_matrix;
}


// analyse "a:b:c"
void MC_matrix::analyse_string(const std::string& to_analyse,int dim,int *start,double *increment,int *end) const
{
  
  if(dim!=-1 && dim!=0 && dim!=1)
    {std::cout<<"Error in MC_matrix::analyse_string("<<to_analyse<<","<<dim<<", ...) dim="<<dim<<"should be 0 or 1"<<std::endl;exit(-1);}
  
  std::vector <std::string> token = mesh_conv::MC_string_tokenizer::tokenize(to_analyse,":");
  if(token.size()<1 || token.size()>3)
    {std::cout<<"Error in MC_matrix::analyse_string("<<to_analyse<<","<<dim<<", ...), cannot analyse string "<<to_analyse<<std::endl;exit(-1);}

  std::vector <std::string> token_start = mesh_conv::MC_string_tokenizer::tokenize(token[0],"-");
  std::vector <std::string> token_end = mesh_conv::MC_string_tokenizer::tokenize(token[token.size()-1],"-");
  int current_value=0;
  int temp_value=0;

  // special case of ":"
  if(to_analyse==":")
    {
      *start=0;
      if(dim==-1)
        *end=size_1()*size_2()-1;
      else if(dim==0)
        *end=size_1()-1;
      else
        *end=size_2()-1;
      *increment=1;
      return ;
    }
  // special case, only one number
  int number=0;
  if(token.size()==1)
  {
      bool is_converted=false;
      number = MC_string_converter::value_of<int>(to_analyse,&is_converted);
      if(is_converted==true)
      {
          *start=number;*end=number;*increment=1;
          return;
      }
  }

  for(unsigned int k=0;k<token_start.size();k++)
    {
      if(token_start[k]=="start")
        temp_value=0;
      else if(token_start[k]=="end")
          if(dim==-1)
              temp_value=size_1()*size_2()-1;
      else if(dim==0)
          temp_value=size_1()-1;
      else
          temp_value=size_2()-1;
      else
      {
          bool is_converted=false;
          temp_value=MC_string_converter::value_of<int>(token_start[k],&is_converted);
          if(is_converted==false)
          {std::cout<<"Error in MC_matrix::analyse_string("<<to_analyse<<","<<dim<<", ...), cannot interpret string"<<std::endl;exit(-1);}
      }
      
      if(k>0)
        current_value -= temp_value;
      else
        current_value = temp_value;

      *start = current_value;
    }

  current_value=0;
  for(unsigned int k=0;k<token_end.size();k++)
    {
      if(token_end[k]=="start")
        temp_value=0;
      else if(token_end[k]=="end")
        if(dim==-1)
          temp_value=size_1()*size_2()-1;
        else if(dim==0)
          temp_value=size_1()-1;
        else
          temp_value=size_2()-1;
        else
        {
            bool is_converted=false;
            temp_value=MC_string_converter::value_of<int>(token_end[k],&is_converted);
            if(is_converted==false)
            {std::cout<<"Error in MC_matrix::analyse_string("<<to_analyse<<","<<dim<<", ...), cannot interpret string"<<token_end[k]<<std::endl;exit(-1);}
        }
        if(k>0)
            current_value -= temp_value;
        else
            current_value = temp_value;

        *end = current_value;
    }

  if(token.size()==3)//there is an increment
    {
      bool is_converted=false;
      *increment = MC_string_converter::value_of <double>(token[1],&is_converted);

      if(is_converted==false)
      {std::cout<<"Error in MC_matrix::analyse_string("<<to_analyse<<","<<dim<<", ...), cannot interpret increment "<<token[1]<<std::endl;exit(-1);}
      return;
    }
  
  
  *increment=1;//default value

}  

void MC_matrix::analyse_string(const char* to_analyse,int dim,int *start,double *increment,int *end) const
{analyse_string(std::string(to_analyse),dim,start,increment,end);}

MC_matrix MC_matrix::reshaped_1(const int& new_size_1) const
{
  int N=M.size();
  if(N%new_size_1 != 0)
    {std::cout<<"Error in MC_matrix::reshape_1("<<new_size_1<<") in MC_matrix, total size "<<M.size()<<" are not compatible"<<std::endl;exit(-1);}
  return reshaped(new_size_1,N/new_size_1);
}

MC_matrix MC_matrix::reshaped_2(const int& new_size_2) const
{
  int N=M.size();
  if(N%new_size_2 != 0)
  {std::cout<<"Error in MC_matrix::reshape_2("<<new_size_2<<") in MC_matrix, total size "<<M.size()<<" are not compatible"<<std::endl;exit(-1);}
  return reshaped(N/new_size_2,new_size_2);
}
MC_matrix MC_matrix::reshaped(const MC_int_vector& new_size) const
{
    // if do nothing
    if(new_size[0]==current_size[0] && new_size[1]==current_size[1])
        return *this;

  if(new_size[0]*new_size[1] != M.size())
    {std::cout<<"Error in MC_matrix::reshape("<<new_size<<"), total size should be "<<M.size()<<std::endl;exit(-1);}

  MC_matrix temp_matrix(new_size[0],new_size[1]);

  int k1=0,k2=0;
  int count=0;
  for(k2=0;k2<new_size[1];k2++)
    for(k1=0;k1<new_size[2];k1++)
      temp_matrix(k1,k2) = M[count++];

  return temp_matrix;
}
MC_matrix MC_matrix::reshaped(const int& new_size_1,const int& new_size_2) const
{return reshaped(MC_int_vector(new_size_1,new_size_2));}

MC_matrix MC_matrix::transposed() const
{
  MC_matrix new_matrix(size_2(),size_1());
  int k1=0,N1=size_1();
  int k2=0,N2=size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      new_matrix(k2,k1)=(*this)(k1,k2);
  return new_matrix;
}


MC_matrix MC_matrix::zeros(const int& new_size_1,const int& new_size_2)
{return MC_matrix(new_size_1,new_size_2);}
MC_matrix MC_matrix::zeros(const int& new_size)
{return MC_matrix(new_size);}

MC_matrix MC_matrix::identity(const int& new_size)
{
  MC_matrix temp(new_size);
  for(int k=0;k<new_size;k++)
    temp(k,k)=1;
  return temp;
}

MC_matrix& MC_matrix::set_block(const MC_int_vector& index_1,const MC_int_vector& index_2,const MC_matrix& block)
{
  int k1=0,N1=index_1.size();
  int k2=0,N2=index_2.size();

  if(N1!=block.size_1() || N2!=block.size_2())
    {std::cout<<"Error in MC_matrix::set_block(MC_int_vector,MC_int_vector,MC_matrix), size are not compatible ("<<N1<<","<<N2<<") - ("<<block.size_1()<<","<<block.size_2()<<")"<<std::endl;exit(-1);}

  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      (*this)(index_1[k1],index_2[k2])=block(k1,k2);

  return *this;
}

MC_matrix& MC_matrix::set_block(const int& start_x,const int& end_x,const int& start_y,const int& end_y,const MC_matrix& block)
{return set_block(MC_int_vector::linspace(start_x,end_x),MC_int_vector::linspace(start_y,end_y),block);}

MC_matrix MC_matrix::repmat(const int& k_repeat) const
{return repmat(k_repeat,k_repeat);}
MC_matrix MC_matrix::repmat(const MC_int_vector& k_repeat) const
{
    if(k_repeat.size()!=2)
    {std::cout<<"Error in MC_matrix::repmat("<<k_repeat<<"), size must be 2"<<std::endl;exit(-1);}
    return repmat(k_repeat[0],k_repeat[1]);
}
MC_matrix MC_matrix::repmat(const int& k_repeat_1,const int& k_repeat_2) const
{
    MC_matrix new_matrix(k_repeat_1*size_1(),k_repeat_2*size_2());

    MC_int_vector L1,L2;
    int k1=0,k2=0;
    for(k1=0;k1<k_repeat_1;k1++)
    {
        L1 = MC_int_vector::linspace(k1*size_1(),(k1+1)*size_1()-1);
        for(k2=0;k2<k_repeat_2;k2++)
        {
            L2 = MC_int_vector::linspace(k2*size_2(),(k2+1)*size_2()-1);
            new_matrix.set_block(L1,L2,(*this));
        }
    }
    return new_matrix;
}

MC_matrix MC_matrix::repmat_1(const int& k_repeat_1) const {return repmat(k_repeat_1,1);}
MC_matrix MC_matrix::repmat_2(const int& k_repeat_2) const {return repmat(1,k_repeat_2);}


MC_matrix operator+(const MC_matrix& M1,const double& alpha)
{
  MC_matrix temp=M1;
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M1.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      temp(k1,k2)+=alpha;
  return temp;
}
MC_matrix operator+(const double& alpha,const MC_matrix& M1)
{return M1+alpha;}

MC_matrix operator-(const MC_matrix& M1,const double& alpha)
{
  MC_matrix temp=M1;
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M1.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      temp(k1,k2)-=alpha;
  return temp;
}
MC_matrix operator-(const double& alpha,const MC_matrix& M1)
{
  MC_matrix temp=M1;
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M1.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      temp(k1,k2)=alpha-M1(k1,k2);
  return temp;
}

MC_matrix operator*(const MC_matrix& M1,const double& alpha)
{
  MC_matrix temp=M1;
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M1.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      temp(k1,k2)*=alpha;
  return temp;
}
MC_matrix operator*(const double& alpha,const MC_matrix& M1)
{return M1*alpha;}
MC_matrix operator/(const MC_matrix& M1,const double& alpha)
{

    double epsilon=0.000001;
    if(abs(alpha)<epsilon)
    {std::cout<<"Error in MC_matrix::operator/("<<alpha<<"), value too small"<<std::endl;exit(-1);}

    MC_matrix temp=M1;
    int k1=0,k2=0;
    int N1=M1.size_1(),N2=M1.size_2();
    for(k1=0;k1<N1;k1++)
        for(k2=0;k2<N2;k2++)
            temp(k1,k2)/=alpha;
    return temp;
}
MC_matrix operator/(const double& alpha,const MC_matrix& M1)
{
  double epsilon=0.000001;
  
  double current=0.0;
  MC_matrix temp=M1;
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M1.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      {
        current = M1(k1,k2);
        if(abs(current)<epsilon)
          {std::cout<<"Error in MC_matrix::operator/("<<alpha<<"), value too small at ("<<k1<<","<<k2<<")"<<std::endl;exit(-1);}

        temp(k1,k2)=alpha/current;
      }
  return temp;
}

MC_matrix operator+(const MC_matrix& M1,const MC_matrix& M2)
{
  if(M1.size_1()!=M2.size_1() || M1.size_2()!=M2.size_2())
    {std::cout<<"Error in MC_matrix::operator+(matrix,matrix), size are not compatible ("<<M1.size_1()<<","<<M1.size_2()<<")!=("<<M2.size_1()<<","<<M2.size_2()<<std::endl;exit(-1);}
  
  MC_matrix temp(M1.size_1(),M1.size_2());
  int k=0,N=M1.M.size();
  for(k=0;k<N;k++)
    temp.M[k] = M1.M[k]+M2.M[k];
  return temp;
}

MC_matrix operator-(const MC_matrix& M1,const MC_matrix& M2)
{
  if(M1.size_1()!=M2.size_1() || M1.size_2()!=M2.size_2())
    {std::cout<<"Error in MC_matrix::operator-(matrix,matrix), size are not compatible ("<<M1.size_1()<<","<<M1.size_2()<<")!=("<<M2.size_1()<<","<<M2.size_2()<<std::endl;exit(-1);}
  
  MC_matrix temp(M1.size_1(),M1.size_2());
  int k=0,N=M1.M.size();
  for(k=0;k<N;k++)
    temp.M[k] = M1.M[k]-M2.M[k];
  return temp;
}

MC_matrix operator*(const MC_matrix& M1,const MC_matrix& M2)
{
  if(M1.size_2()!=M2.size_1())
    {std::cout<<"Error in MC_matrix::operator*(matrix,matrix), size are not compatible ("<<M1.size_1()<<","<<M1.size_2()<<") x ("<<M2.size_1()<<","<<M2.size_2()<<")"<<std::endl;exit(-1);}
  
  MC_matrix temp(M1.size_1(),M2.size_2());
  int k=0,N=M1.size_2();
  int k1=0,k2=0;
  int N1=M1.size_1(),N2=M2.size_2();
  for(k1=0;k1<N1;k1++)
    for(k2=0;k2<N2;k2++)
      for(k=0;k<N;k++)
        temp(k1,k2) += M1(k1,k)*M2(k,k2);

  return temp;
}




MC_matrix MC_matrix::inverted() const
{
    if(size_1()!=size_2())
    {std::cout<<"Error in MC_matrix::invertd(), matrix is not square ! ("<<size_1()<<","<<size_2()<<")"<<std::endl;exit(-1);}


    double epsilon=0.0000001;
    if(size_1()==2 && size_2()==2)
    {
        double det=(*this)(0,0)*(*this)(1,1)-(*this)(1,0)*(*this)(0,1);
        if(std::abs(det)<epsilon)
        {std::cout<<"Error in MC_matrix::inverted() (2x2), matrix has null determinant"<<std::endl;}
        MC_matrix res(2);
        res(0,0)= (*this)(1,1);
        res(1,0)=-(*this)(1,0);
        res(0,1)=-(*this)(0,1);
        res(1,1)= (*this)(0,0);
        res/=det;
        return res;
    }
    else if(size_1()==3 && size_2()==3)
    {
        double v00=(*this)(0,0),v01=(*this)(0,1),v02=(*this)(0,2);
        double v10=(*this)(1,0),v11=(*this)(1,1),v12=(*this)(1,2);
        double v20=(*this)(2,0),v21=(*this)(2,1),v22=(*this)(2,2);

        double det=
                +v00*(v11*v22-v12*v21)
                -v01*(v10*v22-v12*v20)
                +v02*(v10*v21-v11*v20);



        if(std::abs(det)<epsilon)
        {std::cout<<"Warning in MC_matrix::inverted() (3x3), matrix has null determinant"<<std::endl;return MC_matrix::identity(3);}

        MC_matrix res(3);
        res(0,0) = v11*v22-v12*v21;
        res(0,1) = v02*v21-v01*v22;
        res(0,2) = v01*v12-v02*v11;

        res(1,0) = v12*v20-v10*v22;
        res(1,1) = v00*v22-v02*v20;
        res(1,2) = v02*v10-v00*v12;

        res(2,0) = v10*v21-v11*v20;
        res(2,1) = v01*v20-v00*v21;
        res(2,2) = v00*v11-v01*v10;


        res/=det;
        return res;
    }
    else
    {
        exit(-1);

    }
    exit(-1);

}







MC_matrix MC_matrix::lsqr_invert() const
{return ( ((*this).transposed()) * (*this) ).inverted() * (*this).transposed();}


MC_matrix operator*(const MC_matrix& M1,const MC_v3d& V)
{
  MC_matrix vec(V);

  
  if(M1.size_2()==4)
    vec(3,0)=1; //automatic add the 1 value


  if(M1.size_2()==3 || M1.size_2()==4) 
    {return M1*vec;}
  else {std::cout<<"Error in MC_matrix::operator*(MC_matrix,MC_v3d), size of Matrix ("<<M1.size_1()<<","<<M1.size_2()<<") are not compatible"<<std::endl;exit(-1);}
  
}


MC_matrix MC_matrix::rotation_axis_to_axis(const MC_v3d& a1,const MC_v3d& a2)
{

  MC_v3d v0,v1;
  v0 = a1.normalized();
  v1 = a2.normalized();

  MC_v3d n = (v0.cross(v1));
  if(n.norm()<0.00001)
  {
      return MC_matrix::identity(3);
      //std::cout<<"Warning in MC_matrix::rotation_axis_to_axis("<<a1<<","<<a2<<"), parralel vector or norm is null, axis of rotation has norm="<<n.norm()<<std::endl;
  }
  n=n.normalized();

  double cos_t = v0.dot(v1);
  double sin_t = pow(fabs(1.0-cos_t*cos_t),0.5);

  MC_matrix R(3,3);
  R(0,0) = cos_t+n[0]*n[0]*(1-cos_t);
  R(1,0) = n[2]*sin_t+n[0]*n[1]*(1-cos_t);
  R(2,0) =-n[1]*sin_t+n[0]*n[2]*(1-cos_t);

  R(0,1) = n[0]*n[1]*(1-cos_t)-n[2]*sin_t;
  R(1,1) = cos_t+n[1]*n[1]*(1.0-cos_t);
  R(2,1) = n[0]*sin_t+n[1]*n[2]*(1-cos_t);

  R(0,2) = n[1]*sin_t+n[0]*n[2]*(1-cos_t);
  R(1,2) =-n[0]*sin_t+n[1]*n[2]*(1-cos_t);
  R(2,2) = cos_t+n[2]*n[2]*(1-cos_t);

  return R;

}





MC_matrix MC_matrix::operator()(const std::string& string_index)
{
    int start=0;
    double increment=0;
    int end=0;

    analyse_string(string_index,-1,&start,&increment,&end);
    return (*this)(MC_int_vector::linspace(start,end,increment));
}
MC_matrix MC_matrix::operator()(const MC_int_vector& index)
{
    MC_matrix new_matrix(index.size(),1);
    int N=index.size();
    for(int k=0;k<N;k++)
        new_matrix(k)=(*this)(index[k]);
    return new_matrix;

}

MC_matrix MC_matrix::operator()(const std::string& string_index_1,const std::string& string_index_2)
{
    int start=0,end=0;
    double increment=0.0;

    analyse_string(string_index_1,0,&start,&increment,&end);
    MC_int_vector index_x = MC_int_vector::linspace(start,end,increment);

    analyse_string(string_index_2,1,&start,&increment,&end);
    MC_int_vector index_y = MC_int_vector::linspace(start,end,increment);

    return (*this)(index_x,index_y);
}

MC_matrix MC_matrix::rotation(const MC_v3d& _n,const double& angle)
{
  MC_v3d n=_n.normalized();



  double cos_t = cos(angle);
  double sin_t = sin(angle);

  MC_matrix R(3,3);

  R(0,0) = cos_t+n[0]*n[0]*(1-cos_t);
  R(1,0) = n[2]*sin_t+n[0]*n[1]*(1-cos_t);
  R(2,0) =-n[1]*sin_t+n[0]*n[2]*(1-cos_t);

  R(0,1) = n[0]*n[1]*(1-cos_t)-n[2]*sin_t;
  R(1,1) = cos_t+n[1]*n[1]*(1.0-cos_t);
  R(2,1) = n[0]*sin_t+n[1]*n[2]*(1-cos_t);

  R(0,2) = n[1]*sin_t+n[0]*n[2]*(1-cos_t);
  R(1,2) =-n[0]*sin_t+n[1]*n[2]*(1-cos_t);
  R(2,2) = cos_t+n[2]*n[2]*(1-cos_t);
  
  return R;
}




const double& MC_matrix::to_double() const
{
  if(size_1()==1 && size_2()==1)
    return M[0];
  else
    {std::cout<<"Error in MC_matrix::to_double(), size are not compatible (1,1) => ("<<size_1()<<","<<size_2()<<")"<<std::endl; exit(-1);}

}

double MC_matrix::norm_2() const
{
  int k_1=0,k_2=0;
  double n=0.0;
  for(k_1=0;k_1<current_size[0];k_1++)
    for(k_2=0;k_2<current_size[1];k_2++)
      n += (*this)(k_1,k_2)*(*this)(k_1,k_2);
  n = pow(n,0.5);

  return n;
}


MC_matrix& MC_matrix::operator+=(const double& a)
{
  int N=M.size();
  for(int k=0;k<N;k++)
    M[k]+=a;
  return *this;
}

MC_matrix& MC_matrix::operator+=(const MC_matrix& _m)
{
  if(_m.size_1()!=size_1() || _m.size_2()!=size_2())
    {std::cout<<"Error in MC_matrix::operator+=(MC_matrix), size are not compatible ("<<size_1()<<","<<size_2()<<") != ("<<_m.size_1()<<","<<_m.size_2()<<")"<<std::endl;exit(-1);}
  
  int N=M.size();
  for(int k=0;k<N;k++)
    M[k]+=_m(k);

  return *this;
}

MC_matrix& MC_matrix::operator*=(const double& a)
{
  int N=M.size();
  for(int k=0;k<N;k++)
    M[k]*=a;

  return *this;
}

MC_matrix& MC_matrix::operator/=(const double& a)
{
  double epsilon=0.00000001;
  if(std::abs(a)<epsilon)
    {std::cout<<"Error in MC_matrix::operator/=("<<a<<"), divide by zero"<<std::endl;exit(-1);}

  int N=M.size();
  for(int k=0;k<N;k++)
    M[k]/=a;

  return *this;
}


MC_matrix MC_matrix::exp_m() const
{

  if(size_1()!=size_2())
    {std::cout<<"Error in MC_matrix::exp(), matrix is not square ("<<size_1()<<","<<size_2()<<")"<<std::endl;exit(-1);}
  

  MC_matrix A=*this;
  double j=std::max(double(0.0),double(1.0+floor(logf(float(A.norm_2()))/logf(2.0))));


  A = A*pow(2.0,-j);
  MC_matrix D=MC_matrix::identity(size_1());
  MC_matrix N=D;
  MC_matrix X=D;
  double c=1.0;

  int q=6;
  for(int k=1;k<=q;k++)
    {
      c = c*(q-k+1)/(k*(2*q-k+1));
      X = A*X;
      N = N+c*X;
      D = D+pow(-1.0,k)*c*X;
    }

  X = D.inverted()*N;
  X = X.pow_m(int(pow(2.0,j)));


  return X;
}

MC_matrix MC_matrix::pow_m(const int& k_pow) const
{
  if(size_1()!=size_2())
    {std::cout<<"Error in MC_matrix::pow(), matrix is not square ("<<size_1()<<","<<size_2()<<")"<<std::endl;exit(-1);}

  if(k_pow<0)
    {std::cout<<"Error in MC_matrix::pow("<<k_pow<<"), k_pow must be positiv"<<std::endl;exit(-1);}

  MC_matrix Res = MC_matrix::identity(size_1());
  for(int k=0;k<k_pow;k++)
    Res = Res*(*this);
  return Res;
}


MC_matrix& MC_matrix::add_row(const MC_v3d& x0)
{
  if(size_2()!=3)
    {std::cout<<"Error in MC_matrix::add_row, size_2="<<size_2()<<" and must be 3"<<std::endl;exit(-1);}
  
  int Nx = size_1();
  resize(Nx+1,3);
  set_block(Nx+1,Nx+1,0,2,MC_matrix(x0));

  return *this;
}
MC_matrix& MC_matrix::add_row(const MC_v3d& x0,const MC_v3d& x1)
{add_row(x0); add_row(x1); return *this;}
MC_matrix& MC_matrix::add_row(const MC_v3d& x0,const MC_v3d& x1,const MC_v3d& x2)
{add_row(x0,x1); add_row(x2); return *this;}

MC_matrix& MC_matrix::add_col(const MC_v3d& x0)
{
  if(size_1()!=3)
    {std::cout<<"Error in MC_matrix::add_col, size_1="<<size_1()<<" and must be 3"<<std::endl;exit(-1);}
  
  int Ny = size_2();
  resize(2,Ny+1);
  set_block(0,2,Ny+1,Ny+1,MC_matrix(x0));

  return *this;
}
MC_matrix& MC_matrix::add_col(const MC_v3d& x0,const MC_v3d& x1)
{add_col(x0); add_col(x1); return *this;}
MC_matrix& MC_matrix::add_col(const MC_v3d& x0,const MC_v3d& x1,const MC_v3d& x2)
{add_col(x0,x1); add_col(x2); return *this;
}

std::pair <MC_matrix,MC_matrix> MC_matrix::polar_decomposition() const
{
  int counter=0;
  int max_counter=2000;
  bool loop_finished=false;

  MC_matrix R0=*this;
  MC_matrix R1;
  double epsilon=0.000001;

  while(loop_finished==false)
    {

      R1 = 0.5*(R0+R0.inverted().transposed());

      counter++;
      if(counter>=max_counter || (R1-R0).norm_2()<epsilon)
        loop_finished=true;
      else
        R0=R1;
    }
  
  if(counter>=max_counter)
    {std::cout<<"Error in MC_matrix::polar_decomposition(), no convergence"<<std::endl;exit(-1);}

  MC_matrix U = R1.inverted()*(*this);

  return std::pair<MC_matrix,MC_matrix>(R1,U);
}

double MC_matrix::trace() const
{
  if(size_1()!=size_2())
    {std::cout<<"Error in MC_matrix::trace(), not a square matrix"<<std::endl;exit(-1);}

  double tr=0.0;
  for(int k=0;k<size_1();k++)
    tr += (*this)(k,k);

  return tr;
  
}

MC_matrix& MC_matrix::add_col(const MC_double_vector& col)
{
  if(size_1()==0 && size_2()==0)
    {resize(col.size(),1);return set_col(0,col);}
  
  if(size_1()!=col.size())
    {std::cout<<"Error in MC_matrix::add_col(double_vector), size are not compatible: matrix is ("<<size_1()<<"x"<<size_2()<<"), and column vector has size "<<col.size()<<std::endl;exit(-1);}
  
  resize(size_1(),size_2()+1);
  return set_col(size_2()-1,col);
}

MC_matrix& MC_matrix::add_row(const MC_double_vector& row)
{
  if(size_1()==0 && size_2()==0)
    {resize(1,row.size());return set_row(0,row);}
  
  if(size_2()!=row.size())
    {std::cout<<"Error in MC_matrix::add_row(double_vector), size are not compatible: matrix is ("<<size_1()<<"x"<<size_2()<<"), and row vector has size "<<row.size()<<std::endl;exit(-1);}
  
  resize(size_1()+1,size_2());
  return set_row(size_1()-1,row);
}

MC_matrix& MC_matrix::set_col(const int& id_col,const MC_double_vector& col)
{
  int N=col.size();
  for(int k=0;k<N;k++)
    (*this)(k,id_col)=col(k);
  return *this;
}
MC_matrix& MC_matrix::set_row(const int& id_row,const MC_double_vector& row)
{
  int N=row.size();
  for(int k=0;k<N;k++)
    (*this)(id_row,k)=row(k);
  return *this;
}

std::pair<std::vector <MC_matrix>,std::vector <MC_matrix> > MC_matrix::polar_decomposition(const std::vector <MC_matrix>& v_m)
{
  std::pair <std::vector<MC_matrix>,std::vector<MC_matrix> > ret;
  int N=v_m.size();
  ret.first.resize(N);
  ret.second.resize(N);

  for(int k=0;k<N;k++)
    {
      std::pair <MC_matrix,MC_matrix> temp_polar=v_m[k].polar_decomposition();
      ret.first[k]=temp_polar.first;
      ret.second[k]=temp_polar.second;
    }
  return ret;
}


std::pair <MC_matrix,std::vector <MC_matrix> > MC_matrix::rotation_axis_to_axis_with_gradient(const MC_v3d& a0,const MC_v3d& a1)
{
  double epsilon=0.00001;
  if(abs(a0.norm()-1.0)>epsilon || abs(a1.norm()-1.0)>epsilon)
    {std::cout<<"Error in rotation_axis_to_axis_with_gradient(), vector are not of norm 1"<<std::endl;exit(-1);}


  std::pair <MC_v3d,MC_v3d_vector> n_grad = (a0.cross(a1)).normalized_with_gradient();
  MC_v3d n =n_grad.first;
  MC_v3d_vector gradient_normal=n_grad.second;

  double cos_t=a0.dot(a1);
  double sin_t=pow(abs(1-cos_t*cos_t),0.5);
  
  MC_matrix R(3,3);
  R(0,0) = cos_t+n[0]*n[0]*(1-cos_t);
  R(1,0) = n[2]*sin_t+n[0]*n[1]*(1-cos_t);
  R(2,0) =-n[1]*sin_t+n[0]*n[2]*(1-cos_t);

  R(0,1) = n[0]*n[1]*(1-cos_t)-n[2]*sin_t;
  R(1,1) = cos_t+n[1]*n[1]*(1.0-cos_t);
  R(2,1) = n[0]*sin_t+n[1]*n[2]*(1-cos_t);

  R(0,2) = n[1]*sin_t+n[0]*n[2]*(1-cos_t);
  R(1,2) =-n[0]*sin_t+n[1]*n[2]*(1-cos_t);
  R(2,2) = cos_t+n[2]*n[2]*(1-cos_t);


  double x00=a0[0],x01=a0[1],x02=a0[2];
  double x10=a1[0],x11=a1[1],x12=a1[2];


  // nabla cos(t)
  MC_double_vector nabla_c=MC_double_vector(x10)<<x11<<x12<<x00<<x01<<x02;
  // nabla sin(t)
  MC_double_vector nabla_s=-cos_t/(sin_t+epsilon)*nabla_c;


  // nabla nx
  MC_double_vector nabla_nx=MC_double_vector(0.0)<<x12<<-x11<<0.0<<-x02<<x01;
  // nabla ny
  MC_double_vector nabla_ny=MC_double_vector(-x12)<<0.0<<x10<<x02<<0.0<<-x00;
  // nabla nz
  MC_double_vector nabla_nz=MC_double_vector(x11)<<-x10<<0.0<<-x01<<x00<<0.0;



  MC_v3d_vector gradient_normalized(6);
  for(int k=0;k<6;k++)
    for(int k_dim=0;k_dim<3;k_dim++)
      gradient_normalized[k][k_dim]=gradient_normal[k_dim].dot(MC_v3d(nabla_nx[k],nabla_ny[k],nabla_nz[k]));


  nabla_nx=gradient_normalized.component(0);
  nabla_ny=gradient_normalized.component(1);
  nabla_nz=gradient_normalized.component(2);


  //voir calculs
  std::vector <MC_matrix> nabla_R(6);
  for(int k=0;k<6;k++)
    {
      nabla_R[k].resize(3,3);

      
      nabla_R[k](0,0) = nabla_c[k]+2*n[0]*(1-cos_t)*nabla_nx[k]-n[0]*n[0]*nabla_c[k];
      nabla_R[k](1,0) = sin_t*nabla_nz[k]+n[2]*nabla_s[k]+n[1]*(1-cos_t)*nabla_nx[k]+n[0]*(1-cos_t)*nabla_ny[k]-n[0]*n[1]*nabla_c[k];
      nabla_R[k](2,0) = -sin_t*nabla_ny[k]-n[1]*nabla_s[k]+n[2]*(1-cos_t)*nabla_nx[k]+n[0]*(1-cos_t)*nabla_nz[k]-n[0]*n[2]*nabla_c[k];


      nabla_R[k](0,1) = nabla_nx[k]*n[1]*(1-cos_t)+n[0]*(1-cos_t)*nabla_ny[k]-n[0]*n[1]*nabla_c[k]-sin_t*nabla_nz[k]-n[2]*nabla_s[k];
      nabla_R[k](1,1) = nabla_c[k]+2*n[1]*(1-cos_t)*nabla_ny[k]-n[1]*n[1]*nabla_c[k];
      nabla_R[k](2,1) = sin_t*nabla_nx[k]+n[0]*nabla_s[k]+n[2]*(1-cos_t)*nabla_ny[k]+n[1]*(1-cos_t)*nabla_nz[k]-n[1]*n[2]*nabla_c[k];

      nabla_R[k](0,2) = sin_t*nabla_ny[k]+n[1]*nabla_s[k]+n[2]*(1-cos_t)*nabla_nx[k]+n[0]*(1-cos_t)*nabla_nz[k]-n[0]*n[2]*nabla_c[k];
      nabla_R[k](1,2) = -sin_t*nabla_nx[k]-n[0]*nabla_s[k]+n[2]*(1-cos_t)*nabla_ny[k]+n[1]*(1-cos_t)*nabla_nz[k]-n[1]*n[2]*nabla_c[k];
      nabla_R[k](2,2) = nabla_c[k]+2*n[2]*(1-cos_t)*nabla_nz[k]-n[2]*n[2]*nabla_c[k];
    

    }

  return std::pair <MC_matrix,std::vector <MC_matrix> > (R,nabla_R);
}

void MC_matrix::internal_product(MC_v3d* to_multiply) const
{
    if(to_multiply)
    {
        double x0=(*to_multiply)[0];
        double x1=(*to_multiply)[1];
        double x2=(*to_multiply)[2];

        if(size_1()==3 && size_2()==3)
        {

            (*to_multiply)[0]= M[0]*x0+M[3]*x1+M[6]*x2;
            (*to_multiply)[1]= M[1]*x0+M[4]*x1+M[7]*x2;
            (*to_multiply)[2]= M[2]*x0+M[5]*x1+M[8]*x2;
            return ;

        }
        else if(size_1()==4 && size_2()==4) //add the 1 automatically
        {
            (*to_multiply)[0]= M[0]*x0+M[4]*x1+M[8]*x2+M[12];
            (*to_multiply)[1]= M[1]*x0+M[5]*x1+M[9]*x2+M[13];
            (*to_multiply)[2]= M[2]*x0+M[6]*x1+M[10]*x2+M[14];
        }
        else
        {std::cout<<"Error in MC_matrix::internal_product(MC_v3d* "<<*to_multiply<<"), size of the matrix is "<<size()<<std::endl;exit(-1);}

    }
    else
    {std::cout<<"Error in MC_matrix::internal_product(MC_v3d* ), the pointer is null"<<std::endl;exit(-1);}
}

void MC_matrix::internal_product(MC_v4d* to_multiply) const
{
    if(to_multiply)
    {
        double x0=(*to_multiply)[0];
        double x1=(*to_multiply)[1];
        double x2=(*to_multiply)[2];
        double x3=(*to_multiply)[3];

        if(size_1()==4 && size_2()==4)
        {
            (*to_multiply)[0]= M[0]*x0+M[4]*x1+M[8]*x2+M[12]*x3;
            (*to_multiply)[1]= M[1]*x0+M[5]*x1+M[9]*x2+M[13]*x3;
            (*to_multiply)[2]= M[2]*x0+M[6]*x1+M[10]*x2+M[14]*x3;
            (*to_multiply)[3]= M[3]*x0+M[7]*x1+M[11]*x2+M[15]*x3;
        }
        else
        {std::cout<<"Error in MC_matrix::internal_product(MC_v4d* "<<*to_multiply<<"), size of the matrix is "<<size()<<std::endl;exit(-1);}

    }
    else
    {std::cout<<"Error in MC_matrix::internal_product(MC_v4d* ), the pointer is null"<<std::endl;exit(-1);}
}


MC_v3d MC_matrix::translation_part() const
{
    if(size_1()!=4 || size_2()!=4)
    {std::cout<<"Error in MC_matrix::translation_part(), size of the matrix is not 4x4 but "<<size()<<std::endl;exit(-1);}

    return MC_v3d((*this)(0,3),(*this)(1,3),(*this)(2,3));
}

MC_matrix& MC_matrix::add_translation(const MC_v3d& tr)
{
    if(size_1()!=4 || size_2()!=4)
    {std::cout<<"Error in MC_matrix::add_translation("<<tr<<"), size of the matrix should be 4x4 and not "<<size()<<std::endl;exit(-1);}
    M[12]+=tr[0];M[13]+=tr[1];M[14]+=tr[2];
    return *this;
}


MC_matrix MC_matrix::to_matrix4() const
{
    if(size_1()==4 && size_2()==4)
        return *this;
    else if(size_1()==3 && size_2()==3)
    {
        MC_matrix m4=MC_matrix::identity(4);
        m4.set_block(0,2,0,2,*this);
        return m4;
    }
    else
    {std::cout<<"Error in MC_matrix::to_matrix4(), size must be 3x3 and not "<<size()<<std::endl;exit(-1);}

}
std::pair <MC_matrix,MC_v3d> MC_matrix::to_matrix3() const
{
    if(size_1()==3 && size_2()==3)
        return std::pair <MC_matrix,MC_v3d> (*this,MC_v3d(0,0,0));
    else if(size_1()==4 && size_2()==4)
        return std::pair <MC_matrix,MC_v3d> ((*this)(MC_int_vector(0,1,2),MC_int_vector(0,1,2)),(*this)(MC_int_vector(0,1,2),3));
    else
    {std::cout<<"Error in MC_matrix::to_matrix3(), size must be 4x4 and not "<<size()<<std::endl;exit(-1);}
}
const double* MC_matrix::pointer() const
{return M.pointer();}

MC_matrix& MC_matrix::operator*=(const MC_matrix& M1)
{
    *this=(*this)*M1;
    return *this;
}

MC_matrix& MC_matrix::set_translation(const MC_v3d& tr)
{
    if(size_1()!=4 || size_2()!=4)
    {std::cout<<"Error in MC_matrix::set_translation("<<tr<<"), size of the matrix should be 4x4 and not "<<size()<<std::endl;exit(-1);}
    M[12]=tr[0];M[13]=tr[1];M[14]=tr[2];
    return *this;
}
MC_matrix& MC_matrix::set_rotation(const MC_matrix& m)
{
    if( !((size_1()==4 && size_2()==4) || (size_1()==3 && size_2()==3)) )
    {std::cout<<"Error in MC_matrix::set_rotation("<<m<<"), size of the matrix should be 4x4 or 3x3 and not "<<size()<<std::endl;exit(-1);}
    for(int k1=0;k1<3;++k1)
        for(int k2=0;k2<3;++k2)
            M[k1+4*k2]=m(k1,k2);
    return *this;
}

MC_double_vector operator*(const MC_matrix& M1,const MC_double_vector& v)
{
    if(M1.size_2()!=v.size())
    {std::cout<<"Error in MC_matrix::operator*(const MC_matrix& M1,const MC_double_vector& v), size are not compatible : ["<<M1.size()<<"]x"<<v.size()<<std::endl;exit(-1);}

    MC_matrix m;m.add_col(v);
    return (M1*m).to_vec();
}
const MC_double_vector& MC_matrix::to_vec () const
{return M;}

MC_double_vector MC_matrix::get_col(const int& k_index) const
{
    if(k_index<0 || k_index>=size_2())
    {std::cout<<"Error in MC_matrix::get_col("<<k_index<<"), size_2="<<size_2()<<std::endl;exit(-1);}

    return (*this)(MC_int_vector::linspace(0,size_1()-1),k_index).to_vec();
}
MC_double_vector MC_matrix::get_row(const int& k_index) const
{
    if(k_index<0 || k_index>=size_1())
    {std::cout<<"Error in MC_matrix::get_row("<<k_index<<"), size_1="<<size_1()<<std::endl;exit(-1);}

    return (*this)(k_index,MC_int_vector::linspace(0,size_2()-1)).to_vec();
}





MC_matrix MC_matrix::transformation(const std::string& input)
{
    std::vector<std::string> v_input=MC_string_converter::delete_empty(MC_string_tokenizer::tokenize(input,"<>,;: "));
    int counter=0;
    int N=v_input.size();
    //for(int k=0;k<N;++k)
    //    std::cout<<" -- "<<v_input[k]<<std::endl;

    while(counter<N)
    {
        if(v_input[counter]=="rotation")
        {
            if(counter+4<N)
            {
                //read axis and angle
                double x=MC_string_converter::value_of<double>(v_input[counter+1]);
                double y=MC_string_converter::value_of<double>(v_input[counter+2]);
                double z=MC_string_converter::value_of<double>(v_input[counter+3]);
                double theta=MC_string_converter::value_of<double>(v_input[counter+4]);

                return MC_matrix::rotation(MC_v3d(x,y,z),theta);
            }
        }
        if(v_input[counter]=="translation")
        {
            if(counter+3<N)
            {
                double x=MC_string_converter::value_of<double>(v_input[counter+1]);
                double y=MC_string_converter::value_of<double>(v_input[counter+2]);
                double z=MC_string_converter::value_of<double>(v_input[counter+3]);


                return MC_matrix::translation(MC_v3d(x,y,z));
            }
        }
        if(v_input[counter]=="scaling")
        {
            if(counter+3<N)
            {
                bool is_ok_1=false,is_ok_2=false,is_ok_3=false;
                double x=MC_string_converter::value_of<double>(v_input[counter+1],&is_ok_1);
                double y=MC_string_converter::value_of<double>(v_input[counter+2],&is_ok_2);
                double z=MC_string_converter::value_of<double>(v_input[counter+3],&is_ok_3);

                if(is_ok_1==true && is_ok_2==true && is_ok_3==true)
                    return MC_matrix::scaling(x,y,z);
                else if(is_ok_1==true && (is_ok_2==false || is_ok_3==false) )
                {
                    return MC_matrix::scaling(x);
                }

            }
            else if (counter+1<N)
            {
                double s=MC_string_converter::value_of<double>(v_input[counter+1]);

                return MC_matrix::scaling(s);
            }
        }

        ++counter;
    }
    return MC_matrix::identity(4);
}

MC_matrix MC_matrix::translation(const MC_v3d& tr)
{
    MC_matrix M=MC_matrix::identity(4);
    M.set_translation(tr);
    return M;
}

MC_matrix MC_matrix::scaling(const double& s)
{
    return MC_matrix::scaling(s,s,s);
}

MC_matrix MC_matrix::scaling(const double& sx,const double& sy,const double& sz)
{
    MC_matrix M(3);
    M(0,0)=sx;M(1,1)=sy;M(2,2)=sz;
    return M;
}

std::string MC_matrix::to_string() const
{
    std::stringstream stream;stream<<*this;
    return stream.str();
}

MC_matrix MC_matrix::kronecker(const MC_double_vector& v1,const MC_double_vector& v2)
{
    int N=v1.size();
    if(v2.size()!=N)
    {std::cout<<"Error in MC_matrix::kronecker(v1,v2), size are not consistent"<<std::endl;exit(-1);}

    MC_matrix K=MC_matrix::zeros(N);
    for(int k1=0;k1<N;++k1)
        for(int k2=0;k2<N;++k2)
            K(k1,k2) = v1[k1]*v2[k2];
    return K;

}

MC_matrix::MC_matrix(const MC_double_vector& column)
{
    *this=MC_matrix(column.size(),1);
    set_col(0,column);
}


}