basic_algebra.c

Go to the documentation of this file.

#include "basic_algebra.h"

#include "defines.h"
#include "geom.h"

#include <math.h>

CBLAS_INLINE double GeneralDotProduct(unsigned int s,double *v1,double *v2)
{
  #ifdef _CBLAS
    return(cblas_ddot(s,v1,1,v2,1));
  #else
    unsigned int i;
    double dp;
    
    dp=0.0;
    for(i=0;i<s;i++)
      dp+=(v1[i]*v2[i]);
    return(dp);
  #endif
}

CBLAS_INLINE void ScaleVector(double f,unsigned int s,double *v)
{
  #ifdef _CBLAS
    cblas_dscal(s,f,v,1);
  #else
    unsigned int i;

    for(i=0;i<s;i++)
      v[i]*=f;
  #endif
}

CBLAS_INLINE void ScaleVector2(double f,unsigned int s,double *v,double *vout)
{
  #ifdef _CBLAS
    cblas_dcopy(s,v,1,vout,1);
    cblas_dscal(s,f,vout,1);
  #else
    unsigned int i;

    for(i=0;i<s;i++)
      vout[i]=v[i]*f;
  #endif
}

CBLAS_INLINE void AccumulateVector(unsigned int s,double *v1,double *v2)
{
  #ifdef _CBLAS
    cblas_daxpy(s,1.0,v1,1,v2,1);
  #else
    unsigned int i;
    
    for(i=0;i<s;i++)
      v2[i]+=v1[i];
  #endif
}

CBLAS_INLINE void SumVector(unsigned int s,double *v1,double *v2,double *v)
{
  #ifdef _CBLAS
    /* If v==v2 and we copy v1 on v, v2 will be lost */
    if (v==v2)
      cblas_daxpy(s,1.0,v1,1,v,1);
    else
      {
        cblas_dcopy(s,v1,1,v,1); 
        cblas_daxpy(s,1.0,v2,1,v,1);
      }
  #else
    unsigned int i;
    
    for(i=0;i<s;i++)
      v[i]=(v1[i]+v2[i]);
  #endif
}

CBLAS_INLINE void SumVectorScale(unsigned int s,double *v1,double w,double *v2,double *v)
{
  #ifdef _CBLAS
    /* If v==v2 and we copy v1 on v, v2 will be lost */
    if (v==v2)
      {
        cblas_dscal(s,w,v2,1);
        cblas_daxpy(s,1.0,v1,1,v,1); /* v=v+v1=w*v2+v1 */
      }
    else
      {
        cblas_dcopy(s,v1,1,v,1);
        cblas_daxpy(s,w,v2,1,v,1);
      }
  #else
    unsigned int i;

    for(i=0;i<s;i++)
      v[i]=(v1[i]+w*v2[i]);
  #endif
}

void CosVector(unsigned int s,double *v,double *co)
{
  unsigned int i;

  for(i=0;i<s;i++)
    co[i]=cos(v[i]);
}

void SinVector(unsigned int s,double *v,double *si)
{
  unsigned int i;

  for(i=0;i<s;i++)
    si[i]=sin(v[i]);
}

unsigned int MaxVectorElement(unsigned int m,double *v)
{
  if (m==0)
    return(NO_UINT);
  else
    {
      unsigned int i;
      unsigned int me;
      double ma;
      
      me=0;
      ma=v[0];
      for(i=1;i<m;i++)
        {
          if (v[i]>ma)
            {
              ma=v[i];
              me=i;
            }
        }
      return(me);
    }
}

double MaxVector(unsigned int m,double *v)
{
  if (m==0)
    return(0.0);
  else
    {
      unsigned int i;
      double ma;
      
      ma=v[0];
      for(i=1;i<m;i++)
        {
          if (v[i]>ma)
            ma=v[i];
        }
      return(ma);
    }
}

unsigned int MinVectorElement(unsigned int m,double *v)
{
  if (m==0)
    return(NO_UINT);
  else
    {
      unsigned int i;
      unsigned int me;
      double mi;
      
      me=0;
      mi=v[0];
      for(i=1;i<m;i++)
        {
          if (v[i]<mi)
            {
              mi=v[i];
              me=i;
            }
        }
      return(me);
    }
}

