algebra.c

Go to the documentation of this file.

#include "algebra.h"

#include <math.h>
#include <string.h>

/* A generic header that must be re-implemented for each linear algebra package.  */

unsigned int AnalyzeKernel(unsigned int nr,unsigned int nc,double *mT,
                           unsigned int dof,double epsilon,
                           boolean computeRank,boolean checkRank,boolean getT,boolean getBase,
                           boolean *singular,unsigned int *rank,boolean **IR,double **T);


/* BEGIN GSL IMPLEMENTATION */

#if _GSL

void PrintGSLVector(FILE *f,char *label,gsl_vector *v);

void PrintGSLPermutation(FILE *f,char *label,gsl_permutation *p);

void PrintGSLMatrix(FILE *f,char *label,gsl_matrix *M);


/******************************************************************/
/*               Private function definitions                     */

unsigned int AnalyzeKernel(unsigned int nr,unsigned int nc,double *mT,
                           unsigned int dof,double epsilon,
                           boolean computeRank,boolean checkRank,boolean getT,boolean getBase,
                           boolean *singular,unsigned int *rank,boolean **IR,double **T)
{
  unsigned int err;

  #if (_DEBUG<2)
    gsl_set_error_handler_off();
  #endif

  if (dof==nc)
    {
      /* Trivial case of a system without equations. We take this into account for completeness, but
         this function should never be used in this case. */
      *singular=FALSE;
      *rank=0;

      if ((getT)&&(nc>0))
        {
          gsl_matrix_view vT;

          NEW(*T,nc*dof,double);
          vT=gsl_matrix_view_array(*T,nc,dof);
          gsl_matrix_set_identity (&(vT.matrix));
        }
      else
        *T=NULL;

      if ((getBase)&&(nr>0))
        {
          unsigned int i;

          NEW(*IR,nr,boolean);
          for(i=0;i<nr;i++)
            (*IR)[i]=FALSE;
        }
      else
        *IR=NULL;

      err=0;
    }
  else
    {
      gsl_matrix_view At;
      gsl_vector *tau;  
      gsl_permutation *p;
      int signum;
      gsl_vector *norm;

      /* Define A^T */
      At=gsl_matrix_view_array(mT,nc,nr);

      /* Extra space for the QR decomposition of At */      
      tau=gsl_vector_alloc((nr<nc?nr:nc));
      p=gsl_permutation_alloc(nr);
      norm=gsl_vector_alloc(nr);
      
      /* Decompose the matrix, with column pivoting to get the kernel. Note that
         the permutation is not relevant for the kernel P*A*v=0 -> A*v=0  */
      err=gsl_linalg_QRPT_decomp(&(At.matrix),tau,p,&signum,norm);

      if (err)
        {
          *singular=TRUE;
          *rank=NO_UINT;
          *T=NULL;
          *IR=NULL;
          err=3; /* Our error code for decomposition error */
        }
      else
        {
          unsigned int k,mrc;
          double r;

          /***********************************************/
          /* At is nc x nr */
          /* Q is nc x nc */
          /* R is nc x nr -> R^t is nr x nc */

          /* At=Q R P^t -> A= P R^t Q^t -> P^t A Q = R^t 
             Thus, the rows of R (columns of R^t) that are zero
             indicate columns of Q that define part of the kernel.
             Moreover, the norm of teh rows of R indicate how far
             is from defining a new kernel basis vector. Sinc R
             is triangular the norm of the row is the norm from
             the diagonal to the end of the row (nr entries) */

          mrc=(nr<nc?nr:nc);

          if (computeRank)
            {
              /* Rank = number of not-null rows of R */
              /* The rank can not be larger than the mrc=min(nr,nc) */
              *rank=0;
              for(k=0;k<mrc;k++) 
                { 
                  /* Get the norm of row k of R from the diagonal to the end of the row */
                  r=NormWithStride(nr-k,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(k,k,nc,nr)]));
                  if (r>epsilon)
                    (*rank)++;
                }
              if (dof==0)
                {
                  *singular=FALSE;
                  /* temporary set 'dof' form '*rank' */
                  dof=nc-*rank;
                }
              else
                *singular=((nc-dof)!=*rank);
            }
          else
            {
              if (dof==0)
                Error("Null 'dof' parameter in AnalyzeKernel");
              if (dof>nc)
                Error("We can not have more 'dof' than variables");

              *rank=nc-dof;

              /* Check if row *rank-1 of R is actually not null. */
              r=NormWithStride(nr-(*rank-1),(ROW_MAJOR?1:nc),&(mT[RC2INDEX(*rank-1,*rank-1,nc,nr)]));
              *singular=(r<epsilon);
      
              if (checkRank)
                {
                  if (*singular)
                    err=1; //Error("Singular point (increase N_DOF? Decrease EPSILON?)");
              
                  /* Checking row '*rank' is actually null. */
                  r=NormWithStride(nr-*rank,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(*rank,*rank,nc,nr)]));
                  if (r>epsilon)
                    err=2; //Error("Non-null kernel vector (decrease N_DOF? Increase EPSILON?)");
                }
            }

          #if (_DEBUG>0)
            if ((dof>0)&&(!computeRank)&&(getT)&&(!getBase))
              {
                printf("  E=[ ");
                for(k=*rank;k<mrc;k++) 
                  {
                    r=NormWithStride(nr-k,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(k,k,nc,nr)]));
                    printf("%.16f ",r);
                  }
                printf("];\n");
              }
          #endif

          /* If requested, we define T and IR, even if the chart is singular. In this case
             we return the most likely T and IR. */
          if (getT)
            {
              unsigned int i;
              gsl_vector_view vT;

              /* The kernel are the last 'dof' columns of Q */
              /* Note that P^t A q_i = 0 <-> A q_i = 0 */
              NEW(*T,nc*dof,double);
              for(k=*rank,i=0;k<nc;k++,i++)
                {
                  vT=gsl_vector_view_array_with_stride(&((*T)[RC2INDEX(0,i,nc,dof)]),(ROW_MAJOR?dof:1),nc);
                  gsl_vector_set_basis(&(vT.vector),k);
                  gsl_linalg_QR_Qvec(&(At.matrix),tau,&(vT.vector));
                }
            }
          else
            *T=NULL;

          if (getBase)
            {
              NEW(*IR,nr,boolean);
              for(k=0;k<nr;k++)
                (*IR)[k]=FALSE;
              for(k=0;k<*rank;k++)
                (*IR)[gsl_permutation_get(p,k)]=TRUE;
            }
          else
            *IR=NULL;
        }
      gsl_vector_free(norm);
      gsl_vector_free(tau);
      gsl_permutation_free(p);
    }
  return(err);
}

