Ellipsoid3D CalcErrorEllipsoid(Mtrx3D *pcov, double del_chi_2) {
int ndx, iSwitched;
MatrixDouble A_matrix, V_matrix;
VectorDouble W_vector;
double wtemp, vtemp;
Ellipsoid3D ell;
int ierr = 0;
/* allocate A mtrx */
A_matrix = matrix_double(3, 3);
/* load A matrix in NumRec format */
A_matrix[0][0] = pcov->xx;
A_matrix[0][1] = A_matrix[1][0] = pcov->xy;
A_matrix[0][2] = A_matrix[2][0] = pcov->xz;
A_matrix[1][1] = pcov->yy;
A_matrix[1][2] = A_matrix[2][1] = pcov->yz;
A_matrix[2][2] = pcov->zz;
/* allocate V mtrx and W vector */
V_matrix = matrix_double(3, 3);
W_vector = vector_double(3);
/* do SVD */
//if ((istat = nll_svdcmp0(A_matrix, 3, 3, W_vector, V_matrix)) < 0) {
svd_helper(A_matrix, 3, 3, W_vector, V_matrix);
if (W_vector[0] < SMALL_DOUBLE || W_vector[1] < SMALL_DOUBLE || W_vector[2] < SMALL_DOUBLE) {
fprintf(stderr, "ERROR: invalid SVD singular value for confidence ellipsoids.");
ierr = 1;
} else {
/* sort by singular values W */
iSwitched = 1;
while (iSwitched) {
iSwitched = 0;
for (ndx = 0; ndx < 2; ndx++) {
if (W_vector[ndx] > W_vector[ndx + 1]) {
wtemp = W_vector[ndx];
W_vector[ndx] = W_vector[ndx + 1];
W_vector[ndx + 1] = wtemp;
vtemp = V_matrix[0][ndx];
V_matrix[0][ndx] = V_matrix[0][ndx + 1];
V_matrix[0][ndx + 1] = vtemp;
vtemp = V_matrix[1][ndx];
V_matrix[1][ndx] = V_matrix[1][ndx + 1];
V_matrix[1][ndx + 1] = vtemp;
vtemp = V_matrix[2][ndx];
V_matrix[2][ndx] = V_matrix[2][ndx + 1];
V_matrix[2][ndx + 1] = vtemp;
iSwitched = 1;
}
}
}
/* calculate ellipsoid axes */
/* length: w in Num Rec, 2nd ed, fig 15.6.5 must be replaced
by 1/sqrt(w) since we are using SVD of Cov mtrx and not
SVD of A mtrx (compare eqns 2.6.1 & 15.6.10) */
ell.az1 = atan2(V_matrix[0][0], V_matrix[1][0]) * RA2DE;
if (ell.az1 < 0.0)
ell.az1 += 360.0;
ell.dip1 = asin(V_matrix[2][0]) * RA2DE;
ell.len1 = sqrt(del_chi_2) / sqrt(1.0 / W_vector[0]);
ell.az2 = atan2(V_matrix[0][1], V_matrix[1][1]) * RA2DE;
if (ell.az2 < 0.0)
ell.az2 += 360.0;
ell.dip2 = asin(V_matrix[2][1]) * RA2DE;
ell.len2 = sqrt(del_chi_2) / sqrt(1.0 / W_vector[1]);
ell.len3 = sqrt(del_chi_2) / sqrt(1.0 / W_vector[2]);
}
free_matrix_double(A_matrix, 3, 3);
free_matrix_double(V_matrix, 3, 3);
free_vector_double(W_vector);
if (ierr) {
Ellipsoid3D EllipsoidNULL = {-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0};
return (EllipsoidNULL);
}
return (ell);
}
int
main( int argc, char *argv[] )
{
extern void dummy( void * );
float aa, *a, *b, *c, *x, *y;
double aad, *ad, *bd, *cd, *xd, *yd;
int i, j, n;
int inner = 0;
int vector = 0;
int matrix = 0;
int double_precision = 0;
int retval = PAPI_OK;
char papi_event_str[PAPI_MIN_STR_LEN] = "PAPI_FP_OPS";
int papi_event;
int EventSet = PAPI_NULL;
/* Parse the input arguments */
for ( i = 0; i < argc; i++ ) {
if ( strstr( argv[i], "-i" ) )
inner = 1;
else if ( strstr( argv[i], "-v" ) )
vector = 1;
else if ( strstr( argv[i], "-m" ) )
matrix = 1;
else if ( strstr( argv[i], "-e" ) ) {
if ( ( argv[i + 1] == NULL ) || ( strlen( argv[i + 1] ) == 0 ) ) {
print_help( argv );
exit( 1 );
}
strncpy( papi_event_str, argv[i + 1], sizeof ( papi_event_str ) );
i++;
} else if ( strstr( argv[i], "-d" ) )
double_precision = 1;
else if ( strstr( argv[i], "-h" ) ) {
print_help( argv );
exit( 1 );
}
}
/* if no options specified, set all tests to TRUE */
if ( inner + vector + matrix == 0 )
inner = vector = matrix = 1;
tests_quiet( argc, argv ); /* Set TESTS_QUIET variable */
if ( !TESTS_QUIET )
printf( "Initializing..." );
/* Initialize PAPI */
retval = PAPI_library_init( PAPI_VER_CURRENT );
if ( retval != PAPI_VER_CURRENT )
test_fail( __FILE__, __LINE__, "PAPI_library_init", retval );
/* Translate name */
retval = PAPI_event_name_to_code( papi_event_str, &papi_event );
if ( retval != PAPI_OK )
test_fail( __FILE__, __LINE__, "PAPI_event_name_to_code", retval );
if ( PAPI_query_event( papi_event ) != PAPI_OK )
test_skip( __FILE__, __LINE__, "PAPI_query_event", PAPI_ENOEVNT );
if ( ( retval = PAPI_create_eventset( &EventSet ) ) != PAPI_OK )
test_fail( __FILE__, __LINE__, "PAPI_create_eventset", retval );
if ( ( retval = PAPI_add_event( EventSet, papi_event ) ) != PAPI_OK )
test_fail( __FILE__, __LINE__, "PAPI_add_event", retval );
printf( "\n" );
retval = PAPI_OK;
/* Inner Product test */
if ( inner ) {
/* Allocate the linear arrays */
if (double_precision) {
xd = malloc( INDEX5 * sizeof(double) );
yd = malloc( INDEX5 * sizeof(double) );
if ( !( xd && yd ) )
retval = PAPI_ENOMEM;
}
else {
x = malloc( INDEX5 * sizeof(float) );
y = malloc( INDEX5 * sizeof(float) );
if ( !( x && y ) )
retval = PAPI_ENOMEM;
}
if ( retval == PAPI_OK ) {
headerlines( "Inner Product Test", TESTS_QUIET );
/* step through the different array sizes */
for ( n = 0; n < INDEX5; n++ ) {
if ( n < INDEX1 || ( ( n + 1 ) % 50 ) == 0 ) {
/* Initialize the needed arrays at this size */
if ( double_precision ) {
for ( i = 0; i <= n; i++ ) {
xd[i] = ( double ) rand( ) * ( double ) 1.1;
yd[i] = ( double ) rand( ) * ( double ) 1.1;
}
} else {
for ( i = 0; i <= n; i++ ) {
x[i] = ( float ) rand( ) * ( float ) 1.1;
y[i] = ( float ) rand( ) * ( float ) 1.1;
}
}
/* reset PAPI flops count */
reset_flops( "Inner Product Test", EventSet );
/* do the multiplication */
if ( double_precision ) {
aad = inner_double( n, xd, yd );
dummy( ( void * ) &aad );
} else {
aa = inner_single( n, x, y );
dummy( ( void * ) &aa );
}
resultline( n, 1, EventSet );
}
}
}
if (double_precision) {
free( xd );
free( yd );
} else {
free( x );
free( y );
}
}
/* Matrix Vector test */
if ( vector && retval != PAPI_ENOMEM ) {
/* Allocate the needed arrays */
if (double_precision) {
ad = malloc( INDEX5 * INDEX5 * sizeof(double) );
xd = malloc( INDEX5 * sizeof(double) );
yd = malloc( INDEX5 * sizeof(double) );
if ( !( ad && xd && yd ) )
retval = PAPI_ENOMEM;
} else {
a = malloc( INDEX5 * INDEX5 * sizeof(float) );
x = malloc( INDEX5 * sizeof(float) );
y = malloc( INDEX5 * sizeof(float) );
if ( !( a && x && y ) )
retval = PAPI_ENOMEM;
}
if ( retval == PAPI_OK ) {
headerlines( "Matrix Vector Test", TESTS_QUIET );
/* step through the different array sizes */
for ( n = 0; n < INDEX5; n++ ) {
if ( n < INDEX1 || ( ( n + 1 ) % 50 ) == 0 ) {
/* Initialize the needed arrays at this size */
if ( double_precision ) {
for ( i = 0; i <= n; i++ ) {
yd[i] = 0.0;
xd[i] = ( double ) rand( ) * ( double ) 1.1;
for ( j = 0; j <= n; j++ )
ad[i * n + j] =
( double ) rand( ) * ( double ) 1.1;
}
} else {
for ( i = 0; i <= n; i++ ) {
y[i] = 0.0;
x[i] = ( float ) rand( ) * ( float ) 1.1;
for ( j = 0; j <= n; j++ )
a[i * n + j] =
( float ) rand( ) * ( float ) 1.1;
}
}
/* reset PAPI flops count */
reset_flops( "Matrix Vector Test", EventSet );
/* compute the resultant vector */
if ( double_precision ) {
vector_double( n, ad, xd, yd );
dummy( ( void * ) yd );
} else {
vector_single( n, a, x, y );
dummy( ( void * ) y );
}
resultline( n, 2, EventSet );
}
}
}
if (double_precision) {
free( ad );
free( xd );
free( yd );
} else {
free( a );
free( x );
free( y );
}
}
/* Matrix Multiply test */
if ( matrix && retval != PAPI_ENOMEM ) {
/* Allocate the needed arrays */
if (double_precision) {
ad = malloc( INDEX5 * INDEX5 * sizeof(double) );
bd = malloc( INDEX5 * INDEX5 * sizeof(double) );
cd = malloc( INDEX5 * INDEX5 * sizeof(double) );
if ( !( ad && bd && cd ) )
retval = PAPI_ENOMEM;
} else {
a = malloc( INDEX5 * INDEX5 * sizeof(float) );
b = malloc( INDEX5 * INDEX5 * sizeof(float) );
c = malloc( INDEX5 * INDEX5 * sizeof(float) );
if ( !( a && b && c ) )
retval = PAPI_ENOMEM;
}
if ( retval == PAPI_OK ) {
headerlines( "Matrix Multiply Test", TESTS_QUIET );
/* step through the different array sizes */
for ( n = 0; n < INDEX5; n++ ) {
if ( n < INDEX1 || ( ( n + 1 ) % 50 ) == 0 ) {
/* Initialize the needed arrays at this size */
if ( double_precision ) {
for ( i = 0; i <= n * n + n; i++ ) {
cd[i] = 0.0;
ad[i] = ( double ) rand( ) * ( double ) 1.1;
bd[i] = ( double ) rand( ) * ( double ) 1.1;
}
} else {
for ( i = 0; i <= n * n + n; i++ ) {
c[i] = 0.0;
a[i] = ( float ) rand( ) * ( float ) 1.1;
b[i] = ( float ) rand( ) * ( float ) 1.1;
}
}
/* reset PAPI flops count */
reset_flops( "Matrix Multiply Test", EventSet );
/* compute the resultant matrix */
if ( double_precision ) {
matrix_double( n, cd, ad, bd );
dummy( ( void * ) c );
} else {
matrix_single( n, c, a, b );
dummy( ( void * ) c );
}
resultline( n, 3, EventSet );
}
}
}
if (double_precision) {
free( ad );
free( bd );
free( cd );
} else {
free( a );
free( b );
free( c );
}
}
/* exit with status code */
if ( retval == PAPI_ENOMEM )
test_fail( __FILE__, __LINE__, "malloc", retval );
else
test_pass( __FILE__, NULL, 0 );
exit( 1 );
}
/**
Constructs and returns a new singular value decomposition object;
The decomposed matrices can be retrieved via instance methods of the returned decomposition object.
@param A A rectangular matrix.
@return A decomposition object to access <tt>U</tt>, <tt>S</tt> and <tt>V</tt>.
@throws IllegalArgumentException if <tt>A.rows() < A.columns()</tt>.
*/
void SingularValueDecomposition(MatrixDouble A_matrix_orig, int nrows, int ncolumns) {
int i, j, k;
//Property.DEFAULT.checkRectangular(Arg);
// Derived from LINPACK code.
// Initialize.
num_rows = nrows;
num_columns = ncolumns;
// make local copy of original A matrix
MatrixDouble A_matrix = matrix_double(num_rows, num_columns);
for (i = 0; i < num_rows; i++) {
for (j = 0; j < num_columns; j++) {
A_matrix[i][j] = A_matrix_orig[i][j];
}
}
clean_SingularValueDecomposition();
int nu = Math_min(num_rows, num_columns);
singular_values = vector_double(Math_min(num_rows + 1, num_columns));
U_matrix = matrix_double(num_rows, nu);
V_matrix = matrix_double(num_columns, num_columns);
double *e = calloc(num_columns, sizeof (double));
double *work = calloc(num_rows, sizeof (double));
int wantu = 1;
int wantv = 1;
// Reduce A to bidiagonal form, storing the diagonal elements
// in s and the super-diagonal elements in e.
int nct = Math_min(num_rows - 1, num_columns);
int nrt = Math_max(0, Math_min(num_columns - 2, num_rows));
for (k = 0; k < Math_max(nct, nrt); k++) {
if (k < nct) {
// Compute the transformation for the k-th column and
// place the k-th diagonal in s[k].
// Compute 2-norm of k-th column without under/overflow.
singular_values[k] = 0;
for (i = k; i < num_rows; i++) {
singular_values[k] = Algebra_hypot(singular_values[k], A_matrix[i][k]);
}
if (singular_values[k] != 0.0) {
if (A_matrix[k][k] < 0.0) {
singular_values[k] = -singular_values[k];
}
for (i = k; i < num_rows; i++) {
A_matrix[i][k] /= singular_values[k];
}
A_matrix[k][k] += 1.0;
}
singular_values[k] = -singular_values[k];
}
for (j = k + 1; j < num_columns; j++) {
if ((k < nct) & (singular_values[k] != 0.0)) {
// Apply the transformation.
double t = 0;
for (i = k; i < num_rows; i++) {
t += A_matrix[i][k] * A_matrix[i][j];
}
t = -t / A_matrix[k][k];
for (i = k; i < num_rows; i++) {
A_matrix[i][j] += t * A_matrix[i][k];
}
}
// Place the k-th row of A into e for the
// subsequent calculation of the row transformation.
e[j] = A_matrix[k][j];
}
if (wantu & (k < nct)) {
// Place the transformation in U for subsequent back
// multiplication.
for (i = k; i < num_rows; i++) {
U_matrix[i][k] = A_matrix[i][k];
}
}
if (k < nrt) {
// Compute the k-th row transformation and place the
// k-th super-diagonal in e[k].
// Compute 2-norm without under/overflow.
e[k] = 0;
for (i = k + 1; i < num_columns; i++) {
e[k] = Algebra_hypot(e[k], e[i]);
}
if (e[k] != 0.0) {
if (e[k + 1] < 0.0) {
e[k] = -e[k];
}
for (i = k + 1; i < num_columns; i++) {
e[i] /= e[k];
}
e[k + 1] += 1.0;
}
e[k] = -e[k];
if ((k + 1 < num_rows) & (e[k] != 0.0)) {
// Apply the transformation.
for (i = k + 1; i < num_rows; i++) {
work[i] = 0.0;
}
for (j = k + 1; j < num_columns; j++) {
for (i = k + 1; i < num_rows; i++) {
work[i] += e[j] * A_matrix[i][j];
}
}
for (j = k + 1; j < num_columns; j++) {
double t = -e[j] / e[k + 1];
for (i = k + 1; i < num_rows; i++) {
A_matrix[i][j] += t * work[i];
}
}
}
if (wantv) {
// Place the transformation in V for subsequent
// back multiplication.
for (i = k + 1; i < num_columns; i++) {
V_matrix[i][k] = e[i];
}
}
}
}
// Set up the final bidiagonal matrix or order p.
int p = Math_min(num_columns, num_rows + 1);
if (nct < num_columns) {
singular_values[nct] = A_matrix[nct][nct];
}
if (num_rows < p) {
singular_values[p - 1] = 0.0;
}
if (nrt + 1 < p) {
e[nrt] = A_matrix[nrt][p - 1];
}
e[p - 1] = 0.0;
// If required, generate U.
if (wantu) {
for (j = nct; j < nu; j++) {
for (i = 0; i < num_rows; i++) {
U_matrix[i][j] = 0.0;
}
U_matrix[j][j] = 1.0;
}
for (k = nct - 1; k >= 0; k--) {
if (singular_values[k] != 0.0) {
for (j = k + 1; j < nu; j++) {
double t = 0;
for (i = k; i < num_rows; i++) {
t += U_matrix[i][k] * U_matrix[i][j];
}
t = -t / U_matrix[k][k];
for (i = k; i < num_rows; i++) {
U_matrix[i][j] += t * U_matrix[i][k];
}
}
for (i = k; i < num_rows; i++) {
U_matrix[i][k] = -U_matrix[i][k];
}
U_matrix[k][k] = 1.0 + U_matrix[k][k];
for (i = 0; i < k - 1; i++) {
U_matrix[i][k] = 0.0;
}
} else {
for (i = 0; i < num_rows; i++) {
U_matrix[i][k] = 0.0;
}
U_matrix[k][k] = 1.0;
}
}
}
// If required, generate V.
if (wantv) {
for (k = num_columns - 1; k >= 0; k--) {
if ((k < nrt) & (e[k] != 0.0)) {
for (j = k + 1; j < nu; j++) {
double t = 0;
for (i = k + 1; i < num_columns; i++) {
t += V_matrix[i][k] * V_matrix[i][j];
}
t = -t / V_matrix[k + 1][k];
for (i = k + 1; i < num_columns; i++) {
V_matrix[i][j] += t * V_matrix[i][k];
}
}
}
for (i = 0; i < num_columns; i++) {
V_matrix[i][k] = 0.0;
}
V_matrix[k][k] = 1.0;
}
}
// Main iteration loop for the singular values.
int pp = p - 1;
int iter = 0;
double eps = pow(2.0, -52.0);
while (p > 0) {
int k, kase;
// Here is where a test for too many iterations would go.
// This section of the program inspects for
// negligible elements in the s and e arrays. On
// completion the variables kase and k are set as follows.
// kase = 1 if s(p) and e[k-1] are negligible and k<p
// kase = 2 if s(k) is negligible and k<p
// kase = 3 if e[k-1] is negligible, k<p, and
// s(k), ..., s(p) are not negligible (qr step).
// kase = 4 if e(p-1) is negligible (convergence).
for (k = p - 2; k >= -1; k--) {
if (k == -1) {
break;
}
if (fabs(e[k]) <= eps * (fabs(singular_values[k]) + fabs(singular_values[k + 1]))) {
e[k] = 0.0;
break;
}
}
if (k == p - 2) {
kase = 4;
} else {
int ks;
for (ks = p - 1; ks >= k; ks--) {
if (ks == k) {
break;
}
double t = (ks != p ? fabs(e[ks]) : 0.) +
(ks != k + 1 ? fabs(e[ks - 1]) : 0.);
if (fabs(singular_values[ks]) <= eps * t) {
singular_values[ks] = 0.0;
break;
}
}
if (ks == k) {
kase = 3;
} else if (ks == p - 1) {
kase = 1;
} else {
kase = 2;
k = ks;
}
}
k++;
// Perform the task indicated by kase.
switch (kase) {
// Deflate negligible s(p).
case 1:
{
double f = e[p - 2];
e[p - 2] = 0.0;
for (j = p - 2; j >= k; j--) {
double t = Algebra_hypot(singular_values[j], f);
double cs = singular_values[j] / t;
double sn = f / t;
singular_values[j] = t;
if (j != k) {
f = -sn * e[j - 1];
e[j - 1] = cs * e[j - 1];
}
if (wantv) {
for (i = 0; i < num_columns; i++) {
t = cs * V_matrix[i][j] + sn * V_matrix[i][p - 1];
V_matrix[i][p - 1] = -sn * V_matrix[i][j] + cs * V_matrix[i][p - 1];
V_matrix[i][j] = t;
}
}
}
}
break;
// Split at negligible s(k).
case 2:
{
double f = e[k - 1];
e[k - 1] = 0.0;
for (j = k; j < p; j++) {
double t = Algebra_hypot(singular_values[j], f);
double cs = singular_values[j] / t;
double sn = f / t;
singular_values[j] = t;
f = -sn * e[j];
e[j] = cs * e[j];
if (wantu) {
for (i = 0; i < num_rows; i++) {
t = cs * U_matrix[i][j] + sn * U_matrix[i][k - 1];
U_matrix[i][k - 1] = -sn * U_matrix[i][j] + cs * U_matrix[i][k - 1];
U_matrix[i][j] = t;
}
}
}
}
break;
// Perform one qr step.
case 3:
{
// Calculate the shift.
double scale = Math_max(Math_max(Math_max(Math_max(
fabs(singular_values[p - 1]), fabs(singular_values[p - 2])), fabs(e[p - 2])),
fabs(singular_values[k])), fabs(e[k]));
double sp = singular_values[p - 1] / scale;
double spm1 = singular_values[p - 2] / scale;
double epm1 = e[p - 2] / scale;
double sk = singular_values[k] / scale;
double ek = e[k] / scale;
double b = ((spm1 + sp)*(spm1 - sp) + epm1 * epm1) / 2.0;
double c = (sp * epm1)*(sp * epm1);
double shift = 0.0;
if ((b != 0.0) | (c != 0.0)) {
shift = sqrt(b * b + c);
if (b < 0.0) {
shift = -shift;
}
shift = c / (b + shift);
}
double f = (sk + sp)*(sk - sp) + shift;
double g = sk*ek;
// Chase zeros.
for (j = k; j < p - 1; j++) {
double t = Algebra_hypot(f, g);
double cs = f / t;
double sn = g / t;
if (j != k) {
e[j - 1] = t;
}
f = cs * singular_values[j] + sn * e[j];
e[j] = cs * e[j] - sn * singular_values[j];
g = sn * singular_values[j + 1];
singular_values[j + 1] = cs * singular_values[j + 1];
if (wantv) {
for (i = 0; i < num_columns; i++) {
t = cs * V_matrix[i][j] + sn * V_matrix[i][j + 1];
V_matrix[i][j + 1] = -sn * V_matrix[i][j] + cs * V_matrix[i][j + 1];
V_matrix[i][j] = t;
}
}
t = Algebra_hypot(f, g);
cs = f / t;
sn = g / t;
singular_values[j] = t;
f = cs * e[j] + sn * singular_values[j + 1];
singular_values[j + 1] = -sn * e[j] + cs * singular_values[j + 1];
g = sn * e[j + 1];
e[j + 1] = cs * e[j + 1];
if (wantu && (j < num_rows - 1)) {
for (i = 0; i < num_rows; i++) {
t = cs * U_matrix[i][j] + sn * U_matrix[i][j + 1];
U_matrix[i][j + 1] = -sn * U_matrix[i][j] + cs * U_matrix[i][j + 1];
U_matrix[i][j] = t;
}
}
}
e[p - 2] = f;
iter = iter + 1;
}
break;
// Convergence.
case 4:
{
// Make the singular values positive.
if (singular_values[k] <= 0.0) {
singular_values[k] = (singular_values[k] < 0.0 ? -singular_values[k] : 0.0);
if (wantv) {
for (i = 0; i <= pp; i++) {
V_matrix[i][k] = -V_matrix[i][k];
}
}
}
// Order the singular values.
while (k < pp) {
if (singular_values[k] >= singular_values[k + 1]) {
break;
}
double t = singular_values[k];
singular_values[k] = singular_values[k + 1];
singular_values[k + 1] = t;
if (wantv && (k < num_columns - 1)) {
for (i = 0; i < num_columns; i++) {
t = V_matrix[i][k + 1];
V_matrix[i][k + 1] = V_matrix[i][k];
V_matrix[i][k] = t;
}
}
if (wantu && (k < num_rows - 1)) {
for (i = 0; i < num_rows; i++) {
t = U_matrix[i][k + 1];
U_matrix[i][k + 1] = U_matrix[i][k];
U_matrix[i][k] = t;
}
}
k++;
}
iter = 0;
p--;
}
break;
}
}
// clean up
free(e);
e = NULL;
free(work);
work = NULL;
free_matrix_double(A_matrix, num_rows, nu);
}