double MinVector(unsigned int m,double *v)
{
  if (m==0)
    return(0.0);
  else
    {
      unsigned int i;
      double mi;
      
      mi=v[0];
      for(i=1;i<m;i++)
        {
          if (v[i]<mi)
            mi=v[i];
        }
      return(mi);
    }
}

CBLAS_INLINE void SubtractVector(unsigned int s,double *v1,double *v2)
{
  #ifdef _CBLAS
    cblas_daxpy(s,-1.0,v2,1,v1,1);
  #else
    unsigned int i;
    
    for(i=0;i<s;i++)
      v1[i]-=v2[i];
  #endif
}

/* We always inline DifferenceVector because it is used by other functions below */
inline void DifferenceVector(unsigned int s,double *v1,double *v2,double *v)
{
  #ifdef _CBLAS
    /* If v==v2 and we copy v1 on v, v2 will be lost */
    if (v==v2)
      {
        cblas_dscal(s,-1.0,v2,1);
        cblas_daxpy(s,1.0,v1,1,v,1); /* v=v+v1=-v2+v1 */
      }
    else
      {
        cblas_dcopy(s,v1,1,v,1);
        cblas_daxpy(s,-1.0,v2,1,v,1);
      }
  #else
    unsigned int i;

    for(i=0;i<s;i++)
      v[i]=(v1[i]-v2[i]);
  #endif
}

void DifferenceVectorTopology(unsigned int s,unsigned int *tp,
                              double *v1,double *v2,double *v)
{
  if (tp==NULL)
    DifferenceVector(s,v1,v2,v);
  else
    {      
      unsigned int i;
      double d;
      
      for(i=0;i<s;i++)
        {
          d=v1[i]-v2[i];
          if (tp[i]==TOPOLOGY_S)
            PI2PI(d);
          v[i]=d;
        }
    }
}

CBLAS_INLINE double Norm(unsigned int s,double *v)
{
  #ifdef _CBLAS
    return(cblas_dnrm2(s,v,1));
  #else
    unsigned int i;
    double n;

    n=0.0;
    for(i=0;i<s;i++)
      n+=(v[i]*v[i]);
    return(sqrt(n));
  #endif
}

CBLAS_INLINE double NormWithStride(unsigned int s,unsigned int st,double *v)
{
  #ifdef _CBLAS
    return(cblas_dnrm2(s,v,st));
  #else
    unsigned int i,j;
    double n;

    n=0.0;
    for(i=0,j=0;i<s;i++,j+=st)
      n+=(v[j]*v[j]);
    return(sqrt(n));
  #endif
}

/* We inline SquaredDistance in all the cases, not just if CBLAS is present
   because it is used in functions below */
inline double SquaredDistance(unsigned int s,double *v1,double *v2)
{
  double n;

  #ifdef _NO_CBLAS
    /* Not clear if this is more efficient than the naive implementation.
       So, this is not used right now. */
    n=cblas_ddot(s,v1,1,v1,1)
      -2.0*cblas_ddot(s,v1,1,v2,1)
      +cblas_ddot(s,v2,1,v2,1);
  #else
    unsigned int i;
    double v;
    
    n=0;
    for(i=0;i<s;i++)
      {
        v=v1[i]-v2[i];
        n+=(v*v);
    }
  #endif

  return(n);
}

/* We inline Distance in all the cases, not just if CBLAS is present */
inline double Distance(unsigned int s,double *v1,double *v2)
{
  return(sqrt(SquaredDistance(s,v1,v2)));
}

double SquaredDistanceTopology(unsigned int s,unsigned int *tp,
                               double *v1,double *v2)
{
  double n;
  
  /* We in-line the DistanceTopology function, for efficiency reasons. */
  if (tp==NULL)
    n=SquaredDistance(s,v1,v2);
  else
    {
      unsigned int i;
      double n,v;

      for(i=0;i<s;i++)
        {
          v=v1[i]-v2[i];
          if ((tp[i]==TOPOLOGY_S)&&((v<-M_PI)||(v>M_PI)))
            PI2PI(v);
          n+=(v*v);
        }
    }
  return(n);
}