void PrintGSLVector(FILE *f,char *label,gsl_vector *v)
{
  unsigned int i;

  if (label!=NULL)
    fprintf(f,"%s=",label);
  fprintf(f,"[");
  for(i=0;i<v->size;i++)
    fprintf(f,"%.16f ",gsl_vector_get(v,i));
  fprintf(f,"];\n");
}

void PrintGSLPermutation(FILE *f,char *label,gsl_permutation *p)
{
  unsigned int i;

  if (label!=NULL)
    fprintf(f,"%s=",label);
  fprintf(f,"[");
  for(i=0;i<p->size;i++)
    fprintf(f,"%lu ",gsl_permutation_get(p,i));
  fprintf(f,"];\n");
}

void PrintGSLMatrix(FILE *f,char *label,gsl_matrix *M)
{
  unsigned int i,j;

  if (label!=NULL)
    fprintf(f,"%s=",label);
  fprintf(f,"[");
  for(i=0;i<M->size1;i++)
    {
      for(j=0;j<M->size2;j++)
        fprintf(f,"%.16f ",gsl_matrix_get(M,i,j));
      fprintf(f,"\n");
    }
  fprintf(f,"];\n");
}


/******************************************************************/
/*                Public function definitions                     */

int MatrixDeterminantSgn(double epsilon,unsigned int n,double *A)
{
  int s;
  gsl_matrix_view m;
  gsl_permutation *p;
  
  #if (_DEBUG<2)
    gsl_set_error_handler_off();
  #endif

  /* We typically use this function for relatively large matrices
     and, thus, we do not take into account the case where n<=3 */
  p=gsl_permutation_alloc(n);
  m=gsl_matrix_view_array(A,n,n);
  
  gsl_linalg_LU_decomp(&(m.matrix),p,&s);

  s=gsl_linalg_LU_sgndet(&(m.matrix),s);
  
  gsl_permutation_free(p);

  return(s);
}

