#ident "$Id: NeRDS.c,v 1.5 2015/03/20 17:13:46 pwh Exp $"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "NeRDS.h"
/* Defines to enable/disable features. */
/* #define DEBUG */ /* Print debugging information. */
#define VERBOSE /* Print error messages. */
#define NAN_CHECK /* Check for infinite results. */
#define PIVOT /* Avoid divide by zero errors. */
/* Adjust the sensitivity of divide by zero detection. */
#define NEAR_ZERO 1.0e-30 /* How near zero for divide by zero error? */
/* Make sure we are in VERBOSE mode when debugging. */
#ifdef DEBUG
#define VERBOSE
#endif
int NeRDinversion ( int n, double **original, double **inverse )
{
int status = 0;
void *scratchPad;
if ( n < 1 ) {
#ifdef VERBOSE
fprintf ( stderr,
"Error: NeRDinversion () parameter 1 (n=%d) cannot be < 1.\n", n );
#endif /* VERBOSE */
status = NERD_PARAM_CHECK;
} else if ( ! original ) {
#ifdef VERBOSE
fprintf ( stderr,
"Error: NeRDinversion () parameter 2 (original) cannot be NULL.\n" );
#endif /* VERBOSE */
status = NERD_PARAM_CHECK;
} else if ( ! ( scratchPad = malloc ( n * ( sizeof ( double )
#ifdef PIVOT
+ sizeof ( int ) + sizeof ( int )
#endif /* PIVOT */
) ) ) ) {
#ifdef VERBOSE
fprintf ( stderr,
"Error: NeRDinversion () could not allocate scratch pad memory.\n" );
#endif /* VERBOSE */
status = NERD_MEMORY_CHECK;
} else {
int i;
double *rD = ( double * ) scratchPad;
double scale;
double gamma;
#ifdef PIVOT
int *order = ( int * ) ( rD + n );
int *rows = order + n;
int pivot;
#endif /* PIVOT */
if ( ! inverse ) inverse = original;
#ifdef PIVOT
/* Build permutation "matrices". */
for ( i = 0; i < n; ++i ) order [i] = rows [i] = i;
/* Pre-pivot the 1st column. */
pivot = 0;
gamma = fabs ( original [0][0] );
for ( i = 1; i < n; ++i ) {
scale = abs ( original [i][0] );
if ( scale > gamma ) {
gamma = scale;
pivot = i;
}
}
if ( fpclassify ( gamma * gamma ) & ( FP_ZERO | FP_SUBNORMAL ) ) {
status = NERD_ZERO_CHECK;
#ifdef VERBOSE
fprintf ( stderr, "Error: Divide by 0 in column 1.\n" );
#endif /* VERBOSE */
} else if ( pivot != 0 ) {
#ifdef DEBUG
fprintf ( stderr, "Swapping row 1 and %d.\n", pivot + 1 );
#endif /* DEBUG */
rows [0] = pivot;
rows [pivot] = 0;
}
#endif /* PIVOT */
/* Compute inverse, one row at a time. */
for ( i = 0; ! status && i < n; ++i ) {
int j;
#ifdef PIVOT
int iRow;
int iOrder;
pivot = i;
status = NERD_ZERO_CHECK;
/* Compute gamma. */
while ( status && pivot < n ) {
double term;
scale = 0.0;
iOrder = order [pivot];
iRow = rows [iOrder];
gamma = 0.0;
for ( j = 0; j < i; ++j ) {
term = original [iRow][order [j]]
* inverse [rows [order [j]]][iRow];
gamma += term;
scale += term * term;
}
term = original [iRow][iOrder];
gamma += term;
if ( ( fpclassify ( scale ) ) == FP_NORMAL ) scale
= gamma * gamma / scale;
else scale = gamma * gamma;
#ifdef DEBUG
fprintf ( stderr, "Zero check = %13lg\n", scale );
#endif /* DEBUG */
if ( i > 0 && scale < NEAR_ZERO ) ++pivot;
else status = 0;
}
if ( ! status ) {
if ( pivot != i ) {
#ifdef DEBUG
fprintf ( stderr,
"Swapping row[%d] = %d with row[%d] = %d.\n",
i + 1, rows [order [i]] + 1,
pivot + 1, rows [order [pivot]] + 1 );
#endif /* DEBUG */
order [pivot] = order [i];
order [i] = iOrder;
}
rD [iOrder] = 1.0 - gamma;
/* Compute rD vector. */
for ( j = 0; j < n; ++j ) {
if ( j != iOrder ) {
int k;
int jRow = rows [j];
rD [j] = 0.0;
for ( k = 0; k < i; ++k ) rD [j]
-= original [iRow][order [k]]
* inverse [rows [order [k]]][jRow];
}
}
for ( j = i + 1; j < n; ++j ) rD [order [j]]
-= original [iRow][order [j]];
/* Subtract DcrD/gamma from D */
for ( j = 0; j < i; ++j ) {
int k;
int jRow = rows [order [j]];
scale = inverse [jRow][iRow] / gamma;
for ( k = 0; k < n; ++k )
inverse [jRow][rows [k]]
+= scale * rD [k];
}
for ( j = 0; j < n; ++j ) inverse [iRow][rows [j]]
= rD [j] / gamma;
inverse [iRow][iRow] += 1.0;
#ifdef DEBUG
for ( j = 0; j <= i; ++j ) {
int k;
int tempRow = rows [order [j]];
for ( k = 0; k < n; ++k ) fprintf ( stderr,
"%13lg", inverse [tempRow][rows [k]] );
fputc ( '\n', stderr );
}
fputc ( '\n', stderr );
#endif /* DEBUG */
}
#ifdef VERBOSE
else fprintf ( stderr,
"Error: Divide by 0 in row[%d] = %d.\n",
i + 1, order [i] + 1 );
#endif /* VERBOSE */
#else /* ! PIVOT */
scale = 0.0;
/* Compute rD vector. */
for ( j = 0; j < n; ++j ) {
int k;
rD [j] = 0.0;
for ( k = 0; k < i; ++k ) {
double term = original [i][k] * inverse [k][j];
rD [j] -= term;
if ( j == i ) scale += term * term;
}
}
for ( j = i; j < n; ++j ) rD [j] -= original [i][j];
gamma = -rD [i];
scale += original [i][i] * original [i][i];
if ( fpclassify ( scale ) == FP_NORMAL ) scale
= gamma * gamma / scale;
else scale = gamma * gamma;
#ifdef DEBUG
fprintf ( stderr, "Zero check = %13lg\n", scale );
#endif /* DEBUG */
if ( scale < NEAR_ZERO ) {
#ifdef VERBOSE
fprintf ( stderr, "Error: Divide by 0 in row %d.\n",
i + 1 );
#endif /* VERBOSE */
status = NERD_ZERO_CHECK;
} else {
rD [i] += 1.0;
/* Subtract DcrD/gamma from D */
for ( j = 0; j < i; ++j ) {
int k;
scale = inverse [j][i] / gamma;
for ( k = 0; k < n; ++k ) inverse [j][k]
+= scale * rD [k];
}
for ( j = 0; j < n; ++j ) inverse [i][j]
= rD [j] / gamma;
inverse [i][i] += 1.0;
#ifdef DEBUG
for ( j = 0; j <= i; ++j ) {
int k;
for ( k = 0; k < n; ++k ) fprintf ( stderr,
"%13lg", inverse [j][k] );
fputc ( '\n', stderr );
}
fputc ( '\n', stderr );
#endif /* DEBUG */
}
#endif /* ! PIVOT */
}
#if defined (PIVOT) || defined (NAN_CHECK)
/* Permute rows of inverse. */
if ( ! status ) {
#ifdef PIVOT
if ( rows [0] != 0 ) {
double *temp = inverse [0];
inverse [0] = inverse [rows [0]];
inverse [rows [0]] = temp;
}
#endif /* PIVOT */
#ifdef NAN_CHECK
for ( i = 0; ! status && i < n; ++i ) {
int j;
for ( j = 0; ! status && j < n; ++j )
if ( ! isfinite ( inverse [i][j] ) ) {
status = NERD_NAN_CHECK;
#ifdef VERBOSE
fprintf ( stderr,
"Error: Non-finite result detected.\n" );
#endif /* VERBOSE */
}
}
#endif /* NAN_CHECK */
}
#endif /* PIVOT || NAN_CHECK */
free ( scratchPad );
}
return ( status );
}
Last updated: Friday, June 12, 2015 08:21:43 AM