double SquaredDistanceTopologyMin(double t2,unsigned int s,unsigned int *tp,
                                  double *v1,double *v2)
{
  unsigned int i;
  double n,v;
 
  n=0.0;
  if (tp==NULL)
    {
      /* This is the Distance function inlined (and modified) here for
         efficiency (avoid an additonal function call) */
      for(i=0;((i<s)&&(n<t2));i++)
        {
          v=v1[i]-v2[i];
          n+=(v*v);
        }
    }
  else
    {
      for(i=0;((i<s)&&(n<t2));i++)
        {
          v=v1[i]-v2[i];
          if ((tp[i]==TOPOLOGY_S)&&((v<-M_PI)||(v>M_PI)))
            PI2PI(v);
          n+=(v*v);
        }
    }
  return(n);
}

double DistanceTopology(unsigned int s,unsigned int *tp,
                        double *v1,double *v2)
{
  double n;

  if (tp==NULL)
    n=SquaredDistance(s,v1,v2);
  else
    {
      unsigned int i;
      double v;
      
      n=0.0;
      for(i=0;i<s;i++)
        {
          v=v1[i]-v2[i];
          if ((tp[i]==TOPOLOGY_S)&&((v<-M_PI)||(v>M_PI)))
            PI2PI(v);
          n+=(v*v);
        }
    }
  return(sqrt(n));
}

double DistanceTopologyMin(double t,unsigned int s,unsigned int *tp,
                           double *v1,double *v2)
{
  unsigned int i;
  double n,v,t2;
  
  n=0.0;
  t2=t*t;
  if (tp==NULL)
    {
      /* This is the SquaredDistance function inlined and modified for
         efficiency (avoid an additonal function call) */
      for(i=0;(i<s)&&(n<t2);i++)
        {
          v=v1[i]-v2[i];
          n+=(v*v);
        }
    }
  else
    {
      for(i=0;(i<s)&&(n<t2);i++)
        {
          v=v1[i]-v2[i];
          if ((tp[i]==TOPOLOGY_S)&&((v<-M_PI)||(v>M_PI)))
            PI2PI(v);
          n+=(v*v);
        }
    }
  return(sqrt(n));
}

boolean CrossTopologyBorder(unsigned int s,unsigned int *tp,double *v1,double *v2)
{
  return(Distance(s,v1,v2)>DistanceTopology(s,tp,v1,v2));
}

CBLAS_INLINE void Normalize(unsigned int s,double *v)
{
  double n;

  #ifdef _CBLAS
    n=cblas_dnrm2(s,v,1);
    if (n>1e-6)
      cblas_dscal(s,1.0/n,v,1);
    else
      Error("Normalizing a null vector");
  #else
    unsigned int i;

    n=Norm(s,v);
    if (n>1e-6) /* avoid division by (almost) zero */
      {
        for(i=0;i<s;i++)
          v[i]/=n;
      }
    else
      Error("Normalizing a null vector");
  #endif
}

double Mean(unsigned int s,double *v)
{
  unsigned int i;
  double m;
  
  m=0.0;
  for(i=0;i<s;i++)
    m+=v[i];
  
  return(m/((double)s));
}

double StdDev(unsigned int s,double m,double *v)
{
  unsigned int i;
  double sd,d;
  
  sd=0.0;
  for(i=0;i<s;i++)
    {
      d=v[i]-m;
      sd+=(d*d);
    }

  if (s>1)
    return(sqrt(sd/((double)s-1)));
  else
    return(sqrt(sd));
}

void ArrayPi2Pi(unsigned int n,unsigned int *t,double *a)
{
  unsigned int i;
  double *b;
  
  b=a;
  for(i=0;i<n;i++,b++)
    {
      if (t[i]==TOPOLOGY_S)
        PI2PI(*b);
    }
}

CBLAS_INLINE void GetRow(unsigned int k,unsigned int r,unsigned int c,double *m,double *v)
{
  #ifdef _CBLAS
    cblas_dcopy(c,&(m[RC2INDEX(k,0,r,c)]),(ROW_MAJOR?1:r),v,1);
  #else
    unsigned int i;
  
    for(i=0;i<c;i++)
      v[i]=m[RC2INDEX(k,i,r,c)];
  #endif
}

