atl_ctrsmk.c

来自「基于Blas CLapck的.用过的人知道是干啥的」· C语言 代码 · 共 1,052 行 · 第 1/2 页

      t0 = xr2;                       ATL_pfl1W(pB1+PFD);
      xr2 = xr2*ar22 - xi2*ai22;
      xi2 = t0 *ai22 + xi2*ar22;

      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
   }
   #undef PFD
}

static void trsmRU_3(const int M, const TYPE *A, TYPE *B, const int ldb)
/*
 * 'Right', 'Upper', written with all dependencies shown, so that the
 * compiler can optimize.  A is known to be 3x3, with 1/alpha already applied,
 * diagonals already inverted.
 */
{
   const TYPE ar11=*A, ai11=A[1], ar12=A[6], ai12=A[7], ar13=A[12], ai13=A[13];
   const TYPE ar22=A[ 8], ai22=A[ 9], ar23=A[14], ai23=A[15];
   const TYPE ar33=A[16], ai33=A[17];
   const int ldb2 = ldb+ldb;
   TYPE xr1, xi1, xr2, xi2, xr3, xi3, t0;
   TYPE *pB0=B, *pB1 = B+ldb2, *pB2=pB1+ldb2;
   int i;
   #define PFD 8

   for (i=M; i; i--)
   {
      xr1 = *pB0; xr2 = *pB1; xr3 = *pB2;
      xi1 = pB0[1]; xi2 = pB1[1]; xi3 = pB2[1];
/*
 *    real sequence:
 *    x1 *= a11;
 *    x2 = (x2 - x1*a12) * a22;
 *    x3 = (x3 - x1*a13 - x2*a23) * a33;
 */
      t0 = xr1;
      xr1 = xr1*ar11 - xi1*ai11;
      xi1 = t0 *ai11 + xi1*ar11;

      xr2 -= xr1*ar12 - xi1*ai12;
      xi2 -= xr1*ai12 + xi1*ar12;
      t0 = xr2;
      xr2 = xr2*ar22 - xi2*ai22;     ATL_pfl1W(pB0+PFD);
      xi2 = t0 *ai22 + xi2*ar22;     ATL_pfl1W(pB1+PFD);

      xr3 -= xr1*ar13 - xi1*ai13;
      xi3 -= xr1*ai13 + xi1*ar13;
      xr3 -= xr2*ar23 - xi2*ai23;
      xi3 -= xr2*ai23 + xi2*ar23;     ATL_pfl1W(pB2+PFD);
      t0 = xr3;
      xr3 = xr3*ar33 - xi3*ai33;
      xi3 = t0 *ai33 + xi3*ar33;

      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
      *pB2 = xr3; pB2[1] = xi3; pB2 += 2;
   }
   #undef PFD
}

static void trsmRU_4(const int M, const TYPE *A, TYPE *B, const int ldb)
/*
 * 'Right', 'Upper', written with all dependencies shown, so that the
 * compiler can optimize.  A is known to be 4x4, with 1/alpha already applied,
 * diagonals already inverted.
 */
{
   const TYPE ar11=*A, ai11=A[1], ar12=A[8], ai12=A[9], ar13=A[16], ai13=A[17],
              ar14=A[24], ai14=A[25];
   const TYPE ar22=A[10], ai22=A[11], ar23=A[18], ai23=A[19],
              ar24=A[26], ai24=A[27];
   const TYPE ar33=A[20], ai33=A[21], ar34=A[28], ai34=A[29];
   const TYPE ar44=A[30], ai44=A[31];
   const int ldb2 = ldb+ldb;
   TYPE xr1, xi1, xr2, xi2, xr3, xi3, xr4, xi4, t0;
   TYPE *pB0=B, *pB1 = B+ldb2, *pB2=pB1+ldb2, *pB3=pB2+ldb2;
   int i;
   #define PFD 8

   for (i=M; i; i--)
   {
      xr1 = *pB0; xr2 = *pB1; xr3 = *pB2; xr4 = *pB3;
      xi1 = pB0[1]; xi2 = pB1[1]; xi3 = pB2[1]; xi4 = pB3[1];
/*
 *    real sequence:
 *    x1 *= a11;
 *    x2 = (x2 - x1*a12) * a22;
 *    x3 = (x3 - x1*a13 - x2*a23) * a33;
 *    x4 = (x4 - x1*a14 - x2*a24 - x3*a34) * a44;
 */
      t0 = xr1;
      xr1 = xr1*ar11 - xi1*ai11;
      xi1 = t0 *ai11 + xi1*ar11;

      xr2 -= xr1*ar12 - xi1*ai12;
      xi2 -= xr1*ai12 + xi1*ar12;
      t0 = xr2;
      xr2 = xr2*ar22 - xi2*ai22;
      xi2 = t0 *ai22 + xi2*ar22;

      xr3 -= xr1*ar13 - xi1*ai13;
      xi3 -= xr1*ai13 + xi1*ar13;
      xr3 -= xr2*ar23 - xi2*ai23;     ATL_pfl1W(pB0+PFD);
      xi3 -= xr2*ai23 + xi2*ar23;     ATL_pfl1W(pB1+PFD);
      t0 = xr3;
      xr3 = xr3*ar33 - xi3*ai33;
      xi3 = t0 *ai33 + xi3*ar33;

      xr4 -= xr1*ar14 - xi1*ai14;
      xi4 -= xr1*ai14 + xi1*ar14;
      xr4 -= xr2*ar24 - xi2*ai24;
      xi4 -= xr2*ai24 + xi2*ar24;
      xr4 -= xr3*ar34 - xi3*ai34;     ATL_pfl1W(pB2+PFD);
      xi4 -= xr3*ai34 + xi3*ar34;     ATL_pfl1W(pB3+PFD);
      t0 = xr4;
      xr4 = xr4*ar44 - xi4*ai44;
      xi4 = t0 *ai44 + xi4*ar44;

      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
      *pB2 = xr3; pB2[1] = xi3; pB2 += 2;
      *pB3 = xr4; pB3[1] = xi4; pB3 += 2;
   }
   #undef PFD
}