double MatrixDeterminant(unsigned int n,double *A)
{
  double d;

  #if (_DEBUG<2)
    gsl_set_error_handler_off();
  #endif

  /* Observe that det(A)=det(A^t) and thus for the case 2 and 3
     it does not matter if the matrix is stored ROW or COLUMN major */
  switch(n)
    {
    case 1:
      d=A[0];
      break;
    case 2:
      d=A[0]*A[3]-A[1]*A[2];
      break;
    case 3:
      d= A[0]*A[4]*A[8]+A[2]*A[3]*A[7]+A[1]*A[5]*A[6]
        -A[2]*A[4]*A[6]-A[1]*A[3]*A[8]-A[0]*A[5]*A[7];
      break;
    default:
      {
        gsl_matrix_view m;
        gsl_permutation *p;
        int s;
        
        p=gsl_permutation_alloc(n);
        m=gsl_matrix_view_array(A,n,n);

        gsl_linalg_LU_decomp(&(m.matrix),p,&s);
        
        d=gsl_linalg_LU_det(&(m.matrix),s);
        
        gsl_permutation_free(p);
      }
    }
  return(d);
}

void InitNewton(unsigned int nr,unsigned int nc,TNewton *n)
{

  #if (_DEBUG<2)
    gsl_set_error_handler_off();
  #endif

  n->nr=nr;
  n->nc=nc;

  NEW(n->Ab,nr*nc,double);
  NEW(n->bb,nr,double);

  n->A=gsl_matrix_view_array(n->Ab,n->nr,n->nc);
  n->b=gsl_vector_view_array(n->bb,n->nr);

  n->w=gsl_vector_alloc(nc);
  
  /* V and S are initialized to zero since if nr<nc some parts
     of them are actually not used. */
  n->V=gsl_matrix_calloc(nc,nc);
  n->S=gsl_vector_calloc(nc);
  n->tmp=gsl_vector_alloc(nr<nc?nr:nc);

  if (nr<nc)
    {
      /* When doing the SVD of the transpose we actually
         re-use memory for the non-transposed case. This 
         is done to safe memory but mainly to avoid copying
         matrices. */
      n->VT=gsl_matrix_submatrix(n->V,0,0,nc,nr);
      n->UT=gsl_matrix_submatrix(&(n->A.matrix),0,0,nr,nr);
      n->ST=gsl_vector_subvector(n->S,0,nr);
    }
  
  #if (DAMPED_NEWTON)
    n->alpha=DN_F;
  #endif
}

int NewtonStep(double nullSingularValue,double *x,double *dif,TNewton *n)
{
  int err;
  unsigned int i,j;
  double d;

  /* This is a replacement for tolerant_SV_decomp. See this function to
     understand what we do here. */
  if (n->nr<n->nc) 
    {
      /* Store A^t in n->VT (which is actually a submatrix of n->V. Note that
         the space for A will be over-write (to store U). However, only the
         first (nr X nr) block of A is used. The rest is not touched. This
         part can be set to zero, but it is not necessary since these
         values are actually not used when solving. */
      for(i=0;i<n->nr;i++) 
        {
          for(j=0;j<n->nc;j++) 
            gsl_matrix_set(&(n->VT.matrix),j,i,n->Ab[RC2INDEX(i,j,n->nr,n->nc)]);
        }
 
      err=gsl_linalg_SV_decomp(&(n->VT.matrix),&(n->UT.matrix),&(n->ST.vector),n->tmp);
        
      /* There is no need to copy VT in V since they share memory and the same
         applies for UT (shares space with U) and ST (shares space with S). */
    }
  else
    err=gsl_linalg_SV_decomp(&(n->A.matrix),n->V,n->S,n->tmp);

  /* Correct the estimation and compute the error  */
  if (!err)
    {
      /* Adjust the eigenvalues (never more than 'nc' eigenvalues) */
      for(i=0;i<n->nc;i++) 
        {
          if (gsl_vector_get(n->S,i)<nullSingularValue)
            gsl_vector_set(n->S,i,0.0);
        }

      /* This automatically solves least-square or the least-norm depending on
         the problem size and the number of non-null eigenvalues. This
         flexibility can not be achieved with QR typically because our matrices
         are typically not full rank. */
      err=gsl_linalg_SV_solve(&(n->A.matrix),n->V,n->S,&(n->b.vector),n->w);
      
      if (!err)
        {
          (*dif)=0.0;
          
          for(i=0;i<n->nc;i++)
            {
              d=gsl_vector_get(n->w,i);
              #if (DAMPED_NEWTON)
                x[i]-=(n->alpha*d);
              #else
                x[i]-=d;
              #endif
              (*dif)+=(d*d);
            }
          (*dif)=sqrt(*dif);
          
          #if (DAMPED_NEWTON)
            n->alpha+=DN_W*(DN_MF-n->alpha);
          #endif
          
          /* If the error is too large assume will never converge */
          if ((*dif)>1e10)
            err=1;
        }
    }

  return(err);
}