CBLAS_INLINE void GetColumn(unsigned int k,unsigned int r,unsigned int c,double *m,double *v)
{
  #ifdef _CBLAS
    cblas_dcopy(r,&(m[RC2INDEX(0,k,r,c)]),(ROW_MAJOR?c:1),v,1);
  #else
    unsigned int i;
  
    for(i=0;i<r;i++)
      v[i]=m[RC2INDEX(i,k,r,c)];
  #endif
}

CBLAS_INLINE void SetRow(double *v,unsigned int k,unsigned int r,unsigned int c,double *m)
{
  #ifdef _CBLAS
    cblas_dcopy(c,v,1,&(m[RC2INDEX(k,0,r,c)]),(ROW_MAJOR?1:r));
  #else
    unsigned int i;
  
    for(i=0;i<c;i++)
      m[RC2INDEX(k,i,r,c)]=v[i];
  #endif
}

CBLAS_INLINE void SetColumn(double *v,unsigned int k,unsigned int r,unsigned int c,double *m)
{
  #ifdef _CBLAS
    cblas_dcopy(r,v,1,&(m[RC2INDEX(0,k,r,c)]),(ROW_MAJOR?c:1));
  #else
    unsigned int i;
  
    for(i=0;i<r;i++)
      m[RC2INDEX(i,k,r,c)]=v[i];
  #endif
}

CBLAS_INLINE double RowSquaredNorm(unsigned int k,unsigned int r,unsigned int c,double *m)
{  
  double s;
  unsigned int i;

  #ifdef _CBLAS
    i=RC2INDEX(k,0,r,c);
    s=cblas_ddot(c,&(m[i]),(ROW_MAJOR?1:r),&(m[i]),(ROW_MAJOR?1:r));
  #else
    double v;

    s=0.0;
    for(i=0;i<c;i++)
      {
        v=m[RC2INDEX(k,i,r,c)];
        s+=(v*v);
      }
  #endif
  return(s);
}

CBLAS_INLINE double ColumnSquaredNorm(unsigned int k,unsigned int r,unsigned int c,double *m)
{
  double s;
  unsigned int i;

  #ifdef _CBLAS
    i=RC2INDEX(0,k,r,c);
    s=cblas_ddot(r,&(m[i]),(ROW_MAJOR?c:1),&(m[i]),(ROW_MAJOR?c:1));
  #else
    double v;

    s=0.0;
    for(i=0;i<r;i++)
      {
        v=m[RC2INDEX(i,k,r,c)];
        s+=(v*v);
      }
  #endif
  return(s);
}

CBLAS_INLINE void MatrixVectorProduct(unsigned int r,unsigned int c,double *A,double *b,double *o)
{
  #ifdef _CBLAS
    cblas_dgemv((ROW_MAJOR?CblasRowMajor:CblasColMajor),CblasNoTrans,r,c,1.0,A,(ROW_MAJOR?c:r),b,1,0.0,o,1);
  #else
    unsigned int i,j;

    for(i=0;i<r;i++)
      {
        o[i]=0.0;
        for(j=0;j<c;j++)
          o[i]+=(A[RC2INDEX(i,j,r,c)]*b[j]);
      }
  #endif
}

CBLAS_INLINE void TMatrixVectorProduct(unsigned int r,unsigned int c,double *A,double *b,double *o)
{
  #ifdef _CBLAS
    cblas_dgemv((ROW_MAJOR?CblasRowMajor:CblasColMajor),CblasTrans,r,c,1.0,A,(ROW_MAJOR?c:r),b,1,0.0,o,1);
  #else
    unsigned int i,j;

    for(i=0;i<c;i++)
      {
        o[i]=0.0;
        for(j=0;j<r;j++)
          o[i]+=(A[RC2INDEX(j,i,r,c)]*b[j]);
      }
  #endif
}

CBLAS_INLINE void TMatrixVectorStrideProduct(unsigned int r,unsigned int c,double *A,unsigned int s,double *b,double *o)
{
  #ifdef _CBLAS
    cblas_dgemv((ROW_MAJOR?CblasRowMajor:CblasColMajor),CblasTrans,r,c,1.0,A,(ROW_MAJOR?c:r),b,s,0.0,o,1);
  #else
    unsigned int i,j;

    for(i=0;i<c;i++)
      {
        o[i]=0.0;
        for(j=0;j<r;j++)
          o[i]+=(A[RC2INDEX(j,i,r,c)]*b[j*s]);
      }
  #endif
}

CBLAS_INLINE void MatrixMatrixProduct(unsigned int ra,unsigned int ca,double *A,
                                      unsigned int cb,double *B,double *C)
{
  #ifdef _CBLAS
    /* A is ra x ca
       B is ca x cb
       C is ra x cb */
    cblas_dgemm((ROW_MAJOR?CblasRowMajor:CblasColMajor),CblasNoTrans,CblasNoTrans,
                ra,cb,ca,1.0,A,(ROW_MAJOR?ca:ra),B,(ROW_MAJOR?cb:ra),0.0,C,(ROW_MAJOR?cb:ca));
  #else
    unsigned int i,j,k;
  
    /*  A * B */
    for(i=0;i<ra;i++)
      {
        for(j=0;j<cb;j++)
          {
            C[RC2INDEX(i,j,ra,cb)]=0.0;
            for(k=0;k<ca;k++)
              C[RC2INDEX(i,j,ra,cb)]+=(A[RC2INDEX(i,k,ra,ca)]*B[RC2INDEX(k,j,ca,cb)]);
        }
      }
  #endif
}

CBLAS_INLINE void TMatrixMatrixProduct(unsigned int ra,unsigned int ca,double *A,
                                       unsigned int cb,double *B,double *C)
{
  #ifdef _CBLAS
    /* A^t is ca x ra
       B   is ra x cb
       C   is ca x cb */
    cblas_dgemm((ROW_MAJOR?CblasRowMajor:CblasColMajor),CblasTrans,CblasNoTrans,
                ca,cb,ra,1.0,A,(ROW_MAJOR?ca:ra),B,(ROW_MAJOR?cb:ra),0.0,C,(ROW_MAJOR?cb:ca));
  #else
    unsigned int i,j,k;
  
    /*  A^t * B */
    for(i=0;i<ca;i++)
      {
        for(j=0;j<cb;j++)
          {
            C[RC2INDEX(i,j,ca,cb)]=0.0;
            for(k=0;k<ra;k++)
              C[RC2INDEX(i,j,ca,cb)]+=(A[RC2INDEX(k,i,ra,ca)]*B[RC2INDEX(k,j,ra,cb)]);
          }
      }
  #endif
}

double MinCosinusBetweenSubSpaces(unsigned int m,unsigned int k,double *T1,double *T2)
{
  unsigned int i;
  double c,cm;
  double *proj;
  #ifndef _CBLAS
    unsigned int s,j;
  #endif

  NEW(proj,k,double);
  
  cm=1; /*minimal cosinus*/
  for(i=0;i<k;i++)
    {
      #ifdef _CBLAS
        #if (ROW_MAJOR)
          TMatrixVectorStrideProduct(m,k,T1,k,&(T2[i]),proj);
        #else
          TMatrixVectorProduct(m,k,T1,&(T2[i*m]),proj);
        #endif
      #else
        for(j=0;j<k;j++)
          {
            proj[j]=0;
            for(s=0;s<m;s++)
              proj[j]+=(T1[RC2INDEX(s,j,m,k)]*T2[RC2INDEX(s,i,m,k)]);
          }
      #endif
      /* Assuming vector of T1 and T2 orthonormal, c is the cosinus of
         the i-th vector of basis T2 with respect to the subspace defined
         by T1. Note that in the projection the angle can not be larger than
         pi/2 and, therefore, the cosinus in in [0,1]*/
      c=Norm(k,proj);
      
      /* We search for the minimum cosinus = maximum angle. This is a measure
         of the difference between the two subspaces.*/
      if (c<cm) cm=c;
    }
    
  free(proj);

  return(cm);
}