static void trsmRL_2(const int M, const TYPE *A, TYPE *B, const int ldb)
/*
 * 'Right', 'Lower', written with all dependencies shown, so that the
 * compiler can optimize.  A is known to be 2x2, with 1/alpha already applied,
 * diagonals already inverted.
 */
{
   const TYPE ar11=*A, ai11=A[1], ar21=A[2], ai21=A[3];
   const TYPE ar22=A[6], ai22=A[7];
   const int ldb2 = ldb+ldb;
   TYPE *pB0=B, *pB1 = B+ldb2;
   TYPE xr1, xi1, xr2, xi2, t0;
   int i;
   #define PFD 8

   for (i=M; i; i--)
   {
      xr2 = *pB1; xr1 = *pB0;
      xi2 = pB1[1]; xi1 = pB0[1];
/*
 *    REAL SEQUENCE :
 *
 *    x2 *= a22;
 *    x1 = (x1 - x2*a21) * a11;
 */
      t0 = xr2;
      xr2 = xr2*ar22 - xi2*ai22;
      xi2 = t0 *ai22 + xi2*ar22;

      xr1 -= xr2*ar21 - xi2*ai21; ATL_pfl1W(pB1+PFD);
      xi1 -= xr2*ai21 + xi2*ar21; ATL_pfl1W(pB0+PFD);
      t0 = xr1;
      xr1 = xr1*ar11 - xi1*ai11;
      xi1 = t0 *ai11 + xi1*ar11;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
   }
   #undef PFD
}
static void trsmRL_3(const int M, const TYPE *A, TYPE *B, const int ldb)
/*
 * 'Right', 'Lower', written with all dependencies shown, so that the
 * compiler can optimize.  A is known to be 3x3, with 1/alpha already applied,
 * diagonals already inverted.
 */
{
   const TYPE ar11=*A, ai11=A[1], ar21=A[2], ai21=A[3], ar31=A[4], ai31=A[5];
   const TYPE ar22=A[ 8], ai22=A[ 9], ar32=A[10], ai32=A[11];
   const TYPE ar33=A[16], ai33=A[17];
   const int ldb2 = ldb+ldb;
   TYPE xr1, xi1, xr2, xi2, xr3, xi3, t0;
   TYPE *pB0=B, *pB1 = B+ldb2, *pB2=pB1+ldb2;
   int i;
   #define PFD 8

   for (i=M; i; i--)
   {
      xr3 = *pB2; xr2 = *pB1; xr1 = *pB0;
      xi3 = pB2[1]; xi2 = pB1[1]; xi1 = pB0[1];
/*
 *    REAL SEQUENCE :
 *
 *    x3 *= a33;
 *    x2 = (x2 - x3*a32) * a22;
 *    x1 = (x1 - x3*a31 - x2*a21) * a11;
 */
      t0 = xr3;
      xr3 = ar33*xr3 - ai33*xi3;
      xi3 = ar33*xi3 + ai33*t0;

      xr2 -= xr3*ar32 - xi3*ai32;
      xi2 -= xr3*ai32 + xi3*ar32;
      t0 = xr2;
      xr2 = xr2*ar22 - xi2*ai22; ATL_pfl1W(pB2+PFD);
      xi2 = t0 *ai22 + xi2*ar22; ATL_pfl1W(pB1+PFD);

      xr1 -= xr3*ar31 - xi3*ai31;
      xi1 -= xr3*ai31 + xi3*ar31;
      xr1 -= xr2*ar21 - xi2*ai21;
      xi1 -= xr2*ai21 + xi2*ar21; /* ATL_pfl1W(pB0+PFD); */
      t0 = xr1;
      xr1 = xr1*ar11 - xi1*ai11;
      xi1 = t0 *ai11 + xi1*ar11;
      *pB2 = xr3; pB2[1] = xi3; pB2 += 2;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
   }
   #undef PFD
}

static void trsmRL_4(const int M, const TYPE *A, TYPE *B, const int ldb)
/*
 * 'Right', 'Lower', written with all dependencies shown, so that the
 * compiler can optimize.  A is known to be 4x4, with 1/alpha already applied,
 * diagonals already inverted.
 */
{
   const TYPE ar11=*A, ai11=A[1], ar21=A[2], ai21=A[3], ar31=A[4], ai31=A[5],
              ar41=A[6], ai41=A[7];
   const TYPE ar22=A[10], ai22=A[11], ar32=A[12], ai32=A[13],
              ar42=A[14], ai42=A[15];
   const TYPE ar33=A[20], ai33=A[21], ar43=A[22], ai43=A[23];
   const TYPE ar44=A[30], ai44=A[31];
   const int ldb2 = ldb+ldb;
   TYPE xr1, xi1, xr2, xi2, xr3, xi3, xr4, xi4, t0;
   TYPE *pB0=B, *pB1 = B+ldb2, *pB2=pB1+ldb2, *pB3=pB2+ldb2;
   int i;
   #define PFD 8

   for (i=M; i; i--)
   {
      xr4 = *pB3; xr3 = *pB2; xr2 = *pB1; xr1 = *pB0;
      xi4 = pB3[1]; xi3 = pB2[1]; xi2 = pB1[1]; xi1 = pB0[1];

/*
 *    REAL SEQUENCE :
 *
 *    x4 *= a11;
 *    x3 = (x3 - x4*a43) * a33;
 *    x2 = (x2 - x4*a42 - x3*a32) * a22;
 *    x1 = (x1 - x4*a41 - x3*a31 - x2*a21) * a11;
 */
      t0 = xr4;
      xr4 = ar44*xr4 - ai44*xi4;
      xi4 = ar44*xi4 + ai44*t0;

      xr3 -= xr4*ar43 - xi4*ai43;
      xi3 -= xr4*ai43 + xi4*ar43;
      t0 = xr3;
      xr3 = ar33*xr3 - ai33*xi3;
      xi3 = ar33*xi3 + ai33*t0;

      xr2 -= xr4*ar42 - xi4*ai42;
      xi2 -= xr4*ai42 + xi4*ar42; ATL_pfl1W(pB3+PFD);
      xr2 -= xr3*ar32 - xi3*ai32; ATL_pfl1W(pB2+PFD);
      xi2 -= xr3*ai32 + xi3*ar32;
      t0 = xr2;
      xr2 = xr2*ar22 - xi2*ai22;
      xi2 = t0 *ai22 + xi2*ar22;

      xr1 -= xr4*ar41 - xi4*ai41;
      xi1 -= xr4*ai41 + xi4*ar41;
      xr1 -= xr3*ar31 - xi3*ai31;
      xi1 -= xr3*ai31 + xi3*ar31;
      xr1 -= xr2*ar21 - xi2*ai21; ATL_pfl1W(pB1+PFD);
      xi1 -= xr2*ai21 + xi2*ar21; ATL_pfl1W(pB0+PFD);
      t0 = xr1;
      xr1 = xr1*ar11 - xi1*ai11;
      xi1 = t0 *ai11 + xi1*ar11;
      *pB3 = xr4; pB3[1] = xi4; pB3 += 2;
      *pB2 = xr3; pB2[1] = xi3; pB2 += 2;
      *pB1 = xr2; pB1[1] = xi2; pB1 += 2;
      *pB0 = xr1; pB0[1] = xi1; pB0 += 2;
   }
   #undef PFD
}
#endif

static void trsmcpUN
   (const int N, const TYPE *alpha, const TYPE *A, const int lda, TYPE *C)
/*
 * copies Upper, NoTranspose matrix to Upper, NoTrans matrix, applying 1/alpha
 */
{
   const int lda2 = lda+lda, N2 = N+N;
   const TYPE *a=A+lda2;
   TYPE *c=C+N2;
   TYPE tmp[2];
   int j;

   tmp[0] = *alpha; tmp[1] = alpha[1];
   Mjoin(PATL,cplxinvert)(1, tmp, 2, tmp, 2); /* safe cplx inversion */
/*
 * copy non-diagonal portion of matrix, scaled by 1/alpha
 */
   for (j=1; j < N; j++)
   {
      Mjoin(PATL,cpsc)(j, tmp, a, 1, c, 1);
      a += lda2;
      c += N2;
   }
}

static void trsmcpUT
   (const enum ATLAS_TRANS Trans, const int N, const TYPE *alpha,
    const TYPE *A, const int lda, TYPE *C)
/*
 * copies Upper, Transpose matrix to Lower, NoTrans matrix, applying 1/alpha
 */
{
   const int lda2 = lda+lda;
   const TYPE *a=A+lda2;
   TYPE *c=C+2;
   TYPE tmp[2];
   int j;
   void (*move)(const int N, const SCALAR alpha, const TYPE *X, const int incX,
                TYPE *Y, const int incY);

   if (Trans == AtlasConjTrans) move = Mjoin(PATL,moveConj);
   else move = Mjoin(PATL,cpsc);

   tmp[0] = *alpha; tmp[1] = alpha[1];
   Mjoin(PATL,cplxinvert)(1, tmp, 2, tmp, 2); /* safe cplx inversion */
/*
 * copy non-diagonal portion of matrix, scaled by 1/alpha
 */
   for (j=1; j < N; j++)
   {
      move(j, tmp, a, 1, c, N);
      a += lda2;
      c += 2;
   }
}

static void trsmcpLT(const enum ATLAS_TRANS Trans, const int N,
                     const TYPE *alpha, const TYPE *A, const int lda, TYPE *C)
/*
 * copies Lower, Trans matrix to Upper, Notrans matrix, applies 1/alpha
 */
{
   const int lda2 = lda+lda+2, N2 = N+N+2;
   const TYPE *a=A+2;
   TYPE *c=C+N+N;
   TYPE tmp[2];
   int j;
   void (*move)(const int N, const SCALAR alpha, const TYPE *X, const int incX,
                TYPE *Y, const int incY);

   if (Trans == AtlasConjTrans) move = Mjoin(PATL,moveConj);
   else move = Mjoin(PATL,cpsc);

   tmp[0] = *alpha; tmp[1] = alpha[1];
   Mjoin(PATL,cplxinvert)(1, tmp, 2, tmp, 2); /* safe cplx inversion */
/*
 * copy non-diagonal portion of matrix, scaled by 1/alpha
 */
   for (j=0; j != N; j++)
   {
      move(N-j-1, tmp, a, 1, c, N);
      a += lda2;
      c += N2;
   }
}
static void trsmcpLN(const int N, const TYPE *alpha, const TYPE *A,
                     const int lda, TYPE *C)
/*
 * copies Lower, NoTrans matrix to Lower, Notrans matrix, applies 1/alpha
 */
{
   const int lda2 = lda+lda+2, N2 = N+N+2;
   const TYPE *a=A+2;
   TYPE *c=C+2;
   TYPE tmp[2];
   int j;

   tmp[0] = *alpha; tmp[1] = alpha[1];
   Mjoin(PATL,cplxinvert)(1, tmp, 2, tmp, 2); /* safe cplx inversion */
/*
 * copy non-diagonal portion of matrix, scaled by 1/alpha
 */
   for (j=0; j != N; j++)
   {
      Mjoin(PATL,cpsc)(N-j-1, tmp, a, 1, c, 1);
      a += lda2;
      c += N2;
   }
}

static enum ATLAS_DIAG trsmcopy
   (enum ATLAS_UPLO Uplo, enum ATLAS_TRANS Trans, enum ATLAS_DIAG Diag,
    const int N, const TYPE *alpha, const TYPE *A, const int lda, TYPE *C)
{
   enum ATLAS_DIAG diag=AtlasNonUnit;
   if (Uplo == AtlasLower)
   {
      if (Trans == AtlasNoTrans) trsmcpLN(N, alpha, A, lda, C);
      else trsmcpLT(Trans, N, alpha, A, lda, C);
   }
   else
   {
      if (Trans == AtlasNoTrans) trsmcpUN(N, alpha, A, lda, C);
      else trsmcpUT(Trans, N, alpha, A, lda, C);
   }
/*
 * Handle main diagonal of matrix
 */
   if (Diag == AtlasUnit)
   {
      if (*alpha == ATL_rone && alpha[1] == ATL_rzero) diag = AtlasUnit;
      Mjoin(PATLU,set)(N, *alpha, C, N+N+2);
      Mjoin(PATLU,set)(N, alpha[1], C+1, N+N+2);
   }
   else
   {
      if (Trans != AtlasConjTrans) Mjoin(PATL,copy)(N, A, lda+1, C, N+1);
      else Mjoin(PATL,copyConj)(N, A, lda+1, C, N+1);
      Mjoin(PATL,cplxinvert)(N, C, N+N+2, C, N+N+2); /* safe cplx inversion */
      Mjoin(PATL,scal)(N, alpha, C, N+1);
   }
   return(diag);
}