void DeleteNewton(TNewton *n)
{
  free(n->Ab);
  free(n->bb);
  
  gsl_vector_free(n->w);

  gsl_matrix_free(n->V);
  gsl_vector_free(n->S);
  gsl_vector_free(n->tmp);
}

void InitLS(unsigned int nr,unsigned int nc,TLinearSystem *ls)
{
  #if (_DEBUG<2)
    gsl_set_error_handler_off();
  #endif

  if (nr<nc)
    Error("The linear system must be well-constrained or over-constrained");

  ls->nr=nr;
  ls->nc=nc;

  NEW(ls->Ab,nr*nc,double);
  NEW(ls->bb,nr,double);
  NEW(ls->xb,nc,double);

  ls->A=gsl_matrix_view_array(ls->Ab,nr,nc);
  ls->b=gsl_vector_view_array(ls->bb,nr);
  ls->x=gsl_vector_view_array(ls->xb,nc);

  if (nr==nc)
    ls->p=gsl_permutation_alloc(nr);
  else
    {
      /* nr>nc */
      ls->tau=gsl_vector_alloc(nr);
      ls->res=gsl_vector_alloc(nr);
    }
}


int LSSolve(TLinearSystem *ls)
{
  int err;

  if (ls->nr==ls->nc)
    {
      int s;

      /* Well constrained systems -> LU decomposition */
      err=gsl_linalg_LU_decomp(&(ls->A.matrix),ls->p,&s);
      if (!err)
        err=gsl_linalg_LU_solve(&(ls->A.matrix),ls->p,&(ls->b.vector),&(ls->x.vector));
    }
  else
    {
      /* Over-constrained system -> QR decomposition and less-square solve */
      err=gsl_linalg_QR_decomp(&(ls->A.matrix),ls->tau);

      if (!err)
        err=gsl_linalg_QR_lssolve(&(ls->A.matrix),ls->tau,&(ls->b.vector),&(ls->x.vector),ls->res);
    }

  return(err);
}

void DeleteLS(TLinearSystem *ls)
{
  free(ls->Ab);
  free(ls->bb);
  free(ls->xb);
  
  if (ls->nr==ls->nc)
    gsl_permutation_free(ls->p);
  else
    {
      gsl_vector_free(ls->tau);
      gsl_vector_free(ls->res);
    }
}

#endif

/* END GSL IMPLEMENTATION */


/* BEGIN LAPACK IMPLEMENTATION */
#ifdef _LAPACK

