Matrix.cpp

Go to the documentation of this file.

#include "../header/Matrix.h"




Matrix::Matrix()
{
  set_identity();
}

Matrix::~Matrix()
{
}
Matrix::Matrix(const Matrix& _M)
{
  for(int k_x=0;k_x<4;k_x++)
    for(int k_y=0;k_y<4;k_y++)
      this->set_value(_M.M[k_x+4*k_y],k_x,k_y);
}

Matrix Matrix::multiply(Matrix M2)
{

  double temp_M[16];

  int k_x=0,k_y=0,k_m=0;
  double temp=0;

  //all index
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      {
        //multiply
        for(k_m=0,temp=0;k_m<4;k_m++)
          {
            temp += M[k_m+4*k_y] * (M2.value(k_x,k_m));
          }
        temp_M[k_x+4*k_y] = temp;
      }

  //set new matrix
  for(int k=0;k<16;k++)
    M[k]=temp_M[k];

  return *this;
}

double *Matrix::multiply(double *x)
{
  double x_homogeneous[4]={x[0],x[1],x[2],1};
  double temp[4]={0,0,0,0};
  int k_x=0,k_y=0;
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      temp[k_y] += M[k_x+4*k_y]*x_homogeneous[k_x];
  

  //reduce to 3D
  double *output = new double[3];
  for(int k=0;k<3;k++)
    output[k]=temp[k];
  return output;

}

int Matrix::multiply_internally(double *x)//do x=A.x
{
  int ok=0;

  double temp[4]={0,0,0,0};
  double x_homogeneous[4]={x[0],x[1],x[2],1};
  int k_x=0,k_y=0;

  for(k_y=0;k_y<4;k_y++)
    for(k_x=0;k_x<4;k_x++)
      temp[k_y] += M[k_x+4*k_y]*x_homogeneous[k_x];
  for(int k=0;k<3;k++)
    x[k]=temp[k];

  return ok;
}


int Matrix::set_identity()
{
  for(int k_x=0;k_x<4;k_x++)
    for(int k_y=0;k_y<4;k_y++)
      {
        if(k_x==k_y)
          M[k_x+4*k_y]=1;
        else
          M[k_x+4*k_y]=0;
      }
  
  return 0;
}

int Matrix::set_zero()
{
  for(int k=0;k<16;k++)
    M[k]=0;
  return 0;
}


int Matrix::set_translation(double *t)
{
  M[3]  = t[0];
  M[7]  = t[1];
  M[11] = t[2];

  return 0;
}

int Matrix::set_translation(double tx,double ty,double tz)
{
  double t[3]={tx,ty,tz};
  return set_translation(t);
}

ostream& operator << (ostream& flux, Matrix M)
{
  for(int k=0;k<4;k++)
    flux<<"("<<M.value(0,k)<<","<<M.value(1,k)<<","<<M.value(2,k)<<","<<M.value(3,k)<<")\n";
  return flux;
}


double Matrix::value(int k_x,int k_y) const
{
  if(k_x<0 || k_x>=4 || k_y<0 || k_y>=4)
    {printf("Error (%d,%d) is not correct in v(k_x,k_y) in Matrix\n",k_x,k_y);exit(-1);} 
  return M[k_x+4*k_y];
}

double& Matrix::value(int k_x,int k_y)
{
  if(k_x<0 || k_x>=4 || k_y<0 || k_y>=4)
    {printf("Error (%d,%d) is not correct in v(k_x,k_y) in Matrix\n",k_x,k_y);exit(-1);} 
  return M[k_x+4*k_y];
}
double Matrix::v(int k_x,int k_y) const
{
  if(k_x<0 || k_x>=4 || k_y<0 || k_y>=4)
    {printf("Error (%d,%d) is not correct in v(k_x,k_y) in Matrix\n",k_x,k_y);exit(-1);} 
  return M[k_x+4*k_y];
}

double& Matrix::v(int k_x,int k_y)
{
  if(k_x<0 || k_x>=4 || k_y<0 || k_y>=4)
    {printf("Error (%d,%d) is not correct in v(k_x,k_y) in Matrix\n",k_x,k_y);exit(-1);} 
  return M[k_x+4*k_y];
}

double& Matrix::operator [](int k_dim)
{
  if(k_dim<0 || k_dim>9)
    {printf("Error k_dim too large [%d] in operator [] in matrix\n",k_dim);exit(-1);}
  return M[k_dim];
}
double Matrix::operator [](int k_dim) const
{
  if(k_dim<0 || k_dim>=4)
    {printf("Error k_dim too large [%d] in operator [] in matrix\n",k_dim);exit(-1);}
  return M[k_dim];
}



