eigenmath/inv.cpp

242 lines
3.3 KiB
C++

//-----------------------------------------------------------------------------
//
// Input: Matrix on stack
//
// Output: Inverse on stack
//
// Example:
//
// > inv(((1,2),(3,4))
// ((-2,1),(3/2,-1/2))
//
// Note:
//
// Uses Gaussian elimination for numerical matrices.
//
//-----------------------------------------------------------------------------
#include "stdafx.h"
#include "defs.h"
static int
check_arg(void)
{
if (!istensor(p1))
return 0;
else if (p1->u.tensor->ndim != 2)
return 0;
else if (p1->u.tensor->dim[0] != p1->u.tensor->dim[1])
return 0;
else
return 1;
}
void
inv(void)
{
int i, n;
U **a;
save();
p1 = pop();
if (check_arg() == 0) {
push_symbol(INV);
push(p1);
list(2);
restore();
return;
}
n = p1->u.tensor->nelem;
a = p1->u.tensor->elem;
for (i = 0; i < n; i++)
if (!isnum(a[i]))
break;
if (i == n)
yyinvg();
else {
push(p1);
adj();
push(p1);
det();
p2 = pop();
if (iszero(p2))
stop("inverse of singular matrix");
push(p2);
divide();
}
restore();
}
void
invg(void)
{
save();
p1 = pop();
if (check_arg() == 0) {
push_symbol(INVG);
push(p1);
list(2);
restore();
return;
}
yyinvg();
restore();
}
// inverse using gaussian elimination
void
yyinvg(void)
{
int h, i, j, n;
n = p1->u.tensor->dim[0];
h = tos;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
if (i == j)
push(one);
else
push(zero);
for (i = 0; i < n * n; i++)
push(p1->u.tensor->elem[i]);
decomp(n);
p1 = alloc_tensor(n * n);
p1->u.tensor->ndim = 2;
p1->u.tensor->dim[0] = n;
p1->u.tensor->dim[1] = n;
for (i = 0; i < n * n; i++)
p1->u.tensor->elem[i] = stack[h + i];
tos -= 2 * n * n;
push(p1);
}
//-----------------------------------------------------------------------------
//
// Input: n * n unit matrix on stack
//
// n * n operand on stack
//
// Output: n * n inverse matrix on stack
//
// n * n garbage on stack
//
// p2 mangled
//
//-----------------------------------------------------------------------------
#define A(i, j) stack[a + n * (i) + (j)]
#define U(i, j) stack[u + n * (i) + (j)]
void
decomp(int n)
{
int a, d, i, j, u;
a = tos - n * n;
u = a - n * n;
for (d = 0; d < n; d++) {
// diagonal element zero?
if (equal(A(d, d), zero)) {
// find a new row
for (i = d + 1; i < n; i++)
if (!equal(A(i, d), zero))
break;
if (i == n)
stop("inverse of singular matrix");
// exchange rows
for (j = 0; j < n; j++) {
p2 = A(d, j);
A(d, j) = A(i, j);
A(i, j) = p2;
p2 = U(d, j);
U(d, j) = U(i, j);
U(i, j) = p2;
}
}
// multiply the pivot row by 1 / pivot
p2 = A(d, d);
for (j = 0; j < n; j++) {
if (j > d) {
push(A(d, j));
push(p2);
divide();
A(d, j) = pop();
}
push(U(d, j));
push(p2);
divide();
U(d, j) = pop();
}
// clear out the column above and below the pivot
for (i = 0; i < n; i++) {
if (i == d)
continue;
// multiplier
p2 = A(i, d);
// add pivot row to i-th row
for (j = 0; j < n; j++) {
if (j > d) {
push(A(i, j));
push(A(d, j));
push(p2);
multiply();
subtract();
A(i, j) = pop();
}
push(U(i, j));
push(U(d, j));
push(p2);
multiply();
subtract();
U(i, j) = pop();
}
}
}
}