home *** CD-ROM | disk | FTP | other *** search
/ Celestin Apprentice 2 / Apprentice-Release2.iso / Source Code / C / Utilities / Calc 1.24.7 / matfunc.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-02-24  |  27.4 KB  |  1,320 lines  |  [TEXT/R*ch]
  1. /*
  2.  * Copyright (c) 1992 David I. Bell
  3.  * Permission is granted to use, distribute, or modify this source,
  4.  * provided that this copyright notice remains intact.
  5.  *
  6.  * Extended precision rational arithmetic matrix functions.
  7.  * Matrices can contain arbitrary types of elements.
  8.  */
  9.  
  10. #include "calc.h"
  11.  
  12.  
  13. #if 0
  14. static char *abortmsg = "Calculation aborted";
  15. static char *memmsg = "Not enough memory";
  16. #endif
  17.  
  18.  
  19. static void matswaprow(), matsubrow(), matmulrow();
  20. #if 0
  21. static void mataddrow();
  22. #endif
  23.  
  24. static MATRIX *matident();
  25.  
  26.  
  27.  
  28. /*
  29.  * Add two compatible matrices.
  30.  */
  31. MATRIX *
  32. matadd(m1, m2)
  33.     MATRIX *m1, *m2;
  34. {
  35.     int dim;
  36.  
  37.     long min1, min2, max1, max2, index;
  38.     VALUE *v1, *v2, *vres;
  39.     MATRIX *res;
  40.     MATRIX tmp;
  41.  
  42.     if (m1->m_dim != m2->m_dim)
  43.         error("Incompatible matrix dimensions for add");
  44.     tmp.m_dim = m1->m_dim;
  45.     tmp.m_size = m1->m_size;
  46.     for (dim = 0; dim < m1->m_dim; dim++) {
  47.         min1 = m1->m_min[dim];
  48.         max1 = m1->m_max[dim];
  49.         min2 = m2->m_min[dim];
  50.         max2 = m2->m_max[dim];
  51.         if ((min1 && min2 && (min1 != min2)) || ((max1-min1) != (max2-min2)))
  52.             error("Incompatible matrix bounds for add");
  53.         tmp.m_min[dim] = (min1 ? min1 : min2);
  54.         tmp.m_max[dim] = tmp.m_min[dim] + (max1 - min1);
  55.     }
  56.     res = matalloc(m1->m_size);
  57.     *res = tmp;
  58.     v1 = m1->m_table;
  59.     v2 = m2->m_table;
  60.     vres = res->m_table;
  61.     for (index = m1->m_size; index > 0; index--)
  62.         addvalue(v1++, v2++, vres++);
  63.     return res;
  64. }
  65.  
  66.  
  67. /*
  68.  * Subtract two compatible matrices.
  69.  */
  70. MATRIX *
  71. matsub(m1, m2)
  72.     MATRIX *m1, *m2;
  73. {
  74.     int dim;
  75.     long min1, min2, max1, max2, index;
  76.     VALUE *v1, *v2, *vres;
  77.     MATRIX *res;
  78.     MATRIX tmp;
  79.  
  80.     if (m1->m_dim != m2->m_dim)
  81.         error("Incompatible matrix dimensions for sub");
  82.     tmp.m_dim = m1->m_dim;
  83.     tmp.m_size = m1->m_size;
  84.     for (dim = 0; dim < m1->m_dim; dim++) {
  85.         min1 = m1->m_min[dim];
  86.         max1 = m1->m_max[dim];
  87.         min2 = m2->m_min[dim];
  88.         max2 = m2->m_max[dim];
  89.         if ((min1 && min2 && (min1 != min2)) || ((max1-min1) != (max2-min2)))
  90.             error("Incompatible matrix bounds for sub");
  91.         tmp.m_min[dim] = (min1 ? min1 : min2);
  92.         tmp.m_max[dim] = tmp.m_min[dim] + (max1 - min1);
  93.     }
  94.     res = matalloc(m1->m_size);
  95.     *res = tmp;
  96.     v1 = m1->m_table;
  97.     v2 = m2->m_table;
  98.     vres = res->m_table;
  99.     for (index = m1->m_size; index > 0; index--)
  100.         subvalue(v1++, v2++, vres++);
  101.     return res;
  102. }
  103.  
  104.  
  105. /*
  106.  * Produce the negative of a matrix.
  107.  */
  108. MATRIX *
  109. matneg(m)
  110.     MATRIX *m;
  111. {
  112.     register VALUE *val, *vres;
  113.     long index;
  114.     MATRIX *res;
  115.  
  116.     res = matalloc(m->m_size);
  117.     *res = *m;
  118.     val = m->m_table;
  119.     vres = res->m_table;
  120.     for (index = m->m_size; index > 0; index--)
  121.         negvalue(val++, vres++);
  122.     return res;
  123. }
  124.  
  125.  
  126. /*
  127.  * Multiply two compatible matrices.
  128.  */
  129. MATRIX *
  130. matmul(m1, m2)
  131.     MATRIX *m1, *m2;
  132. {
  133.     register MATRIX *res;
  134.     long i1, i2, max1, max2, index, maxindex;
  135.     VALUE *v1, *v2;
  136.     VALUE sum, tmp1, tmp2;
  137.  
  138.     if ((m1->m_dim != 2) || (m2->m_dim != 2))
  139.         error("Matrix dimension must be two for mul");
  140.     if ((m1->m_max[1] - m1->m_min[1]) != (m2->m_max[0] - m2->m_min[0]))
  141.         error("Incompatible bounds for matrix mul");
  142.     max1 = (m1->m_max[0] - m1->m_min[0] + 1);
  143.     max2 = (m2->m_max[1] - m2->m_min[1] + 1);
  144.     maxindex = (m1->m_max[1] - m1->m_min[1] + 1);
  145.     res = matalloc(max1 * max2);
  146.     res->m_dim = 2;
  147.     res->m_min[0] = m1->m_min[0];
  148.     res->m_max[0] = m1->m_max[0];
  149.     res->m_min[1] = m2->m_min[1];
  150.     res->m_max[1] = m2->m_max[1];
  151.     for (i1 = 0; i1 < max1; i1++) {
  152.         for (i2 = 0; i2 < max2; i2++) {
  153.             sum.v_num = qlink(&_qzero_);
  154.             sum.v_type = V_NUM;
  155.             v1 = &m1->m_table[i1 * maxindex];
  156.             v2 = &m2->m_table[i2];
  157.             for (index = 0; index < maxindex; index++) {
  158.                 mulvalue(v1, v2, &tmp1);
  159.                 addvalue(&sum, &tmp1, &tmp2);
  160.                 freevalue(&tmp1);
  161.                 freevalue(&sum);
  162.                 sum = tmp2;
  163.                 v1++;
  164.                 v2 += max2;
  165.             }
  166.             index = (i1 * max2) + i2;
  167.             res->m_table[index] = sum;
  168.         }
  169.     }
  170.     return res;
  171. }
  172.  
  173.  
  174. /*
  175.  * Square a matrix.
  176.  */
  177. MATRIX *
  178. matsquare(m)
  179.     MATRIX *m;
  180. {
  181.     register MATRIX *res;
  182.     long i1, i2, max, index;
  183.     VALUE *v1, *v2;
  184.     VALUE sum, tmp1, tmp2;
  185.  
  186.     if (m->m_dim != 2)
  187.         error("Matrix dimension must be two for square");
  188.     if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
  189.         error("Squaring non-square matrix");
  190.     max = (m->m_max[0] - m->m_min[0] + 1);
  191.     res = matalloc(max * max);
  192.     res->m_dim = 2;
  193.     res->m_min[0] = m->m_min[0];
  194.     res->m_max[0] = m->m_max[0];
  195.     res->m_min[1] = m->m_min[1];
  196.     res->m_max[1] = m->m_max[1];
  197.     for (i1 = 0; i1 < max; i1++) {
  198.         for (i2 = 0; i2 < max; i2++) {
  199.             sum.v_num = qlink(&_qzero_);
  200.             sum.v_type = V_NUM;
  201.             v1 = &m->m_table[i1 * max];
  202.             v2 = &m->m_table[i2];
  203.             for (index = 0; index < max; index++) {
  204.                 mulvalue(v1, v2, &tmp1);
  205.                 addvalue(&sum, &tmp1, &tmp2);
  206.                 freevalue(&tmp1);
  207.                 freevalue(&sum);
  208.                 sum = tmp2;
  209.                 v1++;
  210.                 v2 += max;
  211.             }
  212.             index = (i1 * max) + i2;
  213.             res->m_table[index] = sum;
  214.         }
  215.     }
  216.     return res;
  217. }
  218.  
  219.  
  220. /*
  221.  * Compute the result of raising a square matrix to an integer power.
  222.  * Negative powers mean the positive power of the inverse.
  223.  * Note: This calculation could someday be improved for large powers
  224.  * by using the characteristic polynomial of the matrix.
  225.  */
  226. MATRIX *
  227. matpowi(m, q)
  228.     MATRIX *m;        /* matrix to be raised */
  229.     NUMBER *q;        /* power to raise it to */
  230. {
  231.     MATRIX *res, *tmp;
  232.     long power;        /* power to raise to */
  233.     unsigned long bit;    /* current bit value */
  234.  
  235.     if (m->m_dim != 2)
  236.         error("Matrix dimension must be two for power");
  237.     if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
  238.         error("Raising non-square matrix to a power");
  239.     if (qisfrac(q))
  240.         error("Raising matrix to non-integral power");
  241.     if (isbig(q->num))
  242.         error("Raising matrix to very large power");
  243.     power = (istiny(q->num) ? z1tol(q->num) : z2tol(q->num));
  244.     if (qisneg(q))
  245.         power = -power;
  246.     /*
  247.      * Handle some low powers specially
  248.      */
  249.     if ((power <= 4) && (power >= -2)) {
  250.         switch ((int) power) {
  251.             case 0:
  252.                 return matident(m);
  253.             case 1:
  254.                 return matcopy(m);
  255.             case -1:
  256.                 return matinv(m);
  257.             case 2:
  258.                 return matsquare(m);
  259.             case -2:
  260.                 tmp = matinv(m);
  261.                 res = matsquare(tmp);
  262.                 matfree(tmp);
  263.                 return res;
  264.             case 3:
  265.                 tmp = matsquare(m);
  266.                 res = matmul(m, tmp);
  267.                 matfree(tmp);
  268.                 return res;
  269.             case 4:
  270.                 tmp = matsquare(m);
  271.                 res = matsquare(tmp);
  272.                 matfree(tmp);
  273.                 return res;
  274.         }
  275.     }
  276.     if (power < 0) {
  277.         m = matinv(m);
  278.         power = -power;
  279.     }
  280.     /*
  281.      * Compute the power by squaring and multiplying.
  282.      * This uses the left to right method of power raising.
  283.      */
  284.     bit = TOPFULL;
  285.     while ((bit & power) == 0)
  286.         bit >>= 1L;
  287.     bit >>= 1L;
  288.     res = matsquare(m);
  289.     if (bit & power) {
  290.         tmp = matmul(res, m);
  291.         matfree(res);
  292.         res = tmp;
  293.     }
  294.     bit >>= 1L;
  295.     while (bit) {
  296.         tmp = matsquare(res);
  297.         matfree(res);
  298.         res = tmp;
  299.         if (bit & power) {
  300.             tmp = matmul(res, m);
  301.             matfree(res);
  302.             res = tmp;
  303.         }
  304.         bit >>= 1L;
  305.     }
  306.     if (qisneg(q))
  307.         matfree(m);
  308.     return res;
  309. }
  310.  
  311.  
  312. /*
  313.  * Calculate the cross product of two one dimensional matrices each
  314.  * with three components.
  315.  *    m3 = matcross(m1, m2);
  316.  */
  317. MATRIX *
  318. matcross(m1, m2)
  319.     MATRIX *m1, *m2;
  320. {
  321.     MATRIX *res;
  322.     VALUE *v1, *v2, *vr;
  323.     VALUE tmp1, tmp2;
  324.  
  325.     if ((m1->m_dim != 1) || (m2->m_dim != 1))
  326.         error("Matrix not 1d for cross product");
  327.     if ((m1->m_size != 3) || (m2->m_size != 3))
  328.         error("Matrix not size 3 for cross product");
  329.     res = matalloc(3L);
  330.     res->m_dim = 1;
  331.     res->m_min[0] = 0;
  332.     res->m_max[0] = 2;
  333.     v1 = m1->m_table;
  334.     v2 = m2->m_table;
  335.     vr = res->m_table;
  336.     mulvalue(v1 + 1, v2 + 2, &tmp1);
  337.     mulvalue(v1 + 2, v2 + 1, &tmp2);
  338.     subvalue(&tmp1, &tmp2, vr + 0);
  339.     freevalue(&tmp1);
  340.     freevalue(&tmp2);
  341.     mulvalue(v1 + 2, v2 + 0, &tmp1);
  342.     mulvalue(v1 + 0, v2 + 2, &tmp2);
  343.     subvalue(&tmp1, &tmp2, vr + 1);
  344.     freevalue(&tmp1);
  345.     freevalue(&tmp2);
  346.     mulvalue(v1 + 0, v2 + 1, &tmp1);
  347.     mulvalue(v1 + 1, v2 + 0, &tmp2);
  348.     subvalue(&tmp1, &tmp2, vr + 2);
  349.     freevalue(&tmp1);
  350.     freevalue(&tmp2);
  351.     return res;
  352. }
  353.  
  354.  
  355. /*
  356.  * Return the dot product of two matrices.
  357.  *    result = matdot(m1, m2);
  358.  */
  359. VALUE
  360. matdot(m1, m2)
  361.     MATRIX *m1, *m2;
  362. {
  363.     VALUE *v1, *v2;
  364.     VALUE result, tmp1, tmp2;
  365.     long len;
  366.  
  367.     if ((m1->m_dim != 1) || (m2->m_dim != 1))
  368.         error("Matrix not 1d for dot product");
  369.     if (m1->m_size != m2->m_size)
  370.         error("Incompatible matrix sizes for dot product");
  371.     v1 = m1->m_table;
  372.     v2 = m2->m_table;
  373.     mulvalue(v1, v2, &result);
  374.     len = m1->m_size;
  375.     while (--len > 0) {
  376.         mulvalue(++v1, ++v2, &tmp1);
  377.         addvalue(&result, &tmp1, &tmp2);
  378.         freevalue(&tmp1);
  379.         freevalue(&result);
  380.         result = tmp2;
  381.     }
  382.     return result;
  383. }
  384.  
  385.  
  386. /*
  387.  * Scale the elements of a matrix by a specified power of two.
  388.  */
  389. MATRIX *
  390. matscale(m, n)
  391.     MATRIX *m;        /* matrix to be scaled */
  392.     long n;
  393. {
  394.     register VALUE *val, *vres;
  395.     VALUE num;
  396.     long index;
  397.     MATRIX *res;        /* resulting matrix */
  398.  
  399.     if (n == 0)
  400.         return matcopy(m);
  401.     num.v_type = V_NUM;
  402.     num.v_num = itoq(n);
  403.     res = matalloc(m->m_size);
  404.     *res = *m;
  405.     val = m->m_table;
  406.     vres = res->m_table;
  407.     for (index = m->m_size; index > 0; index--)
  408.         scalevalue(val++, &num, vres++);
  409.     qfree(num.v_num);
  410.     return res;
  411. }
  412.  
  413.  
  414. /*
  415.  * Shift the elements of a matrix by the specified number of bits.
  416.  * Positive shift means leftwards, negative shift rightwards.
  417.  */
  418. MATRIX *
  419. matshift(m, n)
  420.     MATRIX *m;        /* matrix to be scaled */
  421.     long n;
  422. {
  423.     register VALUE *val, *vres;
  424.     VALUE num;
  425.     long index;
  426.     MATRIX *res;        /* resulting matrix */
  427.  
  428.     if (n == 0)
  429.         return matcopy(m);
  430.     num.v_type = V_NUM;
  431.     num.v_num = itoq(n);
  432.     res = matalloc(m->m_size);
  433.     *res = *m;
  434.     val = m->m_table;
  435.     vres = res->m_table;
  436.     for (index = m->m_size; index > 0; index--)
  437.         shiftvalue(val++, &num, FALSE, vres++);
  438.     qfree(num.v_num);
  439.     return res;
  440. }
  441.  
  442.  
  443. /*
  444.  * Multiply the elements of a matrix by a specified value.
  445.  */
  446. MATRIX *
  447. matmulval(m, vp)
  448.     MATRIX *m;        /* matrix to be multiplied */
  449.     VALUE *vp;        /* value to multiply by */
  450. {
  451.     register VALUE *val, *vres;
  452.     long index;
  453.     MATRIX *res;
  454.  
  455.     res = matalloc(m->m_size);
  456.     *res = *m;
  457.     val = m->m_table;
  458.     vres = res->m_table;
  459.     for (index = m->m_size; index > 0; index--)
  460.         mulvalue(val++, vp, vres++);
  461.     return res;
  462. }
  463.  
  464.  
  465. /*
  466.  * Divide the elements of a matrix by a specified value, keeping
  467.  * only the integer quotient.
  468.  */
  469. MATRIX *
  470. matquoval(m, vp)
  471.     MATRIX *m;        /* matrix to be divided */
  472.     VALUE *vp;        /* value to divide by */
  473. {
  474.     register VALUE *val, *vres;
  475.     long index;
  476.     MATRIX *res;
  477.  
  478.     if ((vp->v_type == V_NUM) && qiszero(vp->v_num))
  479.         error("Division by zero");
  480.     res = matalloc(m->m_size);
  481.     *res = *m;
  482.     val = m->m_table;
  483.     vres = res->m_table;
  484.     for (index = m->m_size; index > 0; index--)
  485.         quovalue(val++, vp, vres++);
  486.     return res;
  487. }
  488.  
  489.  
  490. /*
  491.  * Divide the elements of a matrix by a specified value, keeping
  492.  * only the remainder of the division.
  493.  */
  494. MATRIX *
  495. matmodval(m, vp)
  496.     MATRIX *m;        /* matrix to be divided */
  497.     VALUE *vp;        /* value to divide by */
  498. {
  499.     register VALUE *val, *vres;
  500.     long index;
  501.     MATRIX *res;
  502.  
  503.     if ((vp->v_type == V_NUM) && qiszero(vp->v_num))
  504.         error("Division by zero");
  505.     res = matalloc(m->m_size);
  506.     *res = *m;
  507.     val = m->m_table;
  508.     vres = res->m_table;
  509.     for (index = m->m_size; index > 0; index--)
  510.         modvalue(val++, vp, vres++);
  511.     return res;
  512. }
  513.  
  514.  
  515. /*
  516.  * Transpose the elements of a two dimensional matrix.
  517.  */
  518. MATRIX *
  519. mattrans(m)
  520.     MATRIX *m;            /* matrix to be transposed */
  521. {
  522.     register VALUE *v1, *v2;    /* current values */
  523.     long rows, cols;        /* rows and columns in old matrix */
  524.     long row, col;            /* current row and column */
  525.     MATRIX *res;
  526.  
  527.     if (m->m_dim != 2)
  528.         error("Matrix dimension must be two for transpose");
  529.     res = matalloc(m->m_size);
  530.     res->m_dim = 2;
  531.     res->m_min[0] = m->m_min[1];
  532.     res->m_max[0] = m->m_max[1];
  533.     res->m_min[1] = m->m_min[0];
  534.     res->m_max[1] = m->m_max[0];
  535.     rows = (m->m_max[0] - m->m_min[0] + 1);
  536.     cols = (m->m_max[1] - m->m_min[1] + 1);
  537.     v1 = res->m_table;
  538.     for (row = 0; row < rows; row++) {
  539.         v2 = &m->m_table[row];
  540.         for (col = 0; col < cols; col++) {
  541.             copyvalue(v2, v1);
  542.             v1++;
  543.             v2 += rows;
  544.         }
  545.     }
  546.     return res;
  547. }
  548.  
  549.  
  550. /*
  551.  * Produce a matrix with values all of which are conjugated.
  552.  */
  553. MATRIX *
  554. matconj(m)
  555.     MATRIX *m;
  556. {
  557.     register VALUE *val, *vres;
  558.     long index;
  559.     MATRIX *res;
  560.  
  561.     res = matalloc(m->m_size);
  562.     *res = *m;
  563.     val = m->m_table;
  564.     vres = res->m_table;
  565.     for (index = m->m_size; index > 0; index--)
  566.         conjvalue(val++, vres++);
  567.     return res;
  568. }
  569.  
  570.  
  571. /*
  572.  * Produce a matrix with values all of which have been rounded to the
  573.  * specified number of decimal places.
  574.  */
  575. MATRIX *
  576. matround(m, places)
  577.     MATRIX *m;
  578.     long places;
  579. {
  580.     register VALUE *val, *vres;
  581.     VALUE tmp;
  582.     long index;
  583.     MATRIX *res;
  584.  
  585.     if (places < 0)
  586.         error("Negative number of places for matround");
  587.     res = matalloc(m->m_size);
  588.     *res = *m;
  589.     tmp.v_type = V_INT;
  590.     tmp.v_int = places;
  591.     val = m->m_table;
  592.     vres = res->m_table;
  593.     for (index = m->m_size; index > 0; index--)
  594.         roundvalue(val++, &tmp, vres++);
  595.     return res;
  596. }
  597.  
  598.  
  599. /*
  600.  * Produce a matrix with values all of which have been rounded to the
  601.  * specified number of binary places.
  602.  */
  603. MATRIX *
  604. matbround(m, places)
  605.     MATRIX *m;
  606.     long places;
  607. {
  608.     register VALUE *val, *vres;
  609.     VALUE tmp;
  610.     long index;
  611.     MATRIX *res;
  612.  
  613.     if (places < 0)
  614.         error("Negative number of places for matbround");
  615.     res = matalloc(m->m_size);
  616.     *res = *m;
  617.     tmp.v_type = V_INT;
  618.     tmp.v_int = places;
  619.     val = m->m_table;
  620.     vres = res->m_table;
  621.     for (index = m->m_size; index > 0; index--)
  622.         broundvalue(val++, &tmp, vres++);
  623.     return res;
  624. }
  625.  
  626.  
  627. /*
  628.  * Produce a matrix with values all of which have been truncated to integers.
  629.  */
  630. MATRIX *
  631. matint(m)
  632.     MATRIX *m;
  633. {
  634.     register VALUE *val, *vres;
  635.     long index;
  636.     MATRIX *res;
  637.  
  638.     res = matalloc(m->m_size);
  639.     *res = *m;
  640.     val = m->m_table;
  641.     vres = res->m_table;
  642.     for (index = m->m_size; index > 0; index--)
  643.         intvalue(val++, vres++);
  644.     return res;
  645. }
  646.  
  647.  
  648. /*
  649.  * Produce a matrix with values all of which have only the fraction part left.
  650.  */
  651. MATRIX *
  652. matfrac(m)
  653.     MATRIX *m;
  654. {
  655.     register VALUE *val, *vres;
  656.     long index;
  657.     MATRIX *res;
  658.  
  659.     res = matalloc(m->m_size);
  660.     *res = *m;
  661.     val = m->m_table;
  662.     vres = res->m_table;
  663.     for (index = m->m_size; index > 0; index--)
  664.         fracvalue(val++, vres++);
  665.     return res;
  666. }
  667.  
  668.  
  669. /*
  670.  * Search a matrix for the specified value, starting with the specified index.
  671.  * Returns the index of the found value, or -1 if the value was not found.
  672.  */
  673. long
  674. matsearch(m, vp, index)
  675.     MATRIX *m;
  676.     VALUE *vp;
  677.     long index;
  678. {
  679.     register VALUE *val;
  680.  
  681.     if (index < 0)
  682.         index = 0;
  683.     val = &m->m_table[index];
  684.     while (index < m->m_size) {
  685.         if (!comparevalue(vp, val))
  686.             return index;
  687.         index++;
  688.         val++;
  689.     }
  690.     return -1;
  691. }
  692.  
  693.  
  694. /*
  695.  * Search a matrix backwards for the specified value, starting with the
  696.  * specified index.  Returns the index of the found value, or -1 if the
  697.  * value was not found.
  698.  */
  699. long
  700. matrsearch(m, vp, index)
  701.     MATRIX *m;
  702.     VALUE *vp;
  703.     long index;
  704. {
  705.     register VALUE *val;
  706.  
  707.     if (index >= m->m_size)
  708.         index = m->m_size - 1;
  709.     val = &m->m_table[index];
  710.     while (index >= 0) {
  711.         if (!comparevalue(vp, val))
  712.             return index;
  713.         index--;
  714.         val--;
  715.     }
  716.     return -1;
  717. }
  718.  
  719.  
  720. /*
  721.  * Fill all of the elements of a matrix with one of two specified values.
  722.  * All entries are filled with the first specified value, except that if
  723.  * the matrix is square and the second value pointer is non-NULL, then
  724.  * all diagonal entries are filled with the second value.  This routine
  725.  * affects the supplied matrix directly, and doesn't return a copy.
  726.  */
  727. void
  728. matfill(m, v1, v2)
  729.     MATRIX *m;        /* matrix to be filled */
  730.     VALUE *v1;        /* value to fill most of matrix with */
  731.     VALUE *v2;        /* value for diagonal entries (or NULL) */
  732. {
  733.     register VALUE *val;
  734.     long row, col;
  735.     long rows;
  736.     long index;
  737.  
  738.     if (v2 && ((m->m_dim != 2) ||
  739.         ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))))
  740.             error("Filling diagonals of non-square matrix");
  741.     val = m->m_table;
  742.     for (index = m->m_size; index > 0; index--)
  743.         freevalue(val++);
  744.     val = m->m_table;
  745.     if (v2 == NULL) {
  746.         for (index = m->m_size; index > 0; index--)
  747.             copyvalue(v1, val++);
  748.         return;
  749.     }
  750.     rows = m->m_max[0] - m->m_min[0] + 1;
  751.     for (row = 0; row < rows; row++) {
  752.         for (col = 0; col < rows; col++) {
  753.             copyvalue(((row != col) ? v1 : v2), val++);
  754.         }
  755.     }
  756. }
  757.  
  758.  
  759. /*
  760.  * Set a copy of a square matrix to the identity matrix.
  761.  */
  762. static MATRIX *
  763. matident(m)
  764.     MATRIX *m;
  765. {
  766.     register VALUE *val;    /* current value */
  767.     long row, col;        /* current row and column */
  768.     long rows;        /* number of rows */
  769.     MATRIX *res;        /* resulting matrix */
  770.  
  771.     if (m->m_dim != 2)
  772.         error("Matrix dimension must be two for setting to identity");
  773.     if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
  774.         error("Matrix must be square for setting to identity");
  775.     res = matalloc(m->m_size);
  776.     *res = *m;
  777.     val = res->m_table;
  778.     rows = (res->m_max[0] - res->m_min[0] + 1);
  779.     for (row = 0; row < rows; row++) {
  780.         for (col = 0; col < rows; col++) {
  781.             val->v_type = V_NUM;
  782.             val->v_num = ((row == col) ? qlink(&_qone_) : qlink(&_qzero_));
  783.             val++;
  784.         }
  785.     }
  786.     return res;
  787. }
  788.  
  789.  
  790. /*
  791.  * Calculate the inverse of a matrix if it exists.
  792.  * This is done by using transformations on the supplied matrix to convert
  793.  * it to the identity matrix, and simultaneously applying the same set of
  794.  * transformations to the identity matrix.
  795.  */
  796. MATRIX *
  797. matinv(m)
  798.     MATRIX *m;
  799. {
  800.     MATRIX *res;        /* matrix to become the inverse */
  801.     long rows;        /* number of rows */
  802.     long cur;        /* current row being worked on */
  803.     long row, col;        /* temp row and column values */
  804.     VALUE *val;        /* current value in matrix*/
  805.     VALUE mulval;        /* value to multiply rows by */
  806.     VALUE tmpval;        /* temporary value */
  807.  
  808.     if (m->m_dim != 2)
  809.         error("Matrix dimension must be two for inverse");
  810.     if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
  811.         error("Inverting non-square matrix");
  812.     /*
  813.      * Begin by creating the identity matrix with the same attributes.
  814.      */
  815.     res = matalloc(m->m_size);
  816.     *res = *m;
  817.     rows = (m->m_max[0] - m->m_min[0] + 1);
  818.     val = res->m_table;
  819.     for (row = 0; row < rows; row++) {
  820.         for (col = 0; col < rows; col++) {
  821.             if (row == col)
  822.                 val->v_num = qlink(&_qone_);
  823.             else
  824.                 val->v_num = qlink(&_qzero_);
  825.             val->v_type = V_NUM;
  826.             val++;
  827.         }
  828.     }
  829.     /*
  830.      * Now loop over each row, and eliminate all entries in the
  831.      * corresponding column by using row operations.  Do the same
  832.      * operations on the resulting matrix.  Copy the original matrix
  833.      * so that we don't destroy it.
  834.      */
  835.     m = matcopy(m);
  836.     for (cur = 0; cur < rows; cur++) {
  837.         /*
  838.          * Find the first nonzero value in the rest of the column
  839.          * downwards from [cur,cur].  If there is no such value, then
  840.          * the matrix is not invertible.  If the first nonzero entry
  841.          * is not the current row, then swap the two rows to make the
  842.          * current one nonzero.
  843.          */
  844.         row = cur;
  845.         val = &m->m_table[(row * rows) + row];
  846.         while (testvalue(val) == 0) {
  847.             if (++row >= rows) {
  848.                 matfree(m);
  849.                 matfree(res);
  850.                 error("Matrix is not invertible");
  851.             }
  852.             val += rows;
  853.         }
  854.         invertvalue(val, &mulval);
  855.         if (row != cur) {
  856.             matswaprow(m, row, cur);
  857.             matswaprow(res, row, cur);
  858.         }
  859.         /*
  860.          * Now for every other nonzero entry in the current column, subtract
  861.          * the appropriate multiple of the current row to force that entry
  862.          * to become zero.
  863.          */
  864.         val = &m->m_table[cur];
  865.         for (row = 0; row < rows; row++, val += rows) {
  866.             if ((row == cur) || (testvalue(val) == 0))
  867.                 continue;
  868.             mulvalue(val, &mulval, &tmpval);
  869.             matsubrow(m, row, cur, &tmpval);
  870.             matsubrow(res, row, cur, &tmpval);
  871.             freevalue(&tmpval);
  872.         }
  873.         freevalue(&mulval);
  874.     }
  875.     /*
  876.      * Now the original matrix has nonzero entries only on its main diagonal.
  877.      * Scale the rows of the result matrix by the inverse of those entries.
  878.      */
  879.     val = m->m_table;
  880.     for (row = 0; row < rows; row++) {
  881.         if ((val->v_type != V_NUM) || !qisone(val->v_num)) {
  882.             invertvalue(val, &mulval);
  883.             matmulrow(res, row, &mulval);
  884.             freevalue(&mulval);
  885.         }
  886.         val += (rows + 1);
  887.     }
  888.     matfree(m);
  889.     return res;
  890. }
  891.  
  892.  
  893. /*
  894.  * Calculate the determinant of a square matrix.
  895.  * This is done using row operations to create an upper-diagonal matrix.
  896.  */
  897. VALUE
  898. matdet(m)
  899.     MATRIX *m;
  900. {
  901.     long rows;        /* number of rows */
  902.     long cur;        /* current row being worked on */
  903.     long row;        /* temp row values */
  904.     int neg;        /* whether to negate determinant */
  905.     VALUE *val;        /* current value */
  906.     VALUE mulval, tmpval;    /* other values */
  907.  
  908.     if (m->m_dim != 2)
  909.         error("Matrix dimension must be two for determinant");
  910.     if ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1]))
  911.         error("Non-square matrix for determinant");
  912.     /*
  913.      * Loop over each row, and eliminate all lower entries in the
  914.      * corresponding column by using row operations.  Copy the original
  915.      * matrix so that we don't destroy it.
  916.      */
  917.     neg = 0;
  918.     m = matcopy(m);
  919.     rows = (m->m_max[0] - m->m_min[0] + 1);
  920.     for (cur = 0; cur < rows; cur++) {
  921.         /*
  922.          * Find the first nonzero value in the rest of the column
  923.          * downwards from [cur,cur].  If there is no such value, then
  924.          * the determinant is zero.  If the first nonzero entry is not
  925.          * the current row, then swap the two rows to make the current
  926.          * one nonzero, and remember that the determinant changes sign.
  927.          */
  928.         row = cur;
  929.         val = &m->m_table[(row * rows) + row];
  930.         while (testvalue(val) == 0) {
  931.             if (++row >= rows) {
  932.                 matfree(m);
  933.                 mulval.v_type = V_NUM;
  934.                 mulval.v_num = qlink(&_qzero_);
  935.                 return mulval;
  936.             }
  937.             val += rows;
  938.         }
  939.         invertvalue(val, &mulval);
  940.         if (row != cur) {
  941.             matswaprow(m, row, cur);
  942.             neg = !neg;
  943.         }
  944.         /*
  945.          * Now for every other nonzero entry lower down in the current column,
  946.          * subtract the appropriate multiple of the current row to force that
  947.          * entry to become zero.
  948.          */
  949.         row = cur + 1;
  950.         val = &m->m_table[(row * rows) + cur];
  951.         for (; row < rows; row++, val += rows) {
  952.             if (testvalue(val) == 0)
  953.                 continue;
  954.             mulvalue(val, &mulval, &tmpval);
  955.             matsubrow(m, row, cur, &tmpval);
  956.             freevalue(&tmpval);
  957.         }
  958.         freevalue(&mulval);
  959.     }
  960.     /*
  961.      * Now the matrix is upper-diagonal, and the determinant is the
  962.      * product of the main diagonal entries, and is possibly negated.
  963.      */
  964.     val = m->m_table;
  965.     mulval.v_type = V_NUM;
  966.     mulval.v_num = qlink(&_qone_);
  967.     for (row = 0; row < rows; row++) {
  968.         mulvalue(&mulval, val, &tmpval);
  969.         freevalue(&mulval);
  970.         mulval = tmpval;
  971.         val += (rows + 1);
  972.     }
  973.     matfree(m);
  974.     if (neg) {
  975.         negvalue(&mulval, &tmpval);
  976.         freevalue(&mulval);
  977.         return tmpval;
  978.     }
  979.     return mulval;
  980. }
  981.  
  982.  
  983. /*
  984.  * Local utility routine to swap two rows of a square matrix.
  985.  * No checks are made to verify the legality of the arguments.
  986.  */
  987. static void
  988. matswaprow(m, r1, r2)
  989.     MATRIX *m;
  990.     long r1, r2;
  991. {
  992.     register VALUE *v1, *v2;
  993.     register long rows;
  994.     VALUE tmp;
  995.  
  996.     if (r1 == r2)
  997.         return;
  998.     rows = (m->m_max[0] - m->m_min[0] + 1);
  999.     v1 = &m->m_table[r1 * rows];
  1000.     v2 = &m->m_table[r2 * rows];
  1001.     while (rows-- > 0) {
  1002.         tmp = *v1;
  1003.         *v1 = *v2;
  1004.         *v2 = tmp;
  1005.         v1++;
  1006.         v2++;
  1007.     }
  1008. }
  1009.  
  1010.  
  1011. /*
  1012.  * Local utility routine to subtract a multiple of one row to another one.
  1013.  * The row to be changed is oprow, the row to be subtracted is baserow.
  1014.  * No checks are made to verify the legality of the arguments.
  1015.  */
  1016. static void
  1017. matsubrow(m, oprow, baserow, mulval)
  1018.     MATRIX *m;
  1019.     long oprow, baserow;
  1020.     VALUE *mulval;
  1021. {
  1022.     register VALUE *vop, *vbase;
  1023.     register long entries;
  1024.     VALUE tmp1, tmp2;
  1025.  
  1026.     entries = (m->m_max[0] - m->m_min[0] + 1);
  1027.     vop = &m->m_table[oprow * entries];
  1028.     vbase = &m->m_table[baserow * entries];
  1029.     while (entries-- > 0) {
  1030.         mulvalue(vbase, mulval, &tmp1);
  1031.         subvalue(vop, &tmp1, &tmp2);
  1032.         freevalue(&tmp1);
  1033.         freevalue(vop);
  1034.         *vop = tmp2;
  1035.         vop++;
  1036.         vbase++;
  1037.     }
  1038. }
  1039.  
  1040.  
  1041. #if 0
  1042. /*
  1043.  * Local utility routine to add one row to another one.
  1044.  * No checks are made to verify the legality of the arguments.
  1045.  */
  1046. static void
  1047. mataddrow(m, r1, r2)
  1048.     MATRIX *m;
  1049.     long r1;    /* row to be added into */
  1050.     long r2;    /* row to add */
  1051. {
  1052.     register VALUE *v1, *v2;
  1053.     register long rows;
  1054.     VALUE tmp;
  1055.  
  1056.     rows = (m->m_max[0] - m->m_min[0] + 1);
  1057.     v1 = &m->m_table[r1 * rows];
  1058.     v2 = &m->m_table[r2 * rows];
  1059.     while (rows-- > 0) {
  1060.         addvalue(v1, v2, &tmp);
  1061.         freevalue(v1);
  1062.         *v1 = tmp;
  1063.         v1++;
  1064.         v2++;
  1065.     }
  1066. }
  1067. #endif
  1068.  
  1069.  
  1070. /*
  1071.  * Local utility routine to multiply a row by a specified number.
  1072.  * No checks are made to verify the legality of the arguments.
  1073.  */
  1074. static void
  1075. matmulrow(m, row, mulval)
  1076.     MATRIX *m;
  1077.     long row;
  1078.     VALUE *mulval;
  1079. {
  1080.     register VALUE *val;
  1081.     register long rows;
  1082.     VALUE tmp;
  1083.  
  1084.     rows = (m->m_max[0] - m->m_min[0] + 1);
  1085.     val = &m->m_table[row * rows];
  1086.     while (rows-- > 0) {
  1087.         mulvalue(val, mulval, &tmp);
  1088.         freevalue(val);
  1089.         *val = tmp;
  1090.         val++;
  1091.     }
  1092. }
  1093.  
  1094.  
  1095. /*
  1096.  * Make a full copy of a matrix.
  1097.  */
  1098. MATRIX *
  1099. matcopy(m)
  1100.     MATRIX *m;
  1101. {
  1102.     MATRIX *res;
  1103.     register VALUE *v1, *v2;
  1104.     register long i;
  1105.  
  1106.     res = matalloc(m->m_size);
  1107.     *res = *m;
  1108.     v1 = m->m_table;
  1109.     v2 = res->m_table;
  1110.     i = m->m_size;
  1111.     while (i-- > 0) {
  1112.         if (v1->v_type == V_NUM) {
  1113.             v2->v_type = V_NUM;
  1114.             v2->v_num = qlink(v1->v_num);
  1115.         } else
  1116.             copyvalue(v1, v2);
  1117.         v1++;
  1118.         v2++;
  1119.     }
  1120.     return res;
  1121. }
  1122.  
  1123.  
  1124. /*
  1125.  * Allocate a matrix with the specified number of elements.
  1126.  */
  1127. MATRIX *
  1128. matalloc(size)
  1129.     long size;
  1130. {
  1131.     MATRIX *m;
  1132.  
  1133.     m = (MATRIX *) malloc(matsize(size));
  1134.     if (m == NULL)
  1135.         error("Cannot get memory to allocate matrix of size %d", size);
  1136.     m->m_size = size;
  1137.     return m;
  1138. }
  1139.  
  1140.  
  1141. /*
  1142.  * Free a matrix, along with all of its element values.
  1143.  */
  1144. void
  1145. matfree(m)
  1146.     MATRIX *m;
  1147. {
  1148.     register VALUE *vp;
  1149.     register long i;
  1150.  
  1151.     vp = m->m_table;
  1152.     i = m->m_size;
  1153.     while (i-- > 0) {
  1154.         if (vp->v_type == V_NUM) {
  1155.             vp->v_type = V_NULL;
  1156.             qfree(vp->v_num);
  1157.         } else
  1158.             freevalue(vp);
  1159.         vp++;
  1160.     }
  1161.     free(m);
  1162. }
  1163.  
  1164.  
  1165. /*
  1166.  * Test whether a matrix has any nonzero values.
  1167.  * Returns TRUE if so.
  1168.  */
  1169. BOOL
  1170. mattest(m)
  1171.     MATRIX *m;
  1172. {
  1173.     register VALUE *vp;
  1174.     register long i;
  1175.  
  1176.     vp = m->m_table;
  1177.     i = m->m_size;
  1178.     while (i-- > 0) {
  1179.         if ((vp->v_type != V_NUM) || (!qiszero(vp->v_num)))
  1180.             return TRUE;
  1181.         vp++;
  1182.     }
  1183.     return FALSE;
  1184. }
  1185.  
  1186.  
  1187. /*
  1188.  * Test whether or not two matrices are equal.
  1189.  * Equality is determined by the shape and values of the matrices,
  1190.  * but not by their index bounds.  Returns TRUE if they differ.
  1191.  */
  1192. BOOL
  1193. matcmp(m1, m2)
  1194.     MATRIX *m1, *m2;
  1195. {
  1196.     VALUE *v1, *v2;
  1197.     long i;
  1198.  
  1199.     if (m1 == m2)
  1200.         return FALSE;
  1201.     if ((m1->m_dim != m2->m_dim) || (m1->m_size != m2->m_size))
  1202.         return TRUE;
  1203.     for (i = 0; i < m1->m_dim; i++) {
  1204.         if ((m1->m_max[i] - m1->m_min[i]) != (m2->m_max[i] - m2->m_min[i]))
  1205.         return TRUE;
  1206.     }
  1207.     v1 = m1->m_table;
  1208.     v2 = m2->m_table;
  1209.     i = m1->m_size;
  1210.     while (i-- > 0) {
  1211.         if (comparevalue(v1++, v2++))
  1212.             return TRUE;
  1213.     }
  1214.     return FALSE;
  1215. }
  1216.  
  1217.  
  1218. #if 0
  1219. /*
  1220.  * Test whether or not a matrix is the identity matrix.
  1221.  * Returns TRUE if so.
  1222.  */
  1223. BOOL
  1224. matisident(m)
  1225.     MATRIX *m;
  1226. {
  1227.     register VALUE *val;    /* current value */
  1228.     long row, col;        /* row and column numbers */
  1229.  
  1230.     if ((m->m_dim != 2) ||
  1231.         ((m->m_max[0] - m->m_min[0]) != (m->m_max[1] - m->m_min[1])))
  1232.             return FALSE;
  1233.     val = m->m_table;
  1234.     for (row = 0; row < m->m_size; row++) {
  1235.         for (col = 0; col < m->m_size; col++) {
  1236.             if (val->v_type != V_NUM)
  1237.                 return FALSE;
  1238.             if (row == col) {
  1239.                 if (!qisone(val->v_num))
  1240.                     return FALSE;
  1241.             } else {
  1242.                 if (!qiszero(val->v_num))
  1243.                     return FALSE;
  1244.             }
  1245.             val++;
  1246.         }
  1247.     }
  1248.     return TRUE;
  1249. }
  1250. #endif
  1251.  
  1252.  
  1253. /*
  1254.  * Print a matrix and possibly few of its elements.
  1255.  * The argument supplied specifies how many elements to allow printing.
  1256.  * If any elements are printed, they are printed in short form.
  1257.  */
  1258. void
  1259. matprint(m, maxprint)
  1260.     MATRIX *m;
  1261.     long maxprint;
  1262. {
  1263.     VALUE *vp;
  1264.     long fullsize, count, index, num;
  1265.     int dim, i;
  1266.     char *msg;
  1267.     long sizes[MAXDIM];
  1268.  
  1269.     dim = m->m_dim;
  1270.     fullsize = 1;
  1271.     for (i = dim - 1; i >= 0; i--) {
  1272.         sizes[i] = fullsize;
  1273.         fullsize *= (m->m_max[i] - m->m_min[i] + 1);
  1274.     }
  1275.     msg = ((maxprint > 0) ? "\nmat [" : "mat [");
  1276.     for (i = 0; i < dim; i++) {
  1277.         if (m->m_min[i])
  1278.             math_fmt("%s%ld:%ld", msg, m->m_min[i], m->m_max[i]);
  1279.         else
  1280.             math_fmt("%s%ld", msg, m->m_max[i] + 1);
  1281.         msg = ",";
  1282.     }
  1283.     if (maxprint > fullsize)
  1284.         maxprint = fullsize;
  1285.     vp = m->m_table;
  1286.     count = 0;
  1287.     for (index = 0; index < fullsize; index++) {
  1288.         if ((vp->v_type != V_NUM) || !qiszero(vp->v_num))
  1289.             count++;
  1290.         vp++;
  1291.     }
  1292.     math_fmt("] (%ld element%s, %ld nonzero)",
  1293.         fullsize, (fullsize == 1) ? "" : "s", count);
  1294.     if (maxprint <= 0)
  1295.         return;
  1296.  
  1297.     /*
  1298.      * Now print the first few elements of the matrix in short
  1299.      * and unambigous format.
  1300.      */
  1301.     math_str(":\n");
  1302.     vp = m->m_table;
  1303.     for (index = 0; index < maxprint; index++) {
  1304.         msg = "  [";
  1305.         num = index;
  1306.         for (i = 0; i < dim; i++) {
  1307.             math_fmt("%s%ld", msg, m->m_min[i] + (num / sizes[i]));
  1308.             num %= sizes[i];
  1309.             msg = ",";
  1310.         }
  1311.         math_str("] = ");
  1312.         printvalue(vp++, PRINT_SHORT | PRINT_UNAMBIG);
  1313.         math_str("\n");
  1314.     }
  1315.     if (maxprint < fullsize)
  1316.         math_str("  ...\n");
  1317. }
  1318.  
  1319. /* END CODE */
  1320.