Matrix& Matrix::operator/=(const double& alpha)
{
  for(int k_x=0;k_x<4;k_x++)
    for(int k_y=0;k_y<4;k_y++)
      this->set_value(M[k_x+4*k_y]/alpha,k_x,k_y);

  return *this;
}




Matrix& Matrix::operator=(const Matrix& _M)
{
  for(int k_x=0;k_x<4;k_x++)
    for(int k_y=0;k_y<4;k_y++)
      this->set_value(_M.M[k_x+4*k_y],k_x,k_y);

  return *this;
}

Matrix& Matrix::operator+=(const Matrix& _M)
{
  for(int k_x=0;k_x<4;k_x++)
    for(int k_y=0;k_y<4;k_y++)
      this->set_value(M[k_x+4*k_y]+_M.M[k_x+4*k_y],k_x,k_y);

  return *this;
}

int Matrix::set_value(double val,int k_x,int k_y)
{
  M[k_x+4*k_y] = val;
  return 0;
}

int Matrix::set_value(double x00,double x01,double x02,double x03,double x10,double x11,double x12, double x13,double x20,double x21,double x22,double x23,double x30,double x31,double x32,double x33)
{
  M[0] = x00; M[4] = x10; M[8] = x20; M[12] = x30;
  M[1] = x01; M[5] = x11; M[9] = x21; M[13] = x31;
  M[2] = x02; M[6] = x12; M[10]= x22; M[14] = x32;
  M[3] = x03; M[7] = x13; M[11]= x23; M[15] = x33;

  return 0;
}

int Matrix::set_euler_angle(double theta_1,double theta_2,double theta_3)
{
  double theta[3]={theta_1,theta_2,theta_3};
  return set_euler_angle(theta);
}

int Matrix::set_euler_angle(double *r)
{
  double cos_theta = cos(r[0]);
  double sin_theta = sin(r[0]);

  double cos_phi   = cos(r[1]);
  double sin_phi   = sin(r[1]);

  double cos_psi   = cos(r[2]);
  double sin_psi   = sin(r[2]);


  Matrix RX;
  RX.set_value( cos_theta,0,0);
  RX.set_value( cos_theta,1,1);
  RX.set_value( sin_theta,1,0);
  RX.set_value(-sin_theta,0,1);

  Matrix RY;
  RY.set_value( cos_phi,1,1);
  RY.set_value( cos_phi,2,2);
  RY.set_value( sin_phi,2,1);
  RY.set_value(-sin_phi,1,2);

  Matrix RZ;
  RZ.set_value( cos_psi,0,0);
  RZ.set_value( cos_psi,1,1);
  RZ.set_value( sin_psi,1,0);
  RZ.set_value(-sin_psi,0,1);


  multiply(RX);
  multiply(RY);
  multiply(RZ);
  

  return 0;
}

int Matrix::get_position(double *p)
{
  p[0]=M[3];
  p[1]=M[7];
  p[2]=M[11];

  return 0;
}

Matrix operator*(const Matrix& M1,const Matrix& M2)
{
  double temp_M[16];

  int k_x=0,k_y=0,k_m=0;
  double temp=0;

  //all index
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      {
        //multiply
        for(k_m=0,temp=0;k_m<4;k_m++)
          {
            temp += M1.M[k_m+4*k_y] * (M2.M[k_x+4*k_m]);
          }
        temp_M[k_x+4*k_y] = temp;
      }

  Matrix Z;
  //set new matrix
  for(int k=0;k<16;k++)
    Z.set_value(temp_M[k],k,0);

  return Z;
}

Matrix operator*(const Matrix& M1,const double& alpha)
{
  Matrix Z;
  int k_x=0,k_y=0;
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      Z.set_value(alpha*M1.M[k_x+4*k_y],k_x,k_y);
  return Z;
}


Matrix operator*(const double& alpha,const Matrix& M1)
{
  Matrix Z;
  int k_x=0,k_y=0;
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      Z.set_value(alpha*M1.M[k_x+4*k_y],k_x,k_y);
  return Z;
}

V_3D operator*(const Matrix& _M,const V_3D& _V)
{
  V_3D result;
  V_3D copy = _V;
  int k_x=0;
  for(k_x=0;k_x<3;k_x++)
    result.set(k_x,_M.M[0+4*k_x]*copy.get(0)+_M.M[1+4*k_x]*copy.get(1)+_M.M[2+4*k_x]*copy.get(2)+_M.M[3+4*k_x]);
  
  return result;
}


