/* daixyz.c - Convert from Distance Azimuth Inclination to X Y Z
* © Copyright 1999 by John Halleck
* All Rights Reserved
*/
/* Version of August 13th, 1999 */
#include <math.h>
/* We need the trig functions. */
#include "errors.h"
/* We use our package wide error codes. */
#include "mat.h"
/* We will use some matrix operations. */
#include "matdebug.h"
/* we use the debugging routines to print and test matrices */
#include "survey3d.h"
/* Three dimensional covariances and weights. */
#include "daixyz.h"
/* We need the definition of our own routines */
double workcvr[3][3];
double workwht[3][3];
/* covariance matrices have a number of statistical properties that
* are necessary. No physical system can produce a matrix that
* violates these.
*/
error covarok (covariance given) {
if (!given) return ERRnil; /* The matrix must exist. */
/* It is positive definite, therefore: */
/* The diagonal elements must be POSITIVE */
/* Proof is in Golub and Van Loan's Matrix computations, page 141 */
if (X(given) <= 0.0 ||
Y(given) <= 0.0 ||
Z(given) <= 0.0) return ERRfalse;
/* No off diagonal element Gij can be larger than (Gii+Gjj)/2.0 */
/* Proof is in Golub and Van Loan's Matrix computations, page 141 */
if ( (2.0 * XY(given)) > (X(given) + Y(given)) ) return ERRfalse;
if ( (2.0 * XZ(given)) > (X(given) + Z(given)) ) return ERRfalse;
if ( (2.0 * YZ(given)) > (Y(given) + Z(given)) ) return ERRfalse;
/* Olly Betts <olly@muscat.co.uk> points out that a tighter bound exists,
* in that Mij^2 < Mii*Mjj
* This code needs to be fixed to use the tighter bound.
* (And to document the proof)
*/
return NoError;
}
error daixyz (xyz result, /* The final X, Y, Z */
covariance shotcovariance, /* covariance matrix of the result */
dai data, /* Initial data */
covariance datacovariance /* Covariance of data */
) {
double jacobian[3][3]; /* The jacobian matrix we will compute */
double sina, cosa, sini, cosi; /* Intermediate results */
double temp[3][3]; /* Another intermediate result */
error status; /* status out of matrix routines. */
if (!shotcovariance || !result || !data || !datacovariance) return ERRnil;
/* All matrices must be given */
#define x result[0]
#define y result[1]
#define z result[2]
#define D data[0]
#define A data[1]
#define I data[2]
if (D <= 0.0) return ERRnumeric; /* produces singular matrices. */
/* The standard conversion is:
* x = sine(A) * cosine(I) * D
* y = cosine(A) * cosine(I) * D
* z = sine(I) * D
*/
sina = sin(A); cosa = cos(A);
sini = sin(I); cosi = cos(I);
x = sina * cosi * D;
y = cosa * cosi * D;
z = sini * D;
/* Using P(a,b) to mean the partial derivative of a with respect to b,
* the jacobian of the standard transformation is:
*
* [ P(sin(A)*cos(I)*D,D) P(sin(A)*cos(I)*D,A) P(sin(A)*cos(I)*D,I) ]
* [ P(cos(A)*cos(I)*D,D) P(cos(A)*cos(I)*D,A) P(cos(A)*cos(I)*D,I) ]
* [ P(sin(I)*D,D) P(sin(I)*D,A) P(sin(I)*D,I) ]
*
* Which a little calculus reduces to:
*
* [ sin(A)*cos(I) cos(A)*cos(I)*D -sin(A)*sin(I)*D ]
* [ cos(A)*cos(I) -sin(A)*cos(I)*D -cos(A)*sin(I)*D ]
* [ sin(I) 0 cos(I)*D ]
*/
jacobian[0][0] = sina*cosi;
jacobian[0][1] = cosa*cosi*D;
jacobian[0][2] = -sina*sini*D;
jacobian[1][0] = cosa*cosi;
jacobian[1][1] = -sina*cosi*D;
jacobian[1][2] = -cosa*sini*D;
jacobian[2][0] = sini;
jacobian[2][1] = 0;
jacobian[2][2] = cosi*D;
/* The covariance of the final shots is just:
* Jacobian*datacovariance*Jacobian'
* (Where A' means A transpose)
*/
matprint ("Jacobian was:", 3, 3, jacobian);
if ( (status = matfmcvr (workcvr, datacovariance)) ) return status;
if ((status = matmul (3, 3, temp, 3, jacobian, workcvr) ))
return status;
matprint ("Temp was: ", 3, 3, temp);
if ((status = matmult (3, 3, workcvr, 3, temp, jacobian) ))
return status;
/* *************** UNFORTUNATELY ******************* */
/* The derivation above produces a jacobian that may not be full rank.
* (For example with a vertical shot) and may be rank deficient
* in general.
* Note that this doesn't make any real difference, since the
* Uncertainties of the end points can't be zero, so adding
* them will make it non-singular.
*
* Note that the underlying reason for this is that in the virtical
* case we are claiming three independent measurements, but only
* using two. Therefore that one makes no difference, and the
* partials it adds to the jacobian make it singular.
* Another approach is to handle that case as a case of only
* one significant measurement...
*/
shotcovariance[0] = workcvr[0][0];
shotcovariance[1] = workcvr[0][1];
shotcovariance[2] = workcvr[0][2];
shotcovariance[3] = workcvr[1][1];
shotcovariance[4] = workcvr[1][2];
shotcovariance[5] = workcvr[2][2];
return NoError;
}
This page is http://www.cc.utah.edu/~nahaj/cave/survey/code/c/daixyz.c.html
© Copyright 2000 by John Halleck, All Rights Reserved.
This snapshot was last modified on August 23rd, 2000
And the underlying file was last modified on May 11th, 2000