unsigned int AnalyzeKernel(unsigned int nr,unsigned int nc,double *mT,
                           unsigned int dof,double epsilon,
                           boolean computeRank,boolean checkRank,boolean getT,boolean getBase,
                           boolean *singular,unsigned int *rank,boolean **IR,double **T)
{
  unsigned int err;

  if (dof==nc)
    {
      /* Trivial case of a system without equations. We take this into account for completeness, but
         this function should never be used in this case. */
      *singular=FALSE;
      *rank=0;

      if ((getT)&&(nc>0))
        {
          unsigned int i;

          NEWZ(*T,nc*dof,double);
          for(i=0;i<nc;i++)
            (*T)[RC2INDEX(i,i,nc,dof)]=1.0;
        }
      else
        *T=NULL;

      if ((getBase)&&(nr>0))
        {
          unsigned int i;

          NEW(*IR,nr,boolean);
          for(i=0;i<nr;i++)
            (*IR)[i]=FALSE;
        }
      else
        *IR=NULL;

      err=0;
    }
  else
    { 
      int *p,lwork,info;
      double *tau,wOpt,*work;

      /* Extra space for the QR decomposition of At */
      NEW(tau,(nr<nc?nr:nc),double);
      NEWZ(p,nr,int); /* This MUST be initialized to zero! */

      /* allocate the work space. We assume that the work space for the decomposition is larger
         than that for the unpacking. */

      /* dgeqp3(M,N,A,LDA,PVT,TAU,WORK,LWORK,INFO) */
      /* computes the QR factorization, with column pivoting */
      lwork=-1;
      #ifdef _LAPACKE
        info=(int)LAPACKE_dgeqp3_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                      (lapack_int)nc,(lapack_int)nr,mT,(lapack_int)nc,
                                      (lapack_int*)p,tau,&wOpt,(lapack_int)lwork);
      #else
        dgeqp3_((int*)&nc,(int*)&nr,mT,(int*)&nc,p,tau,&wOpt,&lwork,&info);
      #endif
      lwork=(int)wOpt;
      NEW(work,lwork,double);

      /* Decompose the matrix, with column pivoting to get the kernel. Note that
         the permutation is not relevant for the kernel P*A*v=0 -> A*v=0  */
      #ifdef _LAPACKE
        info=(int)LAPACKE_dgeqp3_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                      (lapack_int)nc,(lapack_int)nr,mT,(lapack_int)nc,
                                      (lapack_int*)p,tau,work,(lapack_int)lwork);
      #else      
        dgeqp3_((int*)&nc,(int*)&nr,mT,(int*)&nc,p,tau,work,&lwork,&info);
      #endif

      err=(info!=0);

      if (err)
        {
          *singular=TRUE;
          *rank=NO_UINT;
          *T=NULL;
          *IR=NULL;
          err=3; /* Our error code for decomposition error */
        }
      else
        {
          unsigned int k,mrc;
          double r;

          /***********************************************/
          /* At is nc x nr */
          /* Q is nc x nc */
          /* R is nc x nr -> R^t is nr x nc */

          /* At=Q R P^t -> A= P R^t Q^t -> P^t A Q = R^t 
             Thus, the rows of R (columns of R^t) that are zero
             indicate columns of Q that define part of the kernel.
             Moreover, the norm of teh rows of R indicate how far
             is from defining a new kernel basis vector. Sinc R
             is triangular the norm of the row is the norm from
             the diagonal to the end of the row (nr entries) */

          mrc=(nr<nc?nr:nc);

          if (computeRank)
            {
              /* Rank = number of not-null rows of R */
              /* The rank can not be larger than the mrc=min(nr,nc) */
              *rank=0;
              for(k=0;k<mrc;k++) 
                { 
                  /* Get the norm of row k of R from the diagonal to the end of the row */
                  r=NormWithStride(nr-k,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(k,k,nc,nr)]));
                  if (r>epsilon)
                    (*rank)++;
                }
              if (dof==0)
                {
                  *singular=FALSE;
                  /* temporary set 'dof' form '*rank' */
                  dof=nc-*rank;
                }
              else
                *singular=((nc-dof)!=*rank);
            }
          else
            {
              if (dof==0)
                Error("Null 'dof' parameter in AnalyzeKernel");
              if (dof>nc)
                Error("We can not have more 'dof' than variables");

              *rank=nc-dof;

              /* Check if row *rank-1 of R is actually not null. */
              r=NormWithStride(nr-(*rank-1),(ROW_MAJOR?1:nc),&(mT[RC2INDEX(*rank-1,*rank-1,nc,nr)]));
              *singular=(r<epsilon);
      
              if (checkRank)
                {
                  if (*singular)
                    err=1; //Error("Singular point (increase N_DOF? Decrease EPSILON?)");
              
                  /* Checking row '*rank' is actually null. */
                  r=NormWithStride(nr-*rank,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(*rank,*rank,nc,nr)]));
                  if (r>epsilon)
                    err=2; //Error("Non-null kernel vector (decrease N_DOF? Increase EPSILON?)");
                }
            }

          #if (_DEBUG>0)
            if ((dof>0)&&(!computeRank)&&(getT)&&(!getBase))
              {
                printf("  E=[ ");
                for(k=*rank;k<mrc;k++) 
                  {
                    r=NormWithStride(nr-k,(ROW_MAJOR?1:nc),&(mT[RC2INDEX(k,k,nc,nr)]));
                    printf("%.16f ",r);
                  }
                printf("];\n");
              }
          #endif

          /* If requested, we define T and IR, even if the chart is singular. In this case
             we return the most likely T and IR. */
          if (getT)
            {
              unsigned int i;
              char left='L';
              char noTrans='N';

              /* The kernel are the last 'dof' columns of Q */
              /* Note that P^t A q_i = 0 <-> A q_i = 0 */
              NEWZ(*T,nc*dof,double); /* init to zero */
              for(k=*rank,i=0;k<nc;k++,i++)
                (*T)[RC2INDEX(k,i,nc,dof)]=1.0;
              /* DORMQR(SIDE,TRANS,M,N,K,A,LDA,TAU,C,LDC,WORK,LWORK,INFO) */
              /* (*T)=Q*(*T)  (with Q encoded as elementary reflectors) */
              #ifdef _LAPACKE
                info=(int)LAPACKE_dormqr_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),left,noTrans,
                                              (lapack_int)nc,(lapack_int)dof,(lapack_int)(nr<nc?nr:nc),
                                              mT,(lapack_int)nc,tau,*T,(lapack_int)nc,work,(lapack_int)lwork);
              #else
                dormqr_(&left,&noTrans,(int*)&nc,(int*)&dof,(nr<nc?(int*)&nr:(int*)&nc),mT,(int*)&nc,tau,*T,(int*)&nc,work,&lwork,&info);
              #endif
              err=(info!=0);
            }
          else
            *T=NULL;

          if (getBase)
            {
              NEW(*IR,nr,boolean);
              for(k=0;k<nr;k++)
                (*IR)[k]=FALSE;
              for(k=0;k<*rank;k++)
                (*IR)[p[k]-1]=TRUE; /* permutations numbered from 1 in lapack */
            }
          else
            *IR=NULL;
        }
      free(work);
      free(tau);
      free(p);
    }
  return(err);
}

