home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / mfunc < prev    next >
Encoding:
Text File  |  1994-01-13  |  8.8 KB  |  392 lines
  1.  
  2. /**************************************************************************
  3. **
  4. ** Copyright (C) 1993 David E. Steward & Zbigniew Leyk, all rights reserved.
  5. **
  6. **                 Meschach Library
  7. ** 
  8. ** This Meschach Library is provided "as is" without any express 
  9. ** or implied warranty of any kind with respect to this software. 
  10. ** In particular the authors shall not be liable for any direct, 
  11. ** indirect, special, incidental or consequential damages arising 
  12. ** in any way from use of the software.
  13. ** 
  14. ** Everyone is granted permission to copy, modify and redistribute this
  15. ** Meschach Library, provided:
  16. **  1.  All copies contain this copyright notice.
  17. **  2.  All modified copies shall carry a notice stating who
  18. **      made the last modification and the date of such modification.
  19. **  3.  No charge is made for this software or works derived from it.  
  20. **      This clause shall not be construed as constraining other software
  21. **      distributed on the same medium as this software, nor is a
  22. **      distribution fee considered a charge.
  23. **
  24. ***************************************************************************/
  25.  
  26.  
  27. /*
  28.   This file contains routines for computing functions of matrices
  29.   especially polynomials and exponential functions
  30.   Copyright (C) Teresa Leyk and David Stewart, 1993
  31.   */
  32.  
  33. #include <stdio.h>
  34. #include <math.h>
  35. #include "matrix.h"
  36. #include "matrix2.h"
  37.  
  38. static char    rcsid[] = "$Id: mfunc.c,v 1.1 1994/01/13 05:33:58 des Exp $";
  39.  
  40.  
  41.  
  42. /* _m_pow -- computes integer powers of a square matrix A, A^p
  43.    -- uses tmp as temporary workspace */
  44. MAT    *_m_pow(A, p, tmp, out)
  45. MAT    *A, *tmp, *out;
  46. int    p;
  47. {
  48.    int        it_cnt, k, max_bit;
  49.    
  50.    /*
  51.      File containing routines for evaluating matrix functions
  52.      esp. the exponential function
  53.      */
  54.  
  55. #define    Z(k)    (((k) & 1) ? tmp : out)
  56.    
  57.    if ( ! A )
  58.      error(E_NULL,"_m_pow");
  59.    if ( A->m != A->n )
  60.      error(E_SQUARE,"_m_pow");
  61.    if ( p < 0 )
  62.      error(E_NEG,"_m_pow");
  63.    out = m_resize(out,A->m,A->n);
  64.    tmp = m_resize(tmp,A->m,A->n);
  65.    
  66.    if ( p == 0 )
  67.      m_ident(out);
  68.    else if ( p > 0 )
  69.    {
  70.       it_cnt = 1;
  71.       for ( max_bit = 0; ; max_bit++ )
  72.     if ( (p >> (max_bit+1)) == 0 )
  73.       break;
  74.       tmp = m_copy(A,tmp);
  75.       
  76.       for ( k = 0; k < max_bit; k++ )
  77.       {
  78.      m_mlt(Z(it_cnt),Z(it_cnt),Z(it_cnt+1));
  79.      it_cnt++;
  80.      if ( p & (1 << (max_bit-1)) )
  81.      {
  82.         m_mlt(A,Z(it_cnt),Z(it_cnt+1));
  83.         /* m_copy(Z(it_cnt),out); */
  84.         it_cnt++;
  85.      }
  86.      p <<= 1;
  87.       }
  88.       if (it_cnt & 1)
  89.     out = m_copy(Z(it_cnt),out);
  90.    }
  91.  
  92.    return out;
  93.  
  94. #undef Z   
  95. }
  96.  
  97. /* m_pow -- computes integer powers of a square matrix A, A^p */
  98. MAT    *m_pow(A, p, out)
  99. MAT    *A, *out;
  100. int    p;
  101. {
  102.    static MAT    *wkspace = MNULL;
  103.    
  104.    if ( ! A )
  105.      error(E_NULL,"m_pow");
  106.    if ( A->m != A->n )
  107.      error(E_SQUARE,"m_pow");
  108.    
  109.    wkspace = m_resize(wkspace,A->m,A->n);
  110.    MEM_STAT_REG(wkspace,TYPE_MAT);
  111.    
  112.    return _m_pow(A, p, wkspace, out);
  113.    
  114. }
  115.  
  116. /**************************************************/
  117.  
  118. /* _m_exp -- compute matrix exponential of A and save it in out
  119.    -- uses Pade approximation followed by repeated squaring
  120.    -- eps is the tolerance used for the Pade approximation 
  121.    -- A is not changed
  122.    -- q_out - degree of the Pade approximation (q_out,q_out)
  123.    -- j_out - the power of 2 for scaling the matrix A
  124.               such that ||A/2^j_out|| <= 0.5
  125. */
  126. MAT *_m_exp(A,eps,out,q_out,j_out)
  127. MAT *A,*out;
  128. double eps;
  129. int *q_out, *j_out;
  130. {
  131.    static MAT *D = MNULL, *Apow = MNULL, *N = MNULL, *Y = MNULL;
  132.    static VEC *c1 = VNULL, *tmp = VNULL;
  133.    VEC y0, y1;  /* additional structures */
  134.    static PERM *pivot = PNULL;
  135.    int j, k, l, q, r, s, j2max, t;
  136.    double inf_norm, eqq, power2, c, sign;
  137.    
  138.    if ( ! A )
  139.      error(E_SIZES,"_m_exp");
  140.    if ( A->m != A->n )
  141.      error(E_SIZES,"_m_exp");
  142.    if ( A == out )
  143.      error(E_INSITU,"_m_exp");
  144.    if ( eps < 0.0 )
  145.      error(E_RANGE,"_m_exp");
  146.    else if (eps == 0.0)
  147.      eps = MACHEPS;
  148.       
  149.    N = m_resize(N,A->m,A->n);
  150.    D = m_resize(D,A->m,A->n);
  151.    Apow = m_resize(Apow,A->m,A->n);
  152.    out = m_resize(out,A->m,A->n);
  153.  
  154.    MEM_STAT_REG(N,TYPE_MAT);
  155.    MEM_STAT_REG(D,TYPE_MAT);
  156.    MEM_STAT_REG(Apow,TYPE_MAT);
  157.    
  158.    /* normalise A to have ||A||_inf <= 1 */
  159.    inf_norm = m_norm_inf(A);
  160.    if (inf_norm <= 0.0) {
  161.       m_ident(out);
  162.       *q_out = -1;
  163.       *j_out = 0;
  164.       return out;
  165.    }
  166.    else {
  167.       j2max = floor(1+log(inf_norm)/log(2.0));
  168.       j2max = max(0, j2max);
  169.    }
  170.    
  171.    power2 = 1.0;
  172.    for ( k = 1; k <= j2max; k++ )
  173.      power2 *= 2;
  174.    power2 = 1.0/power2;
  175.    if ( j2max > 0 )
  176.      sm_mlt(power2,A,A);
  177.    
  178.    /* compute order for polynomial approximation */
  179.    eqq = 1.0/6.0;
  180.    for ( q = 1; eqq > eps; q++ )
  181.      eqq /= 16.0*(2.0*q+1.0)*(2.0*q+3.0);
  182.    
  183.    /* construct vector of coefficients */
  184.    c1 = v_resize(c1,q+1);
  185.    MEM_STAT_REG(c1,TYPE_VEC);
  186.    c1->ve[0] = 1.0;
  187.    for ( k = 1; k <= q; k++ ) 
  188.      c1->ve[k] = c1->ve[k-1]*(q-k+1)/((2*q-k+1)*(double)k);
  189.    
  190.    tmp = v_resize(tmp,A->n);
  191.    MEM_STAT_REG(tmp,TYPE_VEC);
  192.    
  193.    s = (int)floor(sqrt((double)q/2.0));
  194.    if ( s <= 0 )  s = 1;
  195.    _m_pow(A,s,out,Apow);
  196.    r = q/s;
  197.    
  198.    Y = m_resize(Y,s,A->n);
  199.    MEM_STAT_REG(Y,TYPE_MAT);
  200.    /* y0 and y1 are pointers to rows of Y, N and D */
  201.    y0.dim = y0.max_dim = A->n;   
  202.    y1.dim = y1.max_dim = A->n;
  203.    
  204.    m_zero(Y);
  205.    m_zero(N);
  206.    m_zero(D);
  207.    
  208.    for( j = 0; j < A->n; j++ )
  209.    {
  210.       if (j > 0)
  211.     Y->me[0][j-1] = 0.0;
  212.       y0.ve = Y->me[0];
  213.       y0.ve[j] = 1.0;
  214.       for ( k = 0; k < s-1; k++ )
  215.       {
  216.      y1.ve = Y->me[k+1];
  217.      mv_mlt(A,&y0,&y1);
  218.      y0.ve = y1.ve;
  219.       }
  220.  
  221.       y0.ve = N->me[j];
  222.       y1.ve = D->me[j];
  223.       t = s*r;
  224.       for ( l = 0; l <= q-t; l++ )
  225.       {
  226.      c = c1->ve[t+l];
  227.      sign = ((t+l) & 1) ? -1.0 : 1.0;
  228.      __mltadd__(y0.ve,Y->me[l],c,     Y->n);
  229.      __mltadd__(y1.ve,Y->me[l],c*sign,Y->n);
  230.       }
  231.       
  232.       for (k=1; k <= r; k++)
  233.       {
  234.      v_copy(mv_mlt(Apow,&y0,tmp),&y0);
  235.      v_copy(mv_mlt(Apow,&y1,tmp),&y1);
  236.      t = s*(r-k);
  237.      for (l=0; l < s; l++)
  238.      {
  239.         c = c1->ve[t+l];
  240.         sign = ((t+l) & 1) ? -1.0 : 1.0;
  241.         __mltadd__(y0.ve,Y->me[l],c,     Y->n);
  242.         __mltadd__(y1.ve,Y->me[l],c*sign,Y->n);
  243.      }
  244.       }
  245.    }
  246.  
  247.    pivot = px_resize(pivot,A->m);
  248.    MEM_STAT_REG(pivot,TYPE_PERM);
  249.    
  250.    /* note that N and D are transposed,
  251.       therefore we use LUTsolve;
  252.       out is saved row-wise, and must be transposed 
  253.       after this */
  254.  
  255.    LUfactor(D,pivot);
  256.    for (k=0; k < A->n; k++)
  257.    {
  258.       y0.ve = N->me[k];
  259.       y1.ve = out->me[k];
  260.       LUTsolve(D,pivot,&y0,&y1);
  261.    }
  262.    m_transp(out,out); 
  263.  
  264.  
  265.    /* Use recursive squaring to turn the normalised exponential to the
  266.       true exponential */
  267.  
  268. #define Z(k)    ((k) & 1 ? Apow : out)
  269.  
  270.    for( k = 1; k <= j2max; k++)
  271.       m_mlt(Z(k-1),Z(k-1),Z(k));
  272.  
  273.    if (Z(k) == out)
  274.      m_copy(Apow,out);
  275.    
  276.    /* output parameters */
  277.    *j_out = j2max;
  278.    *q_out = q;
  279.  
  280.    /* restore the matrix A */
  281.    sm_mlt(1.0/power2,A,A);
  282.    return out;
  283.  
  284. #undef Z
  285. }
  286.  
  287.  
  288. /* simple interface for _m_exp */
  289. MAT *m_exp(A,eps,out)
  290. MAT *A,*out;
  291. double eps;
  292. {
  293.    int q_out, j_out;
  294.  
  295.    return _m_exp(A,eps,out,&q_out,&j_out);
  296. }
  297.  
  298.  
  299. /*--------------------------------*/
  300.  
  301. /* m_poly -- computes sum_i a[i].A^i, where i=0,1,...dim(a);
  302.    -- uses C. Van Loan's fast and memory efficient method  */
  303. MAT *m_poly(A,a,out)
  304. MAT *A,*out;
  305. VEC *a;
  306. {
  307.    static MAT    *Apow = MNULL, *Y = MNULL;
  308.    static VEC   *tmp;
  309.    VEC y0, y1;  /* additional vectors */
  310.    int j, k, l, q, r, s, t;
  311.    
  312.    if ( ! A || ! a )
  313.      error(E_NULL,"m_poly");
  314.    if ( A->m != A->n )
  315.      error(E_SIZES,"m_poly");
  316.    if ( A == out )
  317.      error(E_INSITU,"m_poly");
  318.    
  319.    out = m_resize(out,A->m,A->n);
  320.    Apow = m_resize(Apow,A->m,A->n);
  321.    MEM_STAT_REG(Apow,TYPE_MAT);
  322.    tmp = v_resize(tmp,A->n);
  323.    MEM_STAT_REG(tmp,TYPE_VEC);
  324.  
  325.    q = a->dim - 1;
  326.    if ( q == 0 ) {
  327.       m_zero(out);
  328.       for (j=0; j < out->n; j++)
  329.     out->me[j][j] = a->ve[0];
  330.       return out;
  331.    }
  332.    else if ( q == 1) {
  333.       sm_mlt(a->ve[1],A,out);
  334.       for (j=0; j < out->n; j++)
  335.     out->me[j][j] += a->ve[0];
  336.       return out;
  337.    }
  338.    
  339.    s = (int)floor(sqrt((double)q/2.0));
  340.    if ( s <= 0 ) s = 1;
  341.    _m_pow(A,s,out,Apow);
  342.    r = q/s;
  343.    
  344.    Y = m_resize(Y,s,A->n);
  345.    MEM_STAT_REG(Y,TYPE_MAT);
  346.    /* pointers to rows of Y */
  347.    y0.dim = y0.max_dim = A->n;
  348.    y1.dim = y1.max_dim = A->n;
  349.  
  350.    m_zero(Y);
  351.    m_zero(out);
  352.    
  353. #define Z(k)     ((k) & 1 ? tmp : &y0)
  354. #define ZZ(k)    ((k) & 1 ? tmp->ve : y0.ve)
  355.  
  356.    for( j = 0; j < A->n; j++)
  357.    {
  358.       if( j > 0 )
  359.     Y->me[0][j-1] = 0.0;
  360.       Y->me[0][j] = 1.0;
  361.  
  362.       y0.ve = Y->me[0];
  363.       for (k = 0; k < s-1; k++)
  364.       {
  365.      y1.ve = Y->me[k+1];
  366.      mv_mlt(A,&y0,&y1);
  367.      y0.ve = y1.ve;
  368.       }
  369.       
  370.       y0.ve = out->me[j];
  371.  
  372.       t = s*r;
  373.       for ( l = 0; l <= q-t; l++ )
  374.     __mltadd__(y0.ve,Y->me[l],a->ve[t+l],Y->n);
  375.       
  376.       for (k=1; k <= r; k++)
  377.       {
  378.      mv_mlt(Apow,Z(k-1),Z(k)); 
  379.      t = s*(r-k);
  380.      for (l=0; l < s; l++)
  381.        __mltadd__(ZZ(k),Y->me[l],a->ve[t+l],Y->n);
  382.       }
  383.       if (Z(k) == &y0) v_copy(tmp,&y0);
  384.    }
  385.  
  386.    m_transp(out,out);
  387.    
  388.    return out;
  389. }
  390.  
  391.  
  392.