V_3D operator*(const V_3D& _V,const Matrix& _M)
{
  V_3D result;
  int k_y=0;
  for(k_y=0;k_y<3;k_y++)
    result.set(k_y,_V[0]*_M.M[k_y+4*0]+_V[1]*_M.M[k_y+4*1]+_V[2]*_M.M[k_y+4*2]);

  return result;
}

Matrix operator+(const Matrix& M1,const Matrix& M2)
{
  Matrix Z;
  
  int k_x=0,k_y=0;
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      {
        Z.set_value(M1.M[k_x+4*k_y]+M2.M[k_x+4*k_y],k_x,k_y);
      }

  return Z;
}


Matrix operator-(const Matrix& M1,const Matrix& M2)
{
  Matrix Z;
  
  int k_x=0,k_y=0;
  for(k_x=0;k_x<4;k_x++)
    for(k_y=0;k_y<4;k_y++)
      {
        Z.set_value(M1.M[k_x+4*k_y]-M2.M[k_x+4*k_y],k_x,k_y);
      }

  return Z;
}

int Matrix::add_translation(double *t)
{
  for(int k_dim=0;k_dim<3; M[3+4*k_dim]+=t[k_dim],k_dim++);
  return 0;
}

int Matrix::add_translation(double tx,double ty,double tz)
{
  double t[3]={tx,ty,tz};
  return add_translation(t);
}

//theta in radians
int Matrix::set_vector_rotation(double *_v,double theta)
{
  int ok=0;

  double cos_theta = cos(theta);
  double sin_theta = sin(theta);
  
  //normalize v
  int k_dim=0;
  double n=0;
  for(k_dim=0;k_dim<3;n+=_v[k_dim]*_v[k_dim],k_dim++);
  n=powf(n,0.5);
  for(k_dim=0;k_dim<3;_v[k_dim]/=n,k_dim++);  

  M[0] = cos_theta+(1-cos_theta)*_v[0]*_v[0];
  M[1] = (1-cos_theta)*_v[0]*_v[1]-sin_theta*_v[2];
  M[2] = (1-cos_theta)*_v[0]*_v[2]+sin_theta*_v[1];

  M[4] = (1-cos_theta)*_v[1]*_v[0]+sin_theta*_v[2];
  M[5] = cos_theta + (1-cos_theta)*_v[1]*_v[1];
  M[6] = (1-cos_theta)*_v[1]*_v[2]-sin_theta*_v[0];
  
  M[8] = (1-cos_theta)*_v[2]*_v[0]-sin_theta*_v[1];
  M[9] = (1-cos_theta)*_v[2]*_v[1]+sin_theta*_v[0];
  M[10] = cos_theta+(1-cos_theta)*_v[2]*_v[2];

  return ok;
}


int Matrix::multiply_vector_rotation(double *_v,double theta)
{
  int ok=0;

  Matrix R;
  R.set_vector_rotation(_v,theta);
  multiply(R);

  return ok;
}

int Matrix::scale(double *s)
{
  int k=0,k_dim=0;
  for(k=0;k<4;k++)
    for(k_dim=0;k_dim<3;k_dim++)
      M[k+4*k_dim] *= s[k_dim];
  return 0;
}


int Matrix::set_vector_rotation(double v_x,double v_y,double v_z,double theta)
{
  double _v[3]={v_x,v_y,v_z};
  return set_vector_rotation(_v,theta);
}

Matrix Matrix::get_rotation_part()
{
  Matrix R;

  int k_1=0,k_2=0;
  for(k_1=0;k_1<3;k_1++)
    for(k_2=0;k_2<3;k_2++)
      R.set_value(M[k_1+4*k_2],k_1,k_2);

  return R;
}

int Matrix::multiply_rotation_internally(double *n)
{
  int ok=0;

  double temp[4]={0,0,0,0};
  double n_homogeneous[4]={n[0],n[1],n[2],1};
  int k_x=0,k_y=0;

  for(k_x=0;k_x<3;k_x++)
    for(k_y=0;k_y<3;k_y++)
      temp[k_y] += M[k_x+4*k_y]*n_homogeneous[k_x];
  for(int k=0;k<3;k++)
    n[k]=temp[k];

  return ok;
}

V_3D Matrix::get_position()
{
  V_3D position;
  position.set(M[3+4*0],M[3+4*1],M[3+4*2]);
  return position;
}

