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