CBLAS_INLINE void SubMatrixFromMatrix(unsigned int nr1,unsigned int nc1,double *m1,
                                      unsigned int nri,unsigned int nci,
                                      unsigned int nr,unsigned int nc,double *m)
{
  #ifdef _CBLAS
    unsigned int i,j,k;
    #if (ROW_MAJOR)
      if (nr1<nc1)
        {
          /* copy row by row */
          for(j=0,i=0,k=RC2INDEX(nri,nci,nr,nc);j<nr1;j++,i+=nc1,k+=nc)
            cblas_dcopy(nc1,&(m1[i]),1,&(m[k]),1);
        }
      else
        {
          /* copy column by column */
          for(i=0,k=RC2INDEX(nri,nci,nr,nc);i<nc1;i++,k++)
            cblas_dcopy(nr1,&(m1[i]),nc1,&(m[k]),nc);
        }
    #else
      if (nr1<nc1)
        {
          /* copy row by row */
          for(i=0,k=RC2INDEX(nri,nci,nr,nc);i<nr1;i++,k++)
            cblas_dcopy(nc1,&(m1[i]),nr1,&(m[k]),nr);
        }
      else
        {
          /* copy column by column */
          for(j=0,i=0,k=RC2INDEX(nri,nci,nr,nc);j<nc1;j++,i+=nr1,k+=nr)
            cblas_dcopy(nr1,&(m1[i]),1,&(m[k]),1);
        }
    #endif
  #else
    unsigned int i,j,k,l;
    
    for(i=0,k=nri;i<nr1;i++,k++)
      {
        for(j=0,l=nci;j<nc1;j++,l++)
          m[RC2INDEX(k,l,nr,nc)]=m1[RC2INDEX(i,j,nr1,nc1)];
      }
  #endif
}

CBLAS_INLINE void SubMatrixFromTMatrix(unsigned int nr1,unsigned int nc1,double *m1,
                                       unsigned int nri,unsigned int nci,
                                       unsigned int nr,unsigned int nc,double *m)
{ 
  #ifdef _CBLAS
    unsigned int i,j,k;
    #if (ROW_MAJOR)
      if (nc1<nr1)
        {
          /* copy row by row (of the transposed) */ 
          for(i=0,k=RC2INDEX(nri,nci,nr,nc);i<nc1;i++,k+=nc)
            cblas_dcopy(nr1,&(m1[i]),nc1,&(m[k]),1);
        }
      else
        {
          /* copy column by column (of the transposed) */
          for(j=0,i=0,k=RC2INDEX(nri,nci,nr,nc);j<nr1;j++,i+=nc1,k++)
            cblas_dcopy(nc1,&(m1[i]),1,&(m[k]),nc);
        }
    #else
      if (nc1<nr1)
        {
          /* copy row by row (of the transposed) */
          for(j=0,i=0,k=RC2INDEX(nri,nci,nr,nc);j<nc1;j++,i+=nr1,k++)
            cblas_dcopy(nr1,&(m1[i]),1,&(m[k]),nr);
        }
      else
        {
          /* copy column by column (of the transposed) */
          for(i=0,k=RC2INDEX(nri,nci,nr,nc);i<nr1;i++,k+=nr)
            cblas_dcopy(nc1,&(m1[i]),nr1,&(m[k]),1);
        }    
    #endif
  #else
    unsigned int i,j,k,l;
    
    for(i=0,k=nri;i<nc1;i++,k++)
      {
        for(j=0,l=nci;j<nr1;j++,l++)
          m[RC2INDEX(k,l,nr,nc)]=m1[RC2INDEX(j,i,nr1,nc1)];
      }
  #endif
}

void PrintVector(FILE *f,char *label,unsigned int n,double *v)
{
  unsigned int i;

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

void PrintMatrix(FILE *f,char *label,unsigned int r,unsigned int c,double *m)
{
  unsigned int i,j;

  if (label!=NULL)
    fprintf(f,"%s=",label);
  fprintf(f,"[ ");
  for(i=0;i<r;i++)
    {
      for(j=0;j<c;j++) 
        fprintf(f,"%.16f ",m[RC2INDEX(i,j,r,c)]);
      fprintf(f,"\n");
    }
  fprintf(f,"];\n");
}

void PrintTMatrix(FILE *f,char *label,unsigned int r,unsigned int c,double *m)
{
  unsigned int i,j;

  if (label!=NULL)
    fprintf(f,"%s=",label);
  fprintf(f,"[ ");
  for(i=0;i<c;i++)
    {
      for(j=0;j<r;j++) 
        fprintf(f,"%.16f ",m[RC2INDEX(j,i,r,c)]);
      fprintf(f,"\n");
    }
  fprintf(f,"];\n");
}