int MatrixDeterminantSgn(double epsilon,unsigned int n,double *A)
{
  int s,info,*p;
  double v;

  NEW(p,n,int);
  
  /* LU decomposition */
  /* DGETRF(M,N,A,LDA,IPIV,INFO) */
  /* computes an LU factorization of a general M-by-N matrix using partial pivoting with row interchanges. */
  #ifdef _LAPACKE
    info=(int)LAPACKE_dgetrf_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                  (lapack_int)n,(lapack_int)n,A,(lapack_int)n,(lapack_int*)p);
  #else
    dgetrf_((int*)&n,(int*)&n,A,(int*)&n,p,&info);
  #endif
  
  if (info==0)
    {
      unsigned int i;

      /* Get the sign of the permutation */
      s=1;
      for(i=0;i<n;i++)
        if ((p[i]-1)!=i) s=-s; /* permutations numbered from 1 in lapack */

      /* and now the sign of the diagonal of U */
      for(i=0;((s!=0)&&(i<n));i++)
        {
          v=A[RC2INDEX(i,i,n,n)];
          if (fabs(v)<epsilon) s=0;
          else
            {
              if (v<0.0) s=-s;
            }
        }
    }
  else
    s=0;

  free(p);

  return(s);
}

double MatrixDeterminant(unsigned int n,double *A)
{
  double d;

  /* Observe that det(A)=det(A^t) and thus for the case 2 and 3
     it does not matter if the matrix is stored ROW or COLUMN major */
  switch(n)
    {
    case 1:
      d=A[0];
      break;
    case 2:
      d=A[0]*A[3]-A[1]*A[2];
      break;
    case 3:
      d= A[0]*A[4]*A[8]+A[2]*A[3]*A[7]+A[1]*A[5]*A[6]
        -A[2]*A[4]*A[6]-A[1]*A[3]*A[8]-A[0]*A[5]*A[7];
      break;
    default:
      {
        int info,*p;

        NEW(p,n,int);

        /* LU decomposition */
        /* DGETRF(M,N,A,LDA,IPIV,INFO) */
        /* computes an LU factorization of a general M-by-N matrix using partial pivoting with row interchanges. */
        #ifdef _LAPACKE
          info=(int)LAPACKE_dgetrf_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                        (lapack_int)n,(lapack_int)n,A,(lapack_int)n,(lapack_int*)p);
        #else
          dgetrf_((int*)&n,(int*)&n,A,(int*)&n,p,&info);
        #endif
        if (info==0)
          {
            unsigned int i;

            /* Get the sign of the permutation */
            d=1.0;
            for(i=0;i<n;i++)
              if ((p[i]-1)!=i) d=-d; /* permutations numbered from 1 in lapack */

            /* and now the product of the diagonal of U */
            for(i=0;i<n;i++)
              d*=A[RC2INDEX(i,i,n,n)];
          }
        else
          d=INF;
        
        free(p);
      }
    }
  return(d);
}

