idamnjanovic@60: /**************************************************************************
idamnjanovic@60:  *
idamnjanovic@60:  * File name: myblas.c
idamnjanovic@60:  *
idamnjanovic@60:  * Ron Rubinstein
idamnjanovic@60:  * Computer Science Department
idamnjanovic@60:  * Technion, Haifa 32000 Israel
idamnjanovic@60:  * ronrubin@cs
idamnjanovic@60:  *
idamnjanovic@60:  * Version: 1.1
idamnjanovic@60:  * Last updated: 13.8.2009
idamnjanovic@60:  *
idamnjanovic@60:  *************************************************************************/
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: #include "myblas.h"
idamnjanovic@60: #include <ctype.h>
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* find maximum of absolute values */
idamnjanovic@60: 
idamnjanovic@60: mwIndex maxabs(double c[], mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex maxid=0, k;
idamnjanovic@60:   double absval, maxval = SQR(*c);   /* use square which is quicker than absolute value */
idamnjanovic@60: 
idamnjanovic@60:   for (k=1; k<m; ++k) {
idamnjanovic@60:     absval = SQR(c[k]);
idamnjanovic@60:     if (absval > maxval) {
idamnjanovic@60:       maxval = absval;
idamnjanovic@60:       maxid = k;
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   return maxid;
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*x + y */
idamnjanovic@60: 
idamnjanovic@60: void vec_sum(double alpha, double x[], double y[], mwSize n)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<n; ++i) {
idamnjanovic@60:     y[i] += alpha*x[i];
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A*x */
idamnjanovic@60: 
idamnjanovic@60: void mat_vec(double alpha, double A[], double x[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j, i_n;
idamnjanovic@60:   double *Ax;
idamnjanovic@60: 
idamnjanovic@60:   Ax = mxCalloc(n,sizeof(double));
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     i_n = i*n;
idamnjanovic@60:     for (j=0; j<n; ++j) {
idamnjanovic@60:       Ax[j] += A[i_n+j] * x[i];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (j=0; j<n; ++j) {
idamnjanovic@60:     y[j] = alpha*Ax[j];
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   mxFree(Ax);
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A'*x */
idamnjanovic@60: 
idamnjanovic@60: void matT_vec(double alpha, double A[], double x[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j, n_i;
idamnjanovic@60:   double sum0, sum1, sum2, sum3;
idamnjanovic@60: 
idamnjanovic@60:   for (j=0; j<m; ++j) {
idamnjanovic@60:     y[j] = 0;
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   /* use loop unrolling to accelerate computation */
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     n_i = n*i;
idamnjanovic@60:     sum0 = sum1 = sum2 = sum3 = 0;
idamnjanovic@60:     for (j=0; j+4<n; j+=4) {
idamnjanovic@60:       sum0 += A[n_i+j]*x[j];
idamnjanovic@60:       sum1 += A[n_i+j+1]*x[j+1];
idamnjanovic@60:       sum2 += A[n_i+j+2]*x[j+2];
idamnjanovic@60:       sum3 += A[n_i+j+3]*x[j+3];
idamnjanovic@60:     }
idamnjanovic@60:     y[i] += alpha * ((sum0 + sum1) + (sum2 + sum3));
idamnjanovic@60:     while (j<n) {
idamnjanovic@60:       y[i] += alpha*A[n_i+j]*x[j];
idamnjanovic@60:       j++;
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A*x */
idamnjanovic@60: 
idamnjanovic@60: void mat_sp_vec(double alpha, double pr[], mwIndex ir[], mwIndex jc[], double x[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, j1, j2;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<n; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   j2 = jc[0];
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     j1 = j2; j2 = jc[i+1];
idamnjanovic@60:     for (j=j1; j<j2; ++j) {
idamnjanovic@60:       y[ir[j]] += alpha * pr[j] * x[i];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A'*x */
idamnjanovic@60: 
idamnjanovic@60: void matT_sp_vec(double alpha, double pr[], mwIndex ir[], mwIndex jc[], double x[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, j1, j2;
idamnjanovic@60:   
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   j2 = jc[0];
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     j1 = j2; j2 = jc[i+1];
idamnjanovic@60:     for (j=j1; j<j2; ++j) {
idamnjanovic@60:       y[i] += alpha * pr[j] * x[ir[j]];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A*x */
idamnjanovic@60: 
idamnjanovic@60: void mat_vec_sp(double alpha, double A[], double pr[], mwIndex ir[], mwIndex jc[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, j_n, k, kend;
idamnjanovic@60:   
idamnjanovic@60:   for (i=0; i<n; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   kend = jc[1];
idamnjanovic@60:   if (kend==0) {   /* x is empty */
idamnjanovic@60:     return;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (k=0; k<kend; ++k) {
idamnjanovic@60:     j = ir[k];
idamnjanovic@60:     j_n = j*n;
idamnjanovic@60:     for (i=0; i<n; ++i) {
idamnjanovic@60:       y[i] += alpha * A[i+j_n] * pr[k];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A'*x */
idamnjanovic@60: 
idamnjanovic@60: void matT_vec_sp(double alpha, double A[], double pr[], mwIndex ir[], mwIndex jc[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, j_n, k, kend;
idamnjanovic@60:   
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   kend = jc[1];
idamnjanovic@60:   if (kend==0) {   /* x is empty */
idamnjanovic@60:     return;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (j=0; j<m; ++j) {
idamnjanovic@60:     j_n = j*n;
idamnjanovic@60:     for (k=0; k<kend; ++k) {
idamnjanovic@60:       i = ir[k];
idamnjanovic@60:       y[j] += alpha * A[i+j_n] * pr[k];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A*x */
idamnjanovic@60: 
idamnjanovic@60: void mat_sp_vec_sp(double alpha, double pr[], mwIndex ir[], mwIndex jc[], double prx[], mwIndex irx[], mwIndex jcx[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, k, kend, j1, j2;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<n; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   kend = jcx[1]; 
idamnjanovic@60:   if (kend==0) {   /* x is empty */
idamnjanovic@60:     return;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (k=0; k<kend; ++k) {
idamnjanovic@60:     i = irx[k];
idamnjanovic@60:     j1 = jc[i]; j2 = jc[i+1];
idamnjanovic@60:     for (j=j1; j<j2; ++j) {
idamnjanovic@60:       y[ir[j]] += alpha * pr[j] * prx[k];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* compute y := alpha*A'*x */
idamnjanovic@60: 
idamnjanovic@60: void matT_sp_vec_sp(double alpha, double pr[], mwIndex ir[], mwIndex jc[], double prx[], mwIndex irx[], mwIndex jcx[], double y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i, j, k, jend, kend, jadd, kadd, delta;
idamnjanovic@60:   
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     y[i] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   kend = jcx[1];
idamnjanovic@60:   if (kend==0) {   /* x is empty */
idamnjanovic@60:     return;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i=0; i<m; ++i) {
idamnjanovic@60:     j = jc[i]; 
idamnjanovic@60:     jend = jc[i+1];
idamnjanovic@60:     k = 0;
idamnjanovic@60:     while (j<jend && k<kend) {
idamnjanovic@60:       
idamnjanovic@60:       delta = ir[j] - irx[k];
idamnjanovic@60:       
idamnjanovic@60:       if (delta) { /* if indices differ - increment the smaller one */
idamnjanovic@60:         jadd = delta<0;
idamnjanovic@60:         kadd = 1-jadd;
idamnjanovic@60:         j += jadd;
idamnjanovic@60:         k += kadd;
idamnjanovic@60:       }
idamnjanovic@60:       
idamnjanovic@60:       else {    /* indices are equal - add to result and increment both */
idamnjanovic@60:         y[i] += alpha * pr[j] * prx[k];
idamnjanovic@60:         j++; k++;
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* matrix-matrix multiplication */
idamnjanovic@60: 
idamnjanovic@60: void mat_mat(double alpha, double A[], double B[], double X[], mwSize n, mwSize m, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i1, i2, i3, iX, iA, i2_n;
idamnjanovic@60:   double b;
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<n*k; i1++) {
idamnjanovic@60:     X[i1] = 0;
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<m; ++i2) {
idamnjanovic@60:     i2_n = i2*n;
idamnjanovic@60:     iX = 0;
idamnjanovic@60:     for (i3=0; i3<k; ++i3) {
idamnjanovic@60:       iA = i2_n;
idamnjanovic@60:       b = B[i2+i3*m];
idamnjanovic@60:       for (i1=0; i1<n; ++i1) {
idamnjanovic@60:         X[iX++] += A[iA++]*b;
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<n*k; i1++) {
idamnjanovic@60:     X[i1] *= alpha;
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* matrix-transpose-matrix multiplication */
idamnjanovic@60: 
idamnjanovic@60: void matT_mat(double alpha, double A[], double B[], double X[], mwSize n, mwSize m, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i1, i2, i3, iX, iA, i2_n;
idamnjanovic@60:   double *x, sum0, sum1, sum2, sum3;
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<m; ++i2) {
idamnjanovic@60:     for (i3=0; i3<k; ++i3) {
idamnjanovic@60:       sum0 = sum1 = sum2 = sum3 = 0;
idamnjanovic@60:       for (i1=0; i1+4<n; i1+=4) {
idamnjanovic@60:         sum0 += A[i1+0+i2*n]*B[i1+0+i3*n];
idamnjanovic@60:         sum1 += A[i1+1+i2*n]*B[i1+1+i3*n];
idamnjanovic@60:         sum2 += A[i1+2+i2*n]*B[i1+2+i3*n];
idamnjanovic@60:         sum3 += A[i1+3+i2*n]*B[i1+3+i3*n];
idamnjanovic@60:       }
idamnjanovic@60:       X[i2+i3*m] = (sum0+sum1) + (sum2+sum3);
idamnjanovic@60:       while(i1<n) {
idamnjanovic@60:         X[i2+i3*m] += A[i1+i2*n]*B[i1+i3*n];
idamnjanovic@60:         i1++;
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<m*k; i1++) {
idamnjanovic@60:     X[i1] *= alpha;
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* tensor-matrix product */
idamnjanovic@60: 
idamnjanovic@60: void tens_mat(double alpha, double A[], double B[], double X[], mwSize n, mwSize m, mwSize k, mwSize l)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i1, i2, i3, i4, i2_n, nml;
idamnjanovic@60:   double b;
idamnjanovic@60:   
idamnjanovic@60:   nml = n*m*l;
idamnjanovic@60:   for (i1=0; i1<nml; ++i1) {
idamnjanovic@60:     X[i1] = 0;
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<m; ++i2) {
idamnjanovic@60:     i2_n = i2*n;
idamnjanovic@60:     for (i3=0; i3<k; ++i3) {
idamnjanovic@60:       for (i4=0; i4<l; ++i4) {
idamnjanovic@60:         b = B[i4+i3*l];
idamnjanovic@60:         for (i1=0; i1<n; ++i1) {
idamnjanovic@60:           X[i1 + i2_n + i4*n*m] += A[i1 + i2_n + i3*n*m] * b;
idamnjanovic@60:         }
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<nml; ++i1) {
idamnjanovic@60:     X[i1] *= alpha;
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* tensor-matrix-transpose product */
idamnjanovic@60: 
idamnjanovic@60: void tens_matT(double alpha, double A[], double B[], double X[], mwSize n, mwSize m, mwSize k, mwSize l)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i1, i2, i3, i4, i2_n, nml;
idamnjanovic@60:   double b;
idamnjanovic@60:   
idamnjanovic@60:   nml = n*m*l;
idamnjanovic@60:   for (i1=0; i1<nml; ++i1) {
idamnjanovic@60:     X[i1] = 0;
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<m; ++i2) {
idamnjanovic@60:     i2_n = i2*n;
idamnjanovic@60:     for (i4=0; i4<l; ++i4) {
idamnjanovic@60:       for (i3=0; i3<k; ++i3) {
idamnjanovic@60:         b = B[i3+i4*k];
idamnjanovic@60:         for (i1=0; i1<n; ++i1) {
idamnjanovic@60:           X[i1 + i2_n + i4*n*m] += A[i1 + i2_n + i3*n*m] * b;
idamnjanovic@60:         }
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<nml; ++i1) {
idamnjanovic@60:     X[i1] *= alpha;
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* dot product */
idamnjanovic@60: 
idamnjanovic@60: double dotprod(double a[], double b[], mwSize n)
idamnjanovic@60: {
idamnjanovic@60:   double sum = 0;
idamnjanovic@60:   mwIndex i;
idamnjanovic@60:   for (i=0; i<n; ++i)
idamnjanovic@60:     sum += a[i]*b[i];
idamnjanovic@60:   return sum;
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* find maximum of vector */
idamnjanovic@60: 
idamnjanovic@60: mwIndex maxpos(double c[], mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex maxid=0, k;
idamnjanovic@60:   double val, maxval = *c;
idamnjanovic@60: 
idamnjanovic@60:   for (k=1; k<m; ++k) {
idamnjanovic@60:     val = c[k];
idamnjanovic@60:     if (val > maxval) {
idamnjanovic@60:       maxval = val;
idamnjanovic@60:       maxid = k;
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   return maxid;
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* solve L*x = b */
idamnjanovic@60: 
idamnjanovic@60: void backsubst_L(double L[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j;
idamnjanovic@60:   double rhs;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<k; ++i) {
idamnjanovic@60:     rhs = b[i];
idamnjanovic@60:     for (j=0; j<i; ++j) {
idamnjanovic@60:       rhs -= L[j*n+i]*x[j];
idamnjanovic@60:     }
idamnjanovic@60:     x[i] = rhs/L[i*n+i];
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* solve L'*x = b */
idamnjanovic@60: 
idamnjanovic@60: void backsubst_Lt(double L[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j;
idamnjanovic@60:   double rhs;
idamnjanovic@60: 
idamnjanovic@60:   for (i=k; i>=1; --i) {
idamnjanovic@60:     rhs = b[i-1];
idamnjanovic@60:     for (j=i; j<k; ++j) {
idamnjanovic@60:       rhs -= L[(i-1)*n+j]*x[j];
idamnjanovic@60:     }
idamnjanovic@60:     x[i-1] = rhs/L[(i-1)*n+i-1];
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* solve U*x = b */
idamnjanovic@60: 
idamnjanovic@60: void backsubst_U(double U[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j;
idamnjanovic@60:   double rhs;
idamnjanovic@60: 
idamnjanovic@60:   for (i=k; i>=1; --i) {
idamnjanovic@60:     rhs = b[i-1];
idamnjanovic@60:     for (j=i; j<k; ++j) {
idamnjanovic@60:       rhs -= U[j*n+i-1]*x[j];
idamnjanovic@60:     }
idamnjanovic@60:     x[i-1] = rhs/U[(i-1)*n+i-1];
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* solve U'*x = b */
idamnjanovic@60: 
idamnjanovic@60: void backsubst_Ut(double U[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j;
idamnjanovic@60:   double rhs;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<k; ++i) {
idamnjanovic@60:     rhs = b[i];
idamnjanovic@60:     for (j=0; j<i; ++j) {
idamnjanovic@60:       rhs -= U[i*n+j]*x[j];
idamnjanovic@60:     }
idamnjanovic@60:     x[i] = rhs/U[i*n+i];
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* back substitution solver */
idamnjanovic@60: 
idamnjanovic@60: void backsubst(char ul, double A[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   if (tolower(ul) == 'u') {
idamnjanovic@60:     backsubst_U(A, b, x, n, k);
idamnjanovic@60:   }
idamnjanovic@60:   else if (tolower(ul) == 'l') {
idamnjanovic@60:     backsubst_L(A, b, x, n, k);
idamnjanovic@60:   }
idamnjanovic@60:   else {
idamnjanovic@60:     mexErrMsgTxt("Invalid triangular matrix type: must be ''U'' or ''L''");
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* solve equation set using cholesky decomposition */
idamnjanovic@60: 
idamnjanovic@60: void cholsolve(char ul, double A[], double b[], double x[], mwSize n, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   double *tmp;
idamnjanovic@60: 
idamnjanovic@60:   tmp = mxMalloc(k*sizeof(double));
idamnjanovic@60: 
idamnjanovic@60:   if (tolower(ul) == 'l') {
idamnjanovic@60:     backsubst_L(A, b, tmp, n, k);
idamnjanovic@60:     backsubst_Lt(A, tmp, x, n, k);
idamnjanovic@60:   }
idamnjanovic@60:   else if (tolower(ul) == 'u') {
idamnjanovic@60:     backsubst_Ut(A, b, tmp, n, k);
idamnjanovic@60:     backsubst_U(A, tmp, x, n, k);
idamnjanovic@60:   }
idamnjanovic@60:   else {
idamnjanovic@60:     mexErrMsgTxt("Invalid triangular matrix type: must be either ''U'' or ''L''");
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   mxFree(tmp);
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* perform a permutation assignment y := x(ind(1:k)) */
idamnjanovic@60: 
idamnjanovic@60: void vec_assign(double y[], double x[], mwIndex ind[], mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i;
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<k; ++i)
idamnjanovic@60:     y[i] = x[ind[i]];
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* matrix transpose */
idamnjanovic@60: 
idamnjanovic@60: void transpose(double X[], double Y[], mwSize n, mwSize m)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex i, j, i_m, j_n;
idamnjanovic@60:   
idamnjanovic@60:   if (n<m) {
idamnjanovic@60:     for (j=0; j<m; ++j) {
idamnjanovic@60:       j_n = j*n;
idamnjanovic@60:       for (i=0; i<n; ++i) {
idamnjanovic@60:         Y[j+i*m] = X[i+j_n];
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   else {
idamnjanovic@60:     for (i=0; i<n; ++i) {
idamnjanovic@60:       i_m = i*m;
idamnjanovic@60:       for (j=0; j<m; ++j) {
idamnjanovic@60:         Y[j+i_m] = X[i+j*n];
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* print contents of matrix */
idamnjanovic@60: 
idamnjanovic@60: void printmat(double A[], int n, int m, char* matname)
idamnjanovic@60: {
idamnjanovic@60:   int i, j;
idamnjanovic@60:   mexPrintf("\n%s = \n\n", matname);
idamnjanovic@60: 
idamnjanovic@60:   if (n*m==0) {
idamnjanovic@60:     mexPrintf("   Empty matrix: %d-by-%d\n\n", n, m);
idamnjanovic@60:     return;
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<n; ++i) {
idamnjanovic@60:     for (j=0; j<m; ++j)
idamnjanovic@60:       mexPrintf("   %lf", A[j*n+i]);
idamnjanovic@60:     mexPrintf("\n");
idamnjanovic@60:   }
idamnjanovic@60:   mexPrintf("\n");
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* print contents of sparse matrix */
idamnjanovic@60: 
idamnjanovic@60: void printspmat(mxArray *a, char* matname)
idamnjanovic@60: {
idamnjanovic@60:   mwIndex *aJc = mxGetJc(a);
idamnjanovic@60:   mwIndex *aIr = mxGetIr(a);
idamnjanovic@60:   double *aPr = mxGetPr(a);
idamnjanovic@60: 
idamnjanovic@60:   int i;
idamnjanovic@60: 
idamnjanovic@60:   mexPrintf("\n%s = \n\n", matname);
idamnjanovic@60: 
idamnjanovic@60:   for (i=0; i<aJc[1]; ++i)
idamnjanovic@60:     printf("   (%d,1) = %lf\n", aIr[i]+1,aPr[i]);
idamnjanovic@60: 
idamnjanovic@60:   mexPrintf("\n");
idamnjanovic@60: }
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: /* matrix multiplication using Winograd's algorithm */
idamnjanovic@60: 
idamnjanovic@60: /*
idamnjanovic@60: void mat_mat2(double alpha, double A[], double B[], double X[], mwSize n, mwSize m, mwSize k)
idamnjanovic@60: {
idamnjanovic@60:   
idamnjanovic@60:   mwIndex i1, i2, i3, iX, iA, i2_n;
idamnjanovic@60:   double b, *AA, *BB;
idamnjanovic@60:   
idamnjanovic@60:   AA = mxCalloc(n,sizeof(double));
idamnjanovic@60:   BB = mxCalloc(k,sizeof(double));
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<n*k; i1++) {
idamnjanovic@60:     X[i1] = 0;
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i1=0; i1<n; ++i1) {
idamnjanovic@60:     for (i2=0; i2<m/2; ++i2) {
idamnjanovic@60:       AA[i1] += A[i1+2*i2*n]*A[i1+(2*i2+1)*n];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<k; ++i2) {
idamnjanovic@60:     for (i1=0; i1<m/2; ++i1) {
idamnjanovic@60:       BB[i2] += B[2*i1+i2*m]*B[2*i1+1+i2*m];
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60: 
idamnjanovic@60:   for (i2=0; i2<k; ++i2) {
idamnjanovic@60:     for (i3=0; i3<m/2; ++i3) {
idamnjanovic@60:       for (i1=0; i1<n; ++i1) {
idamnjanovic@60:         X[i1+i2*n] += (A[i1+(2*i3)*n]+B[2*i3+1+i2*m])*(A[i1+(2*i3+1)*n]+B[2*i3+i2*m]);
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   if (m%2) {
idamnjanovic@60:     for (i2=0; i2<k; ++i2) {
idamnjanovic@60:       for (i1=0; i1<n; ++i1) {
idamnjanovic@60:         X[i1+i2*n] += A[i1+(m-1)*n]*B[m-1+i2*m];
idamnjanovic@60:       }
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   for (i2=0; i2<k; ++i2) {
idamnjanovic@60:     for (i1=0; i1<n; ++i1) {
idamnjanovic@60:       X[i1+i2*n] -= (AA[i1] + BB[i2]);
idamnjanovic@60:       X[i1+i2*n] *= alpha;
idamnjanovic@60:     }
idamnjanovic@60:   }
idamnjanovic@60:   
idamnjanovic@60:   mxFree(AA);
idamnjanovic@60:   mxFree(BB);
idamnjanovic@60: }
idamnjanovic@60: */
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: 
idamnjanovic@60: