home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 7 / 07.iso / c / c129 / 1.ddi / MATMATH.C < prev    next >
Encoding:
C/C++ Source or Header  |  1989-04-11  |  11.9 KB  |  523 lines
  1. # include <math.h>
  2. # include <stdio.h>
  3. # include "complex.h" 
  4. # include <stdlib.h>
  5. # include "miscio.h"
  6.  
  7. # define nearzero 1.0E-20
  8.  
  9. float ComplexMag(struct complext *CNum1)
  10. {
  11.    float _fret;
  12.  
  13.       _fret = sqrt( (*CNum1).x * (*CNum1).x +
  14.               (*CNum1).y * (*CNum1).y );
  15.    return(_fret);
  16. }
  17.  
  18.  
  19. void CMath(struct complext c1,char op,
  20.                  struct complext c2,struct complext *result)
  21. {
  22.    struct complext temp1;
  23.    struct complext temp2;
  24.    float       tempr;
  25.  
  26.    switch ( op ) {
  27.       case '+':
  28.          {
  29.  
  30.             (*result).x = c1.x + c2.x;
  31.             (*result).y = c1.y + c2.y;
  32.  
  33.         }
  34.          break;
  35.       case '-':
  36.          {
  37.             (*result).x = c1.x - c2.x;
  38.             (*result).y = c1.y - c2.y;
  39.          }
  40.          break;
  41.       case '*':
  42.          {
  43.             (*result).x = c1.x * c2.x - c1.y * c2.y;
  44.             (*result).y = c1.x * c2.y + c1.y * c2.x;
  45.          }
  46.          break;
  47.       case '/':
  48.          {
  49.             (*result).x = (c1.x + c2.x + c1.y * c2.y) / ((c2.x * c2.x) + (c2.y * c2.y));
  50.             (*result).y = (c1.y * c2.x - c1.x * c2.y) / ((c2.x * c2.x) + (c2.y * c2.y));
  51.          }
  52.          break;
  53.       case 'i':
  54.          {
  55.             tempr = ComplexMag(&c1);
  56.             (*result).y = -(c1.y / tempr);
  57.             (*result).x = c1.x / tempr;
  58.          }
  59.          break;
  60.    }
  61. }
  62.  
  63. void MatPrint(float *m1,int m,int n,char *matname)
  64. {
  65.    int i;
  66.    int j;
  67.  
  68.    printf("%s\n", matname );
  69.    printf("\n");
  70.    for ( i = 0; i <= m-1; ++i ) {
  71.       for ( j = 0; j <= n-1; ++j ) {
  72.          printf("%10.3f  ", m1[i*n+j] );
  73.       }
  74.          printf("\n");
  75.    }
  76.    printf("\n");
  77. }
  78.  
  79.  
  80. void MatProd(float *m1,float *m2,int l,int m,
  81.              int n, float *m3)
  82. {
  83.    int i;
  84.    int j;
  85.    int k;
  86.  
  87.    for ( i = 0; i <= l-1; ++i ) {
  88.       for ( j = 0; j <= n-1; ++j ) {
  89.          m3[i*n+j] = 0;
  90.          for ( k = 0; k <= m-1; ++k ) {
  91.             m3[i*n+j] = m3[i*n+j] + m1[i*m+k] * m2[k*n+j];
  92.          }
  93.       }
  94.    }
  95. }
  96.  
  97. void MatScalarProd(float *m1, float sval,int numrow,
  98.                    int numcol, float *m2)
  99. {
  100.    int i;
  101.    int j;
  102.  
  103.    for ( i = 0; i <= numrow-1; ++i ) {
  104.       for ( j = 0; j <= numcol-1; ++j ) {
  105.          m2[i*numcol+j] = sval * m1[i*numcol+j];
  106.       }
  107.    }
  108. }
  109.  
  110. void MatAdd(float *m1, float *m2,int numrow,
  111.             int numcol,float *m3)
  112. {
  113.    int i;
  114.    int j;
  115.  
  116.    for ( i = 0; i <= numrow-1; ++i ) {
  117.       for ( j = 0; j <= numcol-1; ++j ) {
  118.          m3[i*numcol+j] = m1[i*numcol+j] + m2[i*numcol+j];
  119.       }
  120.    }
  121. }
  122.  
  123. void MatTranspose(float *m1,int numrow,int numcol,float *m2)
  124. {
  125.    int i;
  126.    int j;
  127.    float temp;
  128.  
  129.    for ( i = 0; i <= numrow-1; ++i ) {
  130.       for ( j = 0; j <= numcol-1; ++j ) {
  131.          m2[j*numrow+i] = m1[i*numcol+j];
  132.       }
  133.    }
  134. }
  135.  
  136. float MatDeter(float *m1, int numrow)
  137. {
  138.    float _fret;
  139.    int i;
  140.    int j;
  141.    int k;
  142.    int lx;
  143.    int f;
  144.    int nxt;
  145.    float pivot;
  146.    float rac;
  147.    float bignum;
  148.    float temp1;
  149.    float temp2;
  150.    float eval;
  151.  
  152.    f = 1;
  153.    for ( i = 0; i <= numrow-1; ++i ) {
  154.       bignum = 0.0;
  155.       for ( k = i; k <= numrow-1; ++k ) {
  156.          eval = fabs(m1[k*numrow+i]);
  157.          if ( eval - bignum > 0 ) {
  158.             bignum = eval;
  159.             lx = k;
  160.          }
  161.       }
  162.       if ( i - lx != 0 ) {
  163.          f = -f;
  164.       }
  165.       for ( j = 0; j <= numrow-1; ++j ) {
  166.          temp1 = m1[i*numrow+j];
  167.          m1[i*numrow+j] = m1[lx*numrow+j];
  168.          m1[lx*numrow+j] = temp1;
  169.       }
  170.       pivot = m1[i*numrow+i];
  171.       nxt = i + 1;
  172.       for ( j = nxt; j <= numrow-1; ++j ) {
  173.          rac = m1[j*numrow+i] / pivot;
  174.          for ( k = i; k <= numrow-1; ++k ) {
  175.             m1[j*numrow+k] = m1[j*numrow+k] - rac * m1[i*numrow+k];
  176.          }
  177.       }
  178.    }
  179.    temp2 = 1;
  180.    for ( i = 0; i <= numrow-1; ++i ) {
  181.       temp2 = temp2 * m1[i*numrow+i];
  182.    }
  183.    _fret = temp2 * f;
  184.    return(_fret);
  185. }
  186.  
  187. void matswap(float *s1,float *s2)
  188. {
  189.    float temp;
  190.  
  191.    temp = (*s1);
  192.    (*s1) = (*s2);
  193.    (*s2) = temp;
  194. }
  195.  
  196. void MatInvert(float *matdata, int numcol,float *det,
  197.                float *invary)
  198. {
  199.    int  *pivlst;
  200.    char *pivchk;
  201.    int i;
  202.    int j;
  203.    int k;
  204.    int l;
  205.    int lerow;
  206.    int lecol;
  207.    int l1;
  208.    float piv;
  209.    float t;
  210.    float leval;
  211.  
  212.    pivlst = (int *) calloc(50*2,2);
  213.    pivchk = (char *) calloc(50,1);
  214.    (*det) = 1;
  215.    for ( i = 0; i <= numcol-1; ++i ) {
  216.       pivchk[i] = 0;
  217.       for ( j = 0; j <= numcol-1; ++j ) {
  218.          invary[i*numcol+j] = matdata[i*numcol+j];
  219.       }
  220.    }
  221.    for ( i = 0; i <= numcol-1; ++i ) {
  222.       leval = 0.0;
  223.       for ( j = 0; j <= numcol-1; ++j ) {
  224.          if ( ! (pivchk[j]) ) {
  225.             for ( k = 0; k <= numcol-1; ++k ) {
  226.                if ( ! (pivchk[k]) ) {
  227.                   if ( fabs(invary[j*numcol+k]) > leval ) {
  228.                      lerow = j;
  229.                      lecol = k;
  230.                      leval = fabs(invary[j*numcol+k]);
  231.                   }
  232.                }
  233.             }
  234.          }
  235.       }
  236.       pivchk[lecol] = 1;
  237.       pivlst[i*2] = lerow;
  238.       pivlst[i*2+1] = lecol;
  239.       if ( lerow != lecol ) {
  240.          (*det) = -(*det);
  241.          for ( l = 0; l <= numcol-1; ++l ) {
  242.         matswap(&invary[lerow*numcol+l],&invary[lecol*numcol+l]);
  243.          }
  244.       }
  245.       piv = invary[lecol*numcol+lecol];
  246.       (*det) = (*det) * piv;
  247.       if ( (*det) > 1.0e+30 ) {
  248.          (*det) = 1;
  249.       }
  250.       invary[lecol*numcol+lecol] = 1.0;
  251.       for ( l = 0; l <= numcol-1; ++l ) {
  252.          invary[lecol*numcol+l] = invary[lecol*numcol+l] / piv;
  253.       }
  254.       for ( l1 = 0; l1 <= numcol-1; ++l1 ) {
  255.          if ( l1 != lecol ) {
  256.             t = invary[l1*numcol+lecol];
  257.             invary[l1*numcol+lecol] = 0;
  258.             for ( l = 0; l <= numcol-1; ++l ) {
  259.                invary[l1*numcol+l] = invary[l1*numcol+l] -
  260.                                      invary[lecol*numcol+l] * t;
  261.             }
  262.          }
  263.       }
  264.    }
  265.    for ( i = 0; i <= numcol-1; ++i ) {
  266.       l = numcol - i - 1;
  267.       if ( pivlst[l*2] != pivlst[l*2+1] ) {
  268.          lerow = pivlst[l*2];
  269.          lecol = pivlst[l*2+1];
  270.          for ( k = 0; k <= numcol-1; ++k ) {
  271.        matswap(&invary[k*numcol+lerow],&invary[k*numcol+lecol]);
  272.          }
  273.       }
  274.    }
  275.    free(pivlst); free(pivchk);
  276. }
  277.  
  278. void NormalizeEigenVectors(float *a,int n)
  279. {
  280.    int i;
  281.    int j;
  282.    float max;
  283.  
  284.    for ( j = 0; j <= n-1; ++j ) {
  285.       max = a[j];
  286.       for ( i = 1; i <= n-1; ++i ) {
  287.          if ( fabs(a[i*n+j]) > fabs(max) ) {
  288.             max = a[i*n+j];
  289.          }
  290.       }
  291.       for ( i = 0; i <= n-1; ++i ) {
  292.          a[i*n+j] = a[i*n+j] / max;
  293.       }
  294.    }
  295. }
  296.  
  297. void JacobiRotation(float *m, int p, int q,
  298.                    float *mp, float *mq, float *v,
  299.                    int n )
  300. {
  301.    float g;
  302.    float h;
  303.    float mpp;
  304.    float mqq;
  305.    float mpq;
  306.    float mpj;
  307.    float mqj;
  308.    float tempr;
  309.    float angle;
  310.    float c;
  311.    float s;
  312.    float cs;
  313.    float cSquared;
  314.    float sSquared;
  315.    int j;
  316. /*   float tau; */
  317.  
  318.    mpp = mp[p];
  319.    mpq = mp[q];
  320.    mqq = mq[q];
  321.  
  322.      if (fabs(mpp - mqq) > nearzero) {
  323.       angle = atan(2.0 * mpq / (mpp - mqq)) / 2.0;
  324.       if ( angle > M_PI_4 ) {
  325.          angle = angle - M_PI_2;
  326.       }
  327.    }
  328.    else {
  329.       if ( mpq > 0 ) {
  330.          angle = M_PI_4;
  331.       }
  332.       else {
  333.          angle = -M_PI_4;
  334.       }
  335.    }
  336.    c = cos(angle);
  337.    s = sin(angle);
  338.    cSquared = c * c;
  339.    sSquared = s * s;
  340.    cs = c * s;
  341.    tempr = 2.0 * mpq * cs;
  342.    mp[p] = mpp * cSquared + tempr + mqq * sSquared;
  343.    mq[q] = mpp * sSquared - tempr + mqq * cSquared;
  344.    mp[q] = 0.0;
  345.    mq[p] = 0.0;
  346.    /* tau = s / (1.0 + c); */
  347.    for ( j = 0; j <= n-1; ++j ) {
  348.       if (( j !=p ) && ( j != q)) {
  349.          mpj = mp[j] * c + mq[j] * s;
  350.          mqj = mq[j] * c - mp[j] * s;
  351.          mp[j] = mpj;
  352.          m[j*n+p] = mpj;
  353.          mq[j] = mqj;
  354.          m[j*n+q] = mqj;
  355.       }
  356.    }
  357.    for ( j = 0; j <= n-1; ++j ) {
  358.       g = v[j*n+p];
  359.       h = v[j*n+q];
  360.       v[j*n+p] = c * g + s * h;
  361.       v[j*n+q] = -s * g + c * h;
  362.    }
  363. }
  364.  
  365.  
  366. void CyclicJacobi(float *a,int n,float *eigenvalues,
  367.                   float *v,int *count,char *success)
  368. {
  369.    # define tolerance 1.0e-8
  370.    int maxCount;
  371.    int i;
  372.    int p;
  373.    int q;
  374.    float sumsq;
  375.    float *avp;
  376.    float *avq;
  377.  
  378.    avp = (float *) calloc(n*n,4);
  379.    avq = (float *) calloc(n*n,4);
  380.    maxCount = (*count);
  381.    (*count) = 0;
  382.    for ( p = 0; p <= n-1; ++p ) {
  383.       for ( q = 0; q <= n-1; ++q ) {
  384.          v[p*n+q] = 0.0;
  385.       }
  386.       v[p*n+p] = 1.0;
  387.    }
  388.    do {
  389.       (*count) = (*count) + 1;
  390.       sumsq = 0.0;
  391.       for ( p = 0; p <= n-1; ++p ) {
  392.          sumsq = sumsq + a[p*n+p] * a[p*n+p];
  393.       }
  394.       (*success) = 1;
  395.       for ( p = 0; p <= n - 2; ++p ) {
  396.          for ( q = p + 1; q <= n-1; ++q ) {
  397.             if ( fabs(a[p*n+q] / sumsq) > tolerance ) {
  398.                (*success) = 0;
  399.                JacobiRotation(a,p,q,&a[p*n],&a[q*n], v, n);
  400.             }
  401.          }
  402.       }
  403.    } while ( ! ((*success) || ((*count) > maxCount)) );
  404.    if ( (*success) ) {
  405.       for ( p = 0; p <= n-1; ++p ) {
  406.          eigenvalues[p] = a[p*n+p];
  407.       }
  408.    }
  409.    free(avp); free(avq);
  410. }
  411.  
  412.  
  413.  
  414. void CMatProd(struct complext *m1,struct complext *m2,int l,
  415.               int m,int n,struct complext *m3)
  416. {
  417.    int i;
  418.    int j;
  419.    int k;
  420.    struct complext temp;
  421.  
  422.    for ( i = 0; i <= l-1; ++i ) {
  423.       for ( j = 0; j <= n-1; ++j ) {
  424.          m3[i*n+j].x = 0.0;
  425.          m3[i*n+j].y = 0.0;
  426.          for ( k = 0; k <= m-1; ++k ) {
  427.             CMath(m1[i*m+k],'*',m2[k*n+j],&temp);
  428.             CMath(m3[i*n+j],'+',temp,&m3[i*n+j]);
  429.          }
  430.       }
  431.    }
  432. }
  433.  
  434. void CMatScalarProd(struct complext *m1,struct complext sval,int numrow,
  435.                     int numcol,struct complext *m2)
  436. {
  437.    int i;
  438.    int j;
  439.  
  440.    for ( i = 0; i <= numrow-1; ++i ) {
  441.       for ( j = 0; j <= numcol-1; ++j ) {
  442.          CMath(sval,'*',m1[i*numcol+j],&m2[i*numcol+j]);
  443.       }
  444.    }
  445. }
  446.  
  447. void CMatAdd(struct complext *m1,struct complext *m2,
  448.              int numrow,int numcol,struct complext *m3)
  449. {
  450.    int i;
  451.    int j;
  452.  
  453.    for ( i = 0; i <= numrow-1; ++i ) {
  454.       for ( j = 0; j <= numcol-1; ++j ) {
  455.          CMath(m1[i*numcol+j],'+',m2[i*numcol+j],&m3[i*numcol+j]);
  456.       }
  457.    }
  458. }
  459.  
  460. void CMatTranspose(struct complext *m1,int numrow,int numcol,
  461.                    struct complext *m2 )
  462. {
  463.    int i;
  464.    int j;
  465.    float temp;
  466.  
  467.    for ( i = 0; i <= numrow-1; ++i ) {
  468.       for ( j = 0; j <= numcol-1; ++j ) {
  469.          m2[j*numrow+i].x = m1[i*numcol+j].x;
  470.          m2[j*numrow+i].y = m1[i*numcol+j].y;
  471.       }
  472.    }
  473. }
  474.  
  475. void CMatInvert(struct complext *matdata,int numcol,float *det,
  476.                 struct complext *invary)
  477. {
  478.    float *inv;
  479.    float *coef;
  480.    int nn;
  481.    int i;
  482.    int j;
  483.    float de;
  484.  
  485.    inv = (float *) calloc(4*numcol*numcol,4);
  486.    coef = (float *) calloc(4*numcol*numcol,4);
  487.  
  488.    for ( i = 0; i <= numcol-1; ++i ) {
  489.       for ( j = 0; j <= numcol-1; ++j ) {
  490.          coef[(i * 2 )*2*numcol+(j * 2 )] = matdata[i*numcol +j].x;
  491.          coef[(i * 2 )*2*numcol +(j * 2+1)] = -matdata[i*numcol+j].y;
  492.          coef[(i * 2+1)*2*numcol +(j * 2 )] = matdata[i*numcol+j].y;
  493.          coef[(i * 2+1)*2*numcol +(j * 2+1)] = matdata[i*numcol +j].x;
  494.       }
  495.    }
  496.    nn = 2 * numcol;
  497.    MatInvert(coef,nn,&de,inv);
  498.    for ( i = 0; i <= numcol-1; ++i ) {
  499.       for ( j = 0; j <= numcol-1; ++j ) {
  500.          invary[i*numcol +j].x = inv[(i * 2)*2*numcol +(j * 2 )];
  501.          invary[i*numcol +j].y = inv[(i * 2+1)*2*numcol +(j * 2 )];
  502.       }
  503.    }
  504.    (*det) = de;
  505.    free(coef); free(inv);
  506. }
  507.  
  508. void CMatPrint(struct complext *m1,int m,int n,char *matname)
  509. {
  510.    int i;
  511.    int j;
  512.  
  513.    printf("%s\n", matname);
  514.    printf("\n");
  515.    for ( i = 0; i <= m-1; ++i ) {
  516.       for ( j = 0; j <= n-1; ++j ) {
  517.          printf("[%7.2f %7.2fj]",  m1[i*n+j].x, m1[i*n+j].y );
  518.       }
  519.       printf("\n");
  520.    }
  521.    printf("\n");
  522. }
  523.