void InitNewton(unsigned int nr,unsigned int nc,TNewton *n)
{
  int rank,one=1,info;
  double w,e=1e-6;

  n->nr=(int)nr;
  n->nc=(int)nc;

  NEW(n->Ab,nr*nc,double);
  /* b is used for input and output */
  NEW(n->bb,(nr>nc?nr:nc),double);

  NEW(n->s,(nr<nc?nr:nc),double);

  /* Request the optimal size for the work buffer */
  /* DGELSS(M,N,NRHS,A,LDA,B,LDB,S,RCOND,RANK,WORK,LWORK,INFO) */
  n->lwork=-1;
  #ifdef _LAPACKE
    info=(int)LAPACKE_dgelss_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                  (lapack_int)n->nr,(lapack_int)n->nc,one,n->Ab,
                                  (lapack_int)n->nr,n->bb,(lapack_int)(n->nr>n->nc?n->nr:n->nc),
                                  n->s,e,(lapack_int*)&rank,&w,n->lwork);
  #else
    dgelss_(&n->nr,&n->nc,&one,n->Ab,&n->nr,n->bb,(n->nr>n->nc?&n->nr:&n->nc),n->s,&e,&rank,&w,&n->lwork,&info);
  #endif
    
  if (info!=0)  
    Error("Clapack error in InitNewton");
  
  n->lwork=(int)w;
  NEW(n->work,n->lwork,double);

  #if (DAMPED_NEWTON)
    n->alpha=DN_F;
  #endif
}

int NewtonStep(double nullSingularValue,double *x,double *dif,TNewton *n)
{
  int rank,info,one=1;

  /* Minimum norm solution via SVD (works even if the matrix is not full rank) */

  /* DGELSS(M,N,NRHS,A,LDA,B,LDB,S,RCOND,RANK,WORK,LWORK,INFO) */
  /* computes the minimum norm solution to a real linear least squares problem: */
  /* singular values smaller than MaxSingularValue*Rcond are treated as zero */
  /* This is a difference w.r.t the GSL implementation! */
  #ifdef _LAPACKE
    info=(int)LAPACKE_dgelss_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                  (lapack_int)n->nr,(lapack_int)n->nc,one,n->Ab,
                                  (lapack_int)n->nr,n->bb,(lapack_int)(n->nr>n->nc?n->nr:n->nc),
                                  n->s,nullSingularValue,(lapack_int*)&rank,n->work,n->lwork);
  #else
    dgelss_(&n->nr,&n->nc,&one,n->Ab,&n->nr,n->bb,(n->nr>n->nc?&n->nr:&n->nc),n->s,&nullSingularValue,&rank,n->work,&n->lwork,&info);
  #endif
  
  if (info==0)
    {
      #if (DAMPED_NEWTON)
        SumVectorScale(n->nc,x,-n->alpha,n->bb,x);
        n->alpha+=DN_W*(DN_MF-n->alpha);
      #else
        SubtractVector(n->nc,x,n->bb);
      #endif
      *dif=Norm(n->nc,n->bb);
      
      /* If the error is too large assume will never converge */
      if ((*dif)>1e10)
        info=1;
    }
  
  return(info!=0);
}

void DeleteNewton(TNewton *n)
{
  free(n->Ab);
  free(n->bb);

  free(n->s);

  free(n->work);
}

void InitLS(unsigned int nr,unsigned int nc,TLinearSystem *ls)
{
  if (nr<nc)
    Error("The linear system must be well-constrained or over-constrained");

  ls->nr=(int)nr;
  ls->nc=(int)nc;

  NEW(ls->Ab,nr*nc,double);
  NEW(ls->bb,nr,double);
  ls->xb=ls->bb; /* vector re-used */

  if (nr==nc)
    {
      /* Solve via LU decompositoin */
      NEW(ls->p,nr,int);
    }
  else
    {
      /* Solve via QR decomposition */
      int one=1,info;
      double w;
      char noTrans='N';

      /* Request the optimal size for the work buffer */
      /* DGELS(TRANS,M,N,NRHS,A,LDA,B,LDB,WORK,LWORK,INFO) */
      /* DGELS solves overdetermined or underdetermined real linear systems */
      /* It uses QR or LQ decompositions. */
      /* It assumes the matrix to be full rank. */
      ls->lwork=-1;
      #ifdef _LAPACKE
        info=(int)LAPACKE_dgels_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),noTrans,
                                     (lapack_int)ls->nr,(lapack_int)ls->nc,one,ls->Ab,
                                     (lapack_int)ls->nr,ls->bb,(lapack_int)ls->nr,&w,ls->lwork);
      #else
        dgels_(&noTrans,&ls->nr,&ls->nc,&one,ls->Ab,&ls->nr,ls->bb,&ls->nr,&w,&ls->lwork,&info);
      #endif
      
      if (info!=0)      
        Error("Clapack error in InitLS");
        
      ls->lwork=(int)w;
      NEW(ls->work,ls->lwork,double);
    }
}


