eigenmath/det.cpp

298 lines
4.0 KiB
C++

//-----------------------------------------------------------------------------
//
// Input: Matrix on stack
//
// Output: Determinant on stack
//
// Example:
//
// > det(((1,2),(3,4)))
// -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
det(void)
{
int i, n;
U **a;
save();
p1 = pop();
if (check_arg() == 0) {
push_symbol(DET);
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)
yydetg();
else {
for (i = 0; i < p1->u.tensor->nelem; i++)
push(p1->u.tensor->elem[i]);
determinant(p1->u.tensor->dim[0]);
}
restore();
}
// determinant of n * n matrix elements on the stack
void
determinant(int n)
{
int h, i, j, k, q, s, sign, t;
int *a, *c, *d;
h = tos - n * n;
a = (int *) malloc(3 * n * sizeof (int));
if (a == NULL)
out_of_memory();
c = a + n;
d = c + n;
for (i = 0; i < n; i++) {
a[i] = i;
c[i] = 0;
d[i] = 1;
}
sign = 1;
push(zero);
for (;;) {
if (sign == 1)
push_integer(1);
else
push_integer(-1);
for (i = 0; i < n; i++) {
k = n * a[i] + i;
push(stack[h + k]);
multiply(); // FIXME -- problem here
}
add();
/* next permutation (Knuth's algorithm P) */
j = n - 1;
s = 0;
P4: q = c[j] + d[j];
if (q < 0) {
d[j] = -d[j];
j--;
goto P4;
}
if (q == j + 1) {
if (j == 0)
break;
s++;
d[j] = -d[j];
j--;
goto P4;
}
t = a[j - c[j] + s];
a[j - c[j] + s] = a[j - q + s];
a[j - q + s] = t;
c[j] = q;
sign = -sign;
}
free(a);
stack[h] = stack[tos - 1];
tos = h + 1;
}
//-----------------------------------------------------------------------------
//
// Input: Matrix on stack
//
// Output: Determinant on stack
//
// Note:
//
// Uses Gaussian elimination which is faster for numerical matrices.
//
// Gaussian Elimination works by walking down the diagonal and clearing
// out the columns below it.
//
//-----------------------------------------------------------------------------
void
detg(void)
{
save();
p1 = pop();
if (check_arg() == 0) {
push_symbol(DET);
push(p1);
list(2);
restore();
return;
}
yydetg();
restore();
}
void
yydetg(void)
{
int i, n;
n = p1->u.tensor->dim[0];
for (i = 0; i < n * n; i++)
push(p1->u.tensor->elem[i]);
lu_decomp(n);
tos -= n * n;
push(p1);
}
//-----------------------------------------------------------------------------
//
// Input: n * n matrix elements on stack
//
// Output: p1 determinant
//
// p2 mangled
//
// upper diagonal matrix on stack
//
//-----------------------------------------------------------------------------
#define M(i, j) stack[h + n * (i) + (j)]
void
lu_decomp(int n)
{
int d, h, i, j;
h = tos - n * n;
p1 = one;
for (d = 0; d < n - 1; d++) {
// diagonal element zero?
if (equal(M(d, d), zero)) {
// find a new row
for (i = d + 1; i < n; i++)
if (!equal(M(i, d), zero))
break;
if (i == n) {
p1 = zero;
break;
}
// exchange rows
for (j = d; j < n; j++) {
p2 = M(d, j);
M(d, j) = M(i, j);
M(i, j) = p2;
}
// negate det
push(p1);
negate();
p1 = pop();
}
// update det
push(p1);
push(M(d, d));
multiply();
p1 = pop();
// update lower diagonal matrix
for (i = d + 1; i < n; i++) {
// multiplier
push(M(i, d));
push(M(d, d));
divide();
negate();
p2 = pop();
// update one row
M(i, d) = zero; // clear column below pivot d
for (j = d + 1; j < n; j++) {
push(M(d, j));
push(p2);
multiply();
push(M(i, j));
add();
M(i, j) = pop();
}
}
}
// last diagonal element
push(p1);
push(M(n - 1, n - 1));
multiply();
p1 = pop();
}