inv.cpp

   1 //-----------------------------------------------------------------------------
   2 //
   3 //      Input:          Matrix on stack
   4 //
   5 //      Output:         Inverse on stack
   6 //
   7 //      Example:
   8 //
   9 //      > inv(((1,2),(3,4))
  10 //      ((-2,1),(3/2,-1/2))
  11 //
  12 //      Note:
  13 //
  14 //      Uses Gaussian elimination for numerical matrices.
  15 //
  16 //-----------------------------------------------------------------------------
  17
  18 #include "stdafx.h"
  19 #include "defs.h"
  20
  21 static int
  22 check_arg(void)
  23 {
  24         if (!istensor(p1))
  25                 return 0;
  26         else if (p1->u.tensor->ndim != 2)
  27                 return 0;
  28         else if (p1->u.tensor->dim[0] != p1->u.tensor->dim[1])
  29                 return 0;
  30         else
  31                 return 1;
  32 }
  33
  34 void
  35 inv(void)
  36 {
  37         int i, n;
  38         U **a;
  39
  40         save();
  41
  42         p1 = pop();
  43
  44         if (check_arg() == 0) {
  45                 push_symbol(INV);
  46                 push(p1);
  47                 list(2);
  48                 restore();
  49                 return;
  50         }
  51
  52         n = p1->u.tensor->nelem;
  53
  54         a = p1->u.tensor->elem;
  55
  56         for (i = 0; i < n; i++)
  57                 if (!isnum(a[i]))
  58                         break;
  59
  60         if (i == n)
  61                 yyinvg();
  62         else {
  63                 push(p1);
  64                 adj();
  65                 push(p1);
  66                 det();
  67                 p2 = pop();
  68                 if (iszero(p2))
  69                         stop("inverse of singular matrix");
  70                 push(p2);
  71                 divide();
  72         }
  73
  74         restore();
  75 }
  76
  77 void
  78 invg(void)
  79 {
  80         save();
  81
  82         p1 = pop();
  83
  84         if (check_arg() == 0) {
  85                 push_symbol(INVG);
  86                 push(p1);
  87                 list(2);
  88                 restore();
  89                 return;
  90         }
  91
  92         yyinvg();
  93
  94         restore();
  95 }
  96
  97 // inverse using gaussian elimination
  98
  99 void
 100 yyinvg(void)
 101 {
 102         int h, i, j, n;
 103
 104         n = p1->u.tensor->dim[0];
 105
 106         h = tos;
 107
 108         for (i = 0; i < n; i++)
 109                 for (j = 0; j < n; j++)
 110                         if (i == j)
 111                                 push(one);
 112                         else
 113                                 push(zero);
 114
 115         for (i = 0; i < n * n; i++)
 116                 push(p1->u.tensor->elem[i]);
 117
 118         decomp(n);
 119
 120         p1 = alloc_tensor(n * n);
 121
 122         p1->u.tensor->ndim = 2;
 123         p1->u.tensor->dim[0] = n;
 124         p1->u.tensor->dim[1] = n;
 125
 126         for (i = 0; i < n * n; i++)
 127                 p1->u.tensor->elem[i] = stack[h + i];
 128
 129         tos -= 2 * n * n;
 130
 131         push(p1);
 132 }
 133
 134 //-----------------------------------------------------------------------------
 135 //
 136 //      Input:          n * n unit matrix on stack
 137 //
 138 //                      n * n operand on stack
 139 //
 140 //      Output:         n * n inverse matrix on stack
 141 //
 142 //                      n * n garbage on stack
 143 //
 144 //                      p2 mangled
 145 //
 146 //-----------------------------------------------------------------------------
 147
 148 #define A(i, j) stack[a + n * (i) + (j)]
 149 #define U(i, j) stack[u + n * (i) + (j)]
 150
 151 void
 152 decomp(int n)
 153 {
 154         int a, d, i, j, u;
 155
 156         a = tos - n * n;
 157
 158         u = a - n * n;
 159
 160         for (d = 0; d < n; d++) {
 161
 162                 // diagonal element zero?
 163
 164                 if (equal(A(d, d), zero)) {
 165
 166                         // find a new row
 167
 168                         for (i = d + 1; i < n; i++)
 169                                 if (!equal(A(i, d), zero))
 170                                         break;
 171
 172                         if (i == n)
 173                                 stop("inverse of singular matrix");
 174
 175                         // exchange rows
 176
 177                         for (j = 0; j < n; j++) {
 178
 179                                 p2 = A(d, j);
 180                                 A(d, j) = A(i, j);
 181                                 A(i, j) = p2;
 182
 183                                 p2 = U(d, j);
 184                                 U(d, j) = U(i, j);
 185                                 U(i, j) = p2;
 186                         }
 187                 }
 188
 189                 // multiply the pivot row by 1 / pivot
 190
 191                 p2 = A(d, d);
 192
 193                 for (j = 0; j < n; j++) {
 194
 195                         if (j > d) {
 196                                 push(A(d, j));
 197                                 push(p2);
 198                                 divide();
 199                                 A(d, j) = pop();
 200                         }
 201
 202                         push(U(d, j));
 203                         push(p2);
 204                         divide();
 205                         U(d, j) = pop();
 206                 }
 207
 208                 // clear out the column above and below the pivot
 209
 210                 for (i = 0; i < n; i++) {
 211
 212                         if (i == d)
 213                                 continue;
 214
 215                         // multiplier
 216
 217                         p2 = A(i, d);
 218
 219                         // add pivot row to i-th row
 220
 221                         for (j = 0; j < n; j++) {
 222
 223                                 if (j > d) {
 224                                         push(A(i, j));
 225                                         push(A(d, j));
 226                                         push(p2);
 227                                         multiply();
 228                                         subtract();
 229                                         A(i, j) = pop();
 230                                 }
 231
 232                                 push(U(i, j));
 233                                 push(U(d, j));
 234                                 push(p2);
 235                                 multiply();
 236                                 subtract();
 237                                 U(i, j) = pop();
 238                         }
 239                 }
 240         }
 241 }