int LSSolve(TLinearSystem *ls)
{
  int one=1;
  int info;
  char noTrans='N';

  if (ls->nr==ls->nc)
    {
      /* DGESV(N,NRHS,A,LDA,IPIV,B,LDB,INFO) */
      /* Computes the solution to a real system of linear equations */
      /* The LU decomposition with partial pivoting and row interchanges is used */
      #ifdef _LAPACKE
        info=(int)LAPACKE_dgesv_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),
                                     (lapack_int)ls->nr,one,ls->Ab,(lapack_int)ls->nr,
                                     (lapack_int*)ls->p,ls->bb,(lapack_int)ls->nr);
      #else
        dgesv_(&ls->nr,&one,ls->Ab,&ls->nr,ls->p,ls->bb,&ls->nr,&info);
      #endif
    }
  else
    /* Least square QR */
    /* DGELS(TRANS,M,N,NRHS,A,LDA,B,LDB,WORK,LWORK,INFO) */
    /* DGELS solves overdetermined or underdetermined real linear system using a QR or LQ factorization. */
    /* It is assumed that the matrix has full rank. */
    #ifdef _LAPACKE
      info=(int)LAPACKE_dgels_work((ROW_MAJOR?LAPACK_ROW_MAJOR:LAPACK_COL_MAJOR),noTrans,
                                   (lapack_int)ls->nr,(lapack_int)ls->nc,one,ls->Ab,
                                   (lapack_int)ls->nr,ls->bb,(lapack_int)ls->nr,ls->work,ls->lwork);
    #else
      dgels_(&noTrans,&ls->nr,&ls->nc,&one,ls->Ab,&ls->nr,ls->bb,&ls->nr,ls->work,&ls->lwork,&info);
    #endif

  return(info!=0);
}

void DeleteLS(TLinearSystem *ls)
{
  free(ls->Ab);
  free(ls->bb);

  if (ls->nr==ls->nc)
    free(ls->p);
  else
    free(ls->work);
}


#endif

/* END LAPACK IMPLEMENTATION */


/* GENERIC IMPLEMENTATION */
/***************************************************************************/
/***************************************************************************/
/***************************************************************************/


/* Some functions are independent of the underlying linear algebra package */

unsigned int FindRank(double epsilon,unsigned int nr,unsigned int nc,double *mT)
{
  boolean singular;
  boolean *IR;
  double *T;
  unsigned int rank;
  
  AnalyzeKernel(nr,nc,mT,0,epsilon,
                TRUE,FALSE,FALSE,FALSE, /* computeRank, checkRank, getT, getBase */
                &singular,&rank,&IR,&T);
  
  return(rank);
}

unsigned int FindKernel(unsigned int nr,unsigned int nc,double *mT,
                        unsigned int dof,boolean check,double epsilon,double **T)
{
  unsigned int err;
  boolean singular;
  boolean *IR;
  unsigned int rank;

  err=AnalyzeKernel(nr,nc,mT,dof,epsilon,
                    FALSE,check,TRUE,FALSE, /* computeRank, checkRank, getT, getBase */
                    &singular,&rank,&IR,T);
  return(err);
}

unsigned int FindKernelAndIndependentRows(unsigned int nr,unsigned int nc,double *mT,
                                          unsigned int dof,double epsilon,boolean *singular,
                                          boolean **IR,double **T)
{
  unsigned int err;
  unsigned int rank;

  err=AnalyzeKernel(nr,nc,mT,dof,epsilon,
                    FALSE,TRUE,TRUE,TRUE, /* computeRank, checkRank, getT, getBase */
                    singular,&rank,IR,T);

  return(err);
}

inline double *GetNewtonMatrixBuffer(TNewton *n)
{
  return(n->Ab);
}

inline double *GetNewtonRHBuffer(TNewton *n)
{
  return(n->bb);
}

inline void NewtonSetMatrix(unsigned int i,unsigned int j,double v,TNewton *n)
{
  n->Ab[RC2INDEX(i,j,n->nr,n->nc)]=v;
}

inline void NewtonSetRH(unsigned int i,double v,TNewton *n)
{
  n->bb[i]=v;
}

inline double *GetLSMatrixBuffer(TLinearSystem *ls)
{
  return(ls->Ab);
}

inline double *GetLSRHBuffer(TLinearSystem *ls)
{
  return(ls->bb);
}

inline double *GetLSSolutionBuffer(TLinearSystem *ls)
{
  return(ls->xb);
}

inline void LSSetMatrix(unsigned int i,unsigned int j,double v,TLinearSystem *ls)
{
  ls->Ab[RC2INDEX(i,j,ls->nr,ls->nc)]=v;
}

inline void LSSetRH(unsigned int i,double v,TLinearSystem *ls)
{
  ls->bb[i]=v;
}