static real internal_invert(MxMatrix& A, MxMatrix& B)
{
uint N = A.dim();
uint i, j, k;
real max, t, det, pivot;
/*---------- forward elimination ----------*/
for (i=0; i<N; i++) /* put identity matrix in B */
for (j=0; j<N; j++)
B(i, j) = (real)(i==j);
det = 1.0;
for (i=0; i<N; i++) { /* eliminate in column i, below diag */
max = -1.;
for (k=i; k<N; k++) /* find pivot for column i */
if (fabs(A(k, i)) > max) {
max = fabs(A(k, i));
j = k;
}
if (max<=0.) return 0.; /* if no nonzero pivot, PUNT */
if (j!=i) { /* swap rows i and j */
for (k=i; k<N; k++)
SWAP(A(i, k), A(j, k), t);
for (k=0; k<N; k++)
SWAP(B(i, k), B(j, k), t);
det = -det;
}
pivot = A(i, i);
det *= pivot;
for (k=i+1; k<N; k++) /* only do elems to right of pivot */
A(i, k) /= pivot;
for (k=0; k<N; k++)
B(i, k) /= pivot;
/* we know that A(i, i) will be set to 1, so don't bother to do it */
for (j=i+1; j<N; j++) { /* eliminate in rows below i */
t = A(j, i); /* we're gonna zero this guy */
for (k=i+1; k<N; k++) /* subtract scaled row i from row j */
A(j, k) -= A(i, k)*t; /* (ignore k<=i, we know they're 0) */
for (k=0; k<N; k++)
B(j, k) -= B(i, k)*t;
}
}
/*---------- backward elimination ----------*/
for (i=N-1; i>0; i--) { /* eliminate in column i, above diag */
for (j=0; j<i; j++) { /* eliminate in rows above i */
t = A(j, i); /* we're gonna zero this guy */
for (k=0; k<N; k++) /* subtract scaled row i from row j */
B(j, k) -= B(i, k)*t;
}
}
return det;
}
static
void symmetric_subfrom(MxMatrix& A, const MxVector& a, const MxVector& b)
{
for(unsigned int i=0; i<A.dim(); i++) for(unsigned int j=0; j<A.dim(); j++)
A(i,j) -= a[i]*b[j];
}