int Matrix::set_rotation_matrix(double *axis,double angle)
{
  int k_dim=0;
  //normalization
  double norm=0.0;
  for(k_dim=0;k_dim<3;k_dim++)
    norm += axis[k_dim]*axis[k_dim];
  norm = powf(norm,0.5);
  for(k_dim=0;k_dim<3;k_dim++)
    axis[k_dim] /= norm;

  //http://fr.wikipedia.org/wiki/Rotation_vectorielle

  double cos_phi = cos(angle);
  double sin_phi = sin(angle);
  M[0] = cos_phi + (1-cos_phi)*axis[0]*axis[0];
  M[1] = (1-cos_phi)*axis[0]*axis[1]-sin_phi*axis[2];
  M[2] = (1-cos_phi)*axis[0]*axis[2]+sin_phi*axis[1];
  M[3] = 0.0;
  M[4] = (1-cos_phi)*axis[0]*axis[1]+sin_phi*axis[2];
  M[5] = cos_phi + (1-cos_phi)*axis[1]*axis[1];
  M[6] = (1-cos_phi)*axis[1]*axis[2]-sin_phi*axis[0];
  M[7] = 0.0;
  M[8] = (1-cos_phi)*axis[0]*axis[2]-sin_phi*axis[1];
  M[9] = (1-cos_phi)*axis[1]*axis[2]+sin_phi*axis[0];
  M[10]= cos_phi+(1-cos_phi)*axis[2]*axis[2];
  M[11]= 0.0;

//   Matrix Id;
//   Matrix vectorial;
//   vectorial.set_zero();
//   vectorial.set_value(-axis[2],1,0);
//   vectorial.set_value( axis[1],2,0);
//   vectorial.set_value( axis[2],0,1);
//   vectorial.set_value(-axis[1],2,0);
//   vectorial.set_value( axis[0],1,2);
//   vectorial.set_value(-axis[0],2,1);

//   Matrix full;
//   full.set_zero();
//   full.set_value(

  return 0;

}

int Matrix::set_rotation_matrix(double x_axis,double y_axis,double z_axis,double angle)
{
  double axis[3]={x_axis,y_axis,z_axis};
  return set_rotation_matrix(axis,angle);
}


Matrix Matrix::multiply_rotation(double *axis,double angle)
{
  Matrix R;
  R.set_rotation_matrix(axis,angle);
  *this = *this*R;
  return *this;
}

Matrix Matrix::multiply_rotation(double x_axis,double y_axis,double z_axis,double angle)
{
  Matrix R;
  R.set_rotation_matrix(x_axis,y_axis,z_axis,angle);
  *this = *this*R;
  return *this;
}


int operator==(const Matrix& M1,const Matrix& M2)
{
  double epsilon=0.000001;
  int k=0;
  for(k=0;k<16;k++)
    if(fabs(M1.M[k]-M2.M[k])>epsilon)
      return 0;
  return 1;
}

int operator!=(const Matrix& M1,const Matrix& M2)
{
  return 1-(M1==M2);
}


int Matrix::set_rotation_part(Matrix R)
{
  int k1=0,k2=0;
  for(k1=0;k1<3;k1++)
    for(k2=0;k2<3;k2++)
      M[4*k1+k2] = R.value(k1,k2);
  return 0;
}

int Matrix::set_translation_part(Matrix T)
{
  int k1=0;
  for(k1=0;k1<3;k1++)
    M[4*k1+3] = T.value(k1,3);
  return 0;
}

double Matrix::det() const
{
  return 
    v(0,0)*
    (
     +v(1,1)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
     -v(1,2)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
     +v(1,3)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
     )
    -
    v(0,1)*
    (
     +v(1,0)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
     -v(1,2)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
     +v(1,3)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
     )
    +v(0,2)*
    (
     +v(1,0)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
     -v(1,1)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
     +v(1,3)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
     )
    -v(0,3)*
    (
     +v(1,0)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
     -v(1,1)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
     +v(1,2)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
     )
    ;
    
}

