home *** CD-ROM | disk | FTP | other *** search
/ ARM Club 1 / ARM_CLUB_CD.iso / contents / apps / clib / progs / meschach / !Meschach / c / mfunc < prev    next >
Encoding:
Text File  |  1994-01-13  |  8.8 KB  |  410 lines
  1.     return (-1);
  2.    
  3.    if ( px->pe == (u_int *)NULL ) {
  4.       if (mem_info_is_on()) {
  5.      mem_bytes(TYPE_PERM,sizeof(PERM),0);
  6.      mem_numvar(TYPE_PERM,-1);
  7.       }      
  8.       free((char *)px);
  9.    }
  10.    else
  11.    {
  12.       if (mem_info_is_on()) {
  13.      mem_bytes(TYPE_PERM,sizeof(PERM)+px->max_size*sizeof(u_int),0);
  14.      mem_numvar(TYPE_PERM,-1);
  15.       }
  16.       free((char *)px->pe);
  17.       free((char *)px);
  18.    }
  19.    
  20.    return (0);
  21. }
  22.  
  23.  
  24.  
  25. /* v_free -- returns VEC & asoociated memory back to memory heap */
  26. int    v_free(vec)
  27. VEC    *vec;
  28. {
  29.    if ( vec==(VEC *)NULL || (int)(vec->dim) < 0 )
  30.      /* don't trust it */
  31.      return (-1);
  32.    
  33.    if ( vec->ve == (Real *)NULL ) {
  34.       if (mem_info_is_on()) {
  35.      mem_bytes(TYPE_VEC,sizeof(VEC),0);
  36.      mem_numvar(TYPE_VEC,-1);
  37.       }
  38.       free((char *)vec);
  39.    }
  40.    else
  41.    {
  42.       if (mem_info_is_on()) {
  43.      mem_bytes(TYPE_VEC,sizeof(VEC)+vec->max_dim*sizeof(Real),0);
  44.      mem_numvar(TYPE_VEC,-1);
  45.       }
  46.       free((char *)vec->ve);
  47.       free((char *)vec);
  48.    }
  49.    
  50.    return (0);
  51. }
  52.  
  53.  
  54.  
  55. /* m_resize -- returns the matrix A of size new_m x new_n; A is zeroed
  56.    -- if A == NULL on entry then the effect is equivalent to m_get() */
  57. MAT    *m_resize(A,new_m,new_n)
  58. MAT    *A;
  59. int    new_m, new_n;
  60. {
  61.    int    i;
  62.    int    new_max_m, new_max_n, new_size, old_m, old_n;
  63.    
  64.    if (new_m < 0 || new_n < 0)
  65.      error(E_NEG,"m_resize");
  66.  
  67.    if ( ! A )
  68.      return m_get(new_m,new_n);
  69.  
  70.    /* nothing was changed */
  71.    if (new_m == A->m && new_n == A->n)
  72.      return A;
  73.  
  74.    old_m = A->m;    old_n = A->n;
  75.    if ( new_m > A->max_m )
  76.    {    /* re-allocate A->me */
  77.       if (mem_info_is_on()) {
  78.      mem_bytes(TYPE_MAT,A->max_m*sizeof(Real *),
  79.               new_m*sizeof(Real *));
  80.       }
  81.  
  82.       A->me = RENEW(A->me,new_m,Real *);
  83.       if ( ! A->me )
  84.     error(E_MEM,"m_resize");
  85.    }
  86.    new_max_m = max(new_m,A->max_m);
  87.    new_max_n = max(new_n,A->max_n);
  88.    
  89. #ifndef SEGMENTED
  90.    new_size = new_max_m*new_max_n;
  91.    if ( new_size > A->max_size )
  92.    {    /* re-allocate A->base */
  93.       if (mem_info_is_on()) {
  94.      mem_bytes(TYPE_MAT,A->max_m*A->max_n*sizeof(Real),
  95.               new_size*sizeof(Real));
  96.       }
  97.  
  98.       A->base = RENEW(A->base,new_size,Real);
  99.       if ( ! A->base )
  100.     error(E_MEM,"m_resize");
  101.       A->max_size = new_size;
  102.    }
  103.    
  104.    /* now set up A->me[i] */
  105.    for ( i = 0; i < new_m; i++ )
  106.      A->me[i] = &(A->base[i*new_n]);
  107.    
  108.    /* now shift data in matrix */
  109.    if ( old_n > new_n )
  110.    {
  111.       for ( i = 1; i < min(old_m,new_m); i++ )
  112.     MEM_COPY((char *)&(A->base[i*old_n]),
  113.          (char *)&(A->base[i*new_n]),
  114.          sizeof(Real)*new_n);
  115.    }
  116.    else if ( old_n < new_n )
  117.    {
  118.       for ( i = (int)(min(old_m,new_m))-1; i > 0; i-- )
  119.       {   /* copy & then zero extra space */
  120.      MEM_COPY((char *)&(A->base[i*old_n]),
  121.           (char *)&(A->base[i*new_n]),
  122.           sizeof(Real)*old_n);
  123.      __zero__(&(A->base[i*new_n+old_n]),(new_n-old_n));
  124.       }
  125.       __zero__(&(A->base[old_n]),(new_n-old_n));
  126.       A->max_n = new_n;
  127.    }
  128.    /* zero out the new rows.. */
  129.    for ( i = old_m; i < new_m; i++ )
  130.      __zero__(&(A->base[i*new_n]),new_n);
  131. #else
  132.    if ( A->max_n < new_n )
  133.    {
  134.       Real    *tmp;
  135.       
  136.       for ( i = 0; i < A->max_m; i++ )
  137.       {
  138.      if (mem_info_ade approximation 
  139.    -- A is not changed
  140.    -- q_out - degree of the Pade approximation (q_out,q_out)
  141.    -- j_out - the power of 2 for scaling the matrix A
  142.               such that ||A/2^j_out|| <= 0.5
  143. */
  144. MAT *_m_exp(A,eps,out,q_out,j_out)
  145. MAT *A,*out;
  146. double eps;
  147. int *q_out, *j_out;
  148. {
  149.    static MAT *D = MNULL, *Apow = MNULL, *N = MNULL, *Y = MNULL;
  150.    static VEC *c1 = VNULL, *tmp = VNULL;
  151.    VEC y0, y1;  /* additional structures */
  152.    static PERM *pivot = PNULL;
  153.    int j, k, l, q, r, s, j2max, t;
  154.    double inf_norm, eqq, power2, c, sign;
  155.    
  156.    if ( ! A )
  157.      error(E_SIZES,"_m_exp");
  158.    if ( A->m != A->n )
  159.      error(E_SIZES,"_m_exp");
  160.    if ( A == out )
  161.      error(E_INSITU,"_m_exp");
  162.    if ( eps < 0.0 )
  163.      error(E_RANGE,"_m_exp");
  164.    else if (eps == 0.0)
  165.      eps = MACHEPS;
  166.       
  167.    N = m_resize(N,A->m,A->n);
  168.    D = m_resize(D,A->m,A->n);
  169.    Apow = m_resize(Apow,A->m,A->n);
  170.    out = m_resize(out,A->m,A->n);
  171.  
  172.    MEM_STAT_REG(N,TYPE_MAT);
  173.    MEM_STAT_REG(D,TYPE_MAT);
  174.    MEM_STAT_REG(Apow,TYPE_MAT);
  175.    
  176.    /* normalise A to have ||A||_inf <= 1 */
  177.    inf_norm = m_norm_inf(A);
  178.    if (inf_norm <= 0.0) {
  179.       m_ident(out);
  180.       *q_out = -1;
  181.       *j_out = 0;
  182.       return out;
  183.    }
  184.    else {
  185.       j2max = floor(1+log(inf_norm)/log(2.0));
  186.       j2max = max(0, j2max);
  187.    }
  188.    
  189.    power2 = 1.0;
  190.    for ( k = 1; k <= j2max; k++ )
  191.      power2 *= 2;
  192.    power2 = 1.0/power2;
  193.    if ( j2max > 0 )
  194.      sm_mlt(power2,A,A);
  195.    
  196.    /* compute order for polynomial approximation */
  197.    eqq = 1.0/6.0;
  198.    for ( q = 1; eqq > eps; q++ )
  199.      eqq /= 16.0*(2.0*q+1.0)*(2.0*q+3.0);
  200.    
  201.    /* construct vector of coefficients */
  202.    c1 = v_resize(c1,q+1);
  203.    MEM_STAT_REG(c1,TYPE_VEC);
  204.    c1->ve[0] = 1.0;
  205.    for ( k = 1; k <= q; k++ ) 
  206.      c1->ve[k] = c1->ve[k-1]*(q-k+1)/((2*q-k+1)*(double)k);
  207.    
  208.    tmp = v_resize(tmp,A->n);
  209.    MEM_STAT_REG(tmp,TYPE_VEC);
  210.    
  211.    s = (int)floor(sqrt((double)q/2.0));
  212.    if ( s <= 0 )  s = 1;
  213.    _m_pow(A,s,out,Apow);
  214.    r = q/s;
  215.    
  216.    Y = m_resize(Y,s,A->n);
  217.    MEM_STAT_REG(Y,TYPE_MAT);
  218.    /* y0 and y1 are pointers to rows of Y, N and D */
  219.    y0.dim = y0.max_dim = A->n;   
  220.    y1.dim = y1.max_dim = A->n;
  221.    
  222.    m_zero(Y);
  223.    m_zero(N);
  224.    m_zero(D);
  225.    
  226.    for( j = 0; j < A->n; j++ )
  227.    {
  228.       if (j > 0)
  229.     Y->me[0][j-1] = 0.0;
  230.       y0.ve = Y->me[0];
  231.       y0.ve[j] = 1.0;
  232.       for ( k = 0; k < s-1; k++ )
  233.       {
  234.      y1.ve = Y->me[k+1];
  235.      mv_mlt(A,&y0,&y1);
  236.      y0.ve = y1.ve;
  237.       }
  238.  
  239.       y0.ve = N->me[j];
  240.       y1.ve = D->me[j];
  241.       t = s*r;
  242.       for ( l = 0; l <= q-t; l++ )
  243.       {
  244.      c = c1->ve[t+l];
  245.      sign = ((t+l) & 1) ? -1.0 : 1.0;
  246.      __mltadd__(y0.ve,Y->me[l],c,     Y->n);
  247.      __mltadd__(y1.ve,Y->me[l],c*sign,Y->n);
  248.       }
  249.       
  250.       for (k=1; k <= r; k++)
  251.       {
  252.      v_copy(mv_mlt(Apow,&y0,tmp),&y0);
  253.      v_copy(mv_mlt(Apow,&y1,tmp),&y1);
  254.      t = s*(r-k);
  255.      for (l=0; l < s; l++)
  256.      {
  257.         c = c1->ve[t+l];
  258.         sign = ((t+l) & 1) ? -1.0 : 1.0;
  259.         __mltadd__(y0.ve,Y->me[l],c,     Y->n);
  260.         __mltadd__(y1.ve,Y->me[l],c*sign,Y->n);
  261.      }
  262.       }
  263.    }
  264.  
  265.    pivot = px_resize(pivot,A->m);
  266.    MEM_STAT_REG(pivot,TYPE_PERM);
  267.    
  268.    /* note that N and D are transposed,
  269.       therefore we use LUTsolve;
  270.       out is saved row-wise, and must be transposed 
  271.       after this */
  272.  
  273.    LUfactor(D,pivot);
  274.    for (k=0; k < A->n; k++)
  275.    {
  276.       y0.ve = N->me[k];
  277.       y1.ve = out->me[k];
  278.       LUTsolve(D,pivot,&y0,&y1);
  279.    }
  280.    m_transp(out,out); 
  281.  
  282.  
  283.    /* Use recursive squaring to turn the normalised exponential to the
  284.       true exponential */
  285.  
  286. #define Z(k)    ((k) & 1 ? Apow : out)
  287.  
  288.    for( k = 1; k <= j2max; k++)
  289.       m_mlt(Z(k-1),Z(k-1),Z(k));
  290.  
  291.    if (Z(k) == out)
  292.      m_copy(Apow,out);
  293.    
  294.    /* output parameters */
  295.    *j_out = j2max;
  296.    *q_out = q;
  297.  
  298.    /* restore the matrix A */
  299.    sm_mlt(1.0/power2,A,A);
  300.    return out;
  301.  
  302. #undef Z
  303. }
  304.  
  305.  
  306. /* simple interface for _m_exp */
  307. MAT *m_exp(A,eps,out)
  308. MAT *A,*out;
  309. double eps;
  310. {
  311.    int q_out, j_out;
  312.  
  313.    return _m_exp(A,eps,out,&q_out,&j_out);
  314. }
  315.  
  316.  
  317. /*--------------------------------*/
  318.  
  319. /* m_poly -- computes sum_i a[i].A^i, where i=0,1,...dim(a);
  320.    -- uses C. Van Loan's fast and memory efficient method  */
  321. MAT *m_poly(A,a,out)
  322. MAT *A,*out;
  323. VEC *a;
  324. {
  325.    static MAT    *Apow = MNULL, *Y = MNULL;
  326.    static VEC   *tmp;
  327.    VEC y0, y1;  /* additional vectors */
  328.    int j, k, l, q, r, s, t;
  329.    
  330.    if ( ! A || ! a )
  331.      error(E_NULL,"m_poly");
  332.    if ( A->m != A->n )
  333.      error(E_SIZES,"m_poly");
  334.    if ( A == out )
  335.      error(E_INSITU,"m_poly");
  336.    
  337.    out = m_resize(out,A->m,A->n);
  338.    Apow = m_resize(Apow,A->m,A->n);
  339.    MEM_STAT_REG(Apow,TYPE_MAT);
  340.    tmp = v_resize(tmp,A->n);
  341.    MEM_STAT_REG(tmp,TYPE_VEC);
  342.  
  343.    q = a->dim - 1;
  344.    if ( q == 0 ) {
  345.       m_zero(out);
  346.       for (j=0; j < out->n; j++)
  347.     out->me[j][j] = a->ve[0];
  348.       return out;
  349.    }
  350.    else if ( q == 1) {
  351.       sm_mlt(a->ve[1],A,out);
  352.       for (j=0; j < out->n; j++)
  353.     out->me[j][j] += a->ve[0];
  354.       return out;
  355.    }
  356.    
  357.    s = (int)floor(sqrt((double)q/2.0));
  358.    if ( s <= 0 ) s = 1;
  359.    _m_pow(A,s,out,Apow);
  360.    r = q/s;
  361.    
  362.    Y = m_resize(Y,s,A->n);
  363.    MEM_STAT_REG(Y,TYPE_MAT);
  364.    /* pointers to rows of Y */
  365.    y0.dim = y0.max_dim = A->n;
  366.    y1.dim = y1.max_dim = A->n;
  367.  
  368.    m_zero(Y);
  369.    m_zero(out);
  370.    
  371. #define Z(k)     ((k) & 1 ? tmp : &y0)
  372. #define ZZ(k)    ((k) & 1 ? tmp->ve : y0.ve)
  373.  
  374.    for( j = 0; j < A->n; j++)
  375.    {
  376.       if( j > 0 )
  377.     Y->me[0][j-1] = 0.0;
  378.       Y->me[0][j] = 1.0;
  379.  
  380.       y0.ve = Y->me[0];
  381.       for (k = 0; k < s-1; k++)
  382.       {
  383.      y1.ve = Y->me[k+1];
  384.      mv_mlt(A,&y0,&y1);
  385.      y0.ve = y1.ve;
  386.       }
  387.       
  388.       y0.ve = out->me[j];
  389.  
  390.       t = s*r;
  391.       for ( l = 0; l <= q-t; l++ )
  392.     __mltadd__(y0.ve,Y->me[l],a->ve[t+l],Y->n);
  393.       
  394.       for (k=1; k <= r; k++)
  395.       {
  396.      mv_mlt(Apow,Z(k-1),Z(k)); 
  397.      t = s*(r-k);
  398.      for (l=0; l < s; l++)
  399.        __mltadd__(ZZ(k),Y->me[l],a->ve[t+l],Y->n);
  400.       }
  401.       if (Z(k) == &y0) v_copy(tmp,&y0);
  402.    }
  403.  
  404.    m_transp(out,out);
  405.    
  406.    return out;
  407. }
  408.  
  409.  
  410.