/*
 * In unbelievably beautiful kludge from the depths of hell, reuse the
 * 'Left' code for 'Right' by swapping M & N, and renaming routs; if you
 * must be ill, please don't get it on the code . . .
 */
#ifdef Right_
   #define trsmLU_4 trsmRU_4
   #define trsmLU_3 trsmRU_3
   #define trsmLU_2 trsmRU_2
   #define trsmLL_4 trsmRL_4
   #define trsmLL_3 trsmRL_3
   #define trsmLL_2 trsmRL_2
void Mjoin(PATL,CtrsmKR)
   (enum ATLAS_UPLO Uplo, enum ATLAS_TRANS Trans, enum ATLAS_DIAG Diag,
    const int N, const int M, const SCALAR alpha, const TYPE *A, const int lda,
    TYPE *B, const int ldb)
#else
void Mjoin(PATL,CtrsmKL)
   (enum ATLAS_UPLO Uplo, enum ATLAS_TRANS Trans, enum ATLAS_DIAG Diag,
    const int M, const int N, const SCALAR alpha, const TYPE *A, const int lda,
    TYPE *B, const int ldb)
#endif
{
   TYPE tmp[2], ra, ia;
   void *vp;
   TYPE *a;

   if (N > 0)
   {
      if (M > 1)
      {
         vp = malloc(ATL_Cachelen + ATL_MulBySize(M)*M);
         ATL_assert(vp);
         a = ATL_AlignPtr(vp);
         Diag = trsmcopy(Uplo, Trans, Diag, M, alpha, A, lda, a);
         if (Trans != AtlasNoTrans)
         {
            if (Uplo == AtlasLower) Uplo = AtlasUpper;
            else Uplo = AtlasLower;
         }
         switch(M)
         {
         case 2:
            if (Uplo == AtlasLower) trsmLL_2(N, a, B, ldb);
            else trsmLU_2(N, a, B, ldb);
            break;
         case 3:
            if (Uplo == AtlasLower) trsmLL_3(N, a, B, ldb);
            else trsmLU_3(N, a, B, ldb);
            break;
         case 4:
            if (Uplo == AtlasLower) trsmLL_4(N, a, B, ldb);
            else trsmLU_4(N, a, B, ldb);
            break;
         default: /* this crap should never be used */
               tmp[0] = ATL_rone; tmp[1] = ATL_rzero;
               Mjoin(PATL,cplxinvert)(M, a, M+M+2, a, M+M+2);
               Mjoin(PATL,reftrsm)(AtlasLeft, Uplo, AtlasNoTrans, Diag, M, N,
                                   tmp, a, M, B, ldb);
         }
         free(vp);
      }
      else if (M == 1)
      {
         if (Diag == AtlasUnit)
         #ifdef Right_
            Mjoin(PATL,scal)(N, alpha, B, 1);
         #else
            Mjoin(PATL,scal)(N, alpha, B, ldb);
         #endif
         else
         {
            tmp[0] = A[0];
            if (Trans != AtlasConjTrans) tmp[1] = A[1];
            else tmp[1] = -A[1];
            Mjoin(PATL,cplxinvert)(1, tmp, 2, tmp, 2); /* safe cplx invers */
            ra = tmp[0]; ia = tmp[1];
            tmp[0] = *alpha * ra - alpha[1] * ia;
            tmp[1] = *alpha * ia + alpha[1] * ra;
            #ifdef Right_
               Mjoin(PATL,scal)(N, tmp, B, 1);
            #else
               Mjoin(PATL,scal)(N, tmp, B, ldb);
            #endif
         }
      }
   }
}
#ifdef Right_
   #undef  trsmLU_4
   #undef  trsmLU_3
   #undef  trsmLU_2
   #undef  trsmLL_4
   #undef  trsmLL_3
   #undef  trsmLL_2
#endif