Matrix Matrix::invert() const
{
  Matrix A;
  double d=det();

  A.set_value(+v(1,1)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
              -v(1,2)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
              +v(1,3)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
              ,0,0);

  A.set_value(-v(0,1)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
              +v(0,2)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
              -v(0,3)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
              ,0,1);

  A.set_value(+v(0,1)*(v(1,2)*v(3,3)-v(1,3)*v(3,2))
              -v(0,2)*(v(1,1)*v(3,3)-v(1,3)*v(3,1))
              +v(0,3)*(v(1,1)*v(3,2)-v(1,2)*v(3,1))
              ,0,2);

  A.set_value(-v(0,1)*(v(1,2)*v(2,3)-v(1,3)*v(2,2))
              +v(0,2)*(v(1,1)*v(2,3)-v(1,3)*v(2,1))
              -v(0,3)*(v(1,1)*v(2,2)-v(1,2)*v(2,1))
              ,0,3);


  
  A.set_value(-v(1,0)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
              +v(1,2)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
              -v(1,3)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
              ,1,0);

  A.set_value(+v(0,0)*(v(2,2)*v(3,3)-v(2,3)*v(3,2))
              -v(0,2)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
              +v(0,3)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
              ,1,1);

  A.set_value(-v(0,0)*(v(1,2)*v(3,3)-v(1,3)*v(3,2))
              +v(0,2)*(v(1,0)*v(3,3)-v(1,3)*v(3,0))
              -v(0,3)*(v(1,0)*v(3,2)-v(1,2)*v(3,0))
              ,1,2);

  A.set_value(+v(0,0)*(v(1,2)*v(2,3)-v(1,3)*v(2,2))
              -v(0,2)*(v(1,0)*v(2,3)-v(1,3)*v(2,0))
              +v(0,3)*(v(1,0)*v(2,2)-v(1,2)*v(2,0))
              ,1,3);




  A.set_value(+v(1,0)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
              -v(1,1)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
              +v(1,3)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
              ,2,0);

  A.set_value(-v(0,0)*(v(2,1)*v(3,3)-v(2,3)*v(3,1))
              +v(0,1)*(v(2,0)*v(3,3)-v(2,3)*v(3,0))
              -v(0,3)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
              ,2,1);

  A.set_value(+v(0,0)*(v(1,1)*v(3,3)-v(1,3)*v(3,1))
              -v(0,1)*(v(1,0)*v(3,3)-v(1,3)*v(3,0))
              +v(0,3)*(v(1,0)*v(3,1)-v(1,1)*v(3,0))
              ,2,2);

  A.set_value(-v(0,0)*(v(1,1)*v(2,3)-v(1,3)*v(2,1))
              +v(0,1)*(v(1,0)*v(2,3)-v(1,3)*v(2,0))
              -v(0,3)*(v(1,0)*v(2,1)-v(1,1)*v(2,0))
              ,2,3);





  A.set_value(-v(1,0)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
              +v(1,1)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
              -v(1,2)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
              ,3,0);

  A.set_value(+v(0,0)*(v(2,1)*v(3,2)-v(2,2)*v(3,1))
              -v(0,1)*(v(2,0)*v(3,2)-v(2,2)*v(3,0))
              +v(0,2)*(v(2,0)*v(3,1)-v(2,1)*v(3,0))
              ,3,1);

  A.set_value(-v(0,0)*(v(1,1)*v(3,2)-v(1,2)*v(3,1))
              +v(0,1)*(v(1,0)*v(3,2)-v(1,2)*v(3,0))
              -v(0,2)*(v(1,0)*v(3,1)-v(1,1)*v(3,0))
              ,3,2);

  A.set_value(+v(0,0)*(v(1,1)*v(2,2)-v(1,2)*v(2,1))
              -v(0,1)*(v(1,0)*v(2,2)-v(1,2)*v(2,0))
              +v(0,2)*(v(1,0)*v(2,1)-v(1,1)*v(2,0))
              ,3,3);

  A/=d;

  return A;
}


int Matrix::build_rotation_matrix_to_vector(const V_3D u,const V_3D w)
{
  V_3D v0,v1;
  v0 = u.normalized();
  v1 = w.normalized();

  V_3D n = (v0.vector_prod(v1)).normalized();
  double cos_t=v0.dot(v1);
  double sin_t=powf(1.0-cos_t*cos_t,0.5);

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

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

  (*this)(0,2)=n[1]*sin_t+n[0]*n[2]*(1-cos_t);
  (*this)(1,2)=-n[0]*sin_t+n[1]*n[2]*(1-cos_t);
  (*this)(2,2)=cos_t+n[2]*n[2]*(1-cos_t);
  (*this)(3,2)=0.0+90;
                
  (*this)(0,3)=0.0;
  (*this)(1,3)=0.0;
  (*this)(2,3)=0.0;
  (*this)(3,3)=1.0;

  return 0;
}

double Matrix::operator ()(int k_1,int k_2) const
{return value(k_2,k_1);}

double& Matrix::operator()(int k_1,int k_2)
{return value(k_2,k_1);}