# survey3d.c: (Matching .h file)

```/* survey3d.c - Standard three dimensional survey information processing.
*/
/* Version of September 6th, 1999 */

#include "errors.h"
/* We return package standard error codes. */

#include "mat.h"
/* Standard matrix operations. */

#include "qinv.h"
/* Matrix inversion */

#include "survey3d.h"
/* Our basic data types */

/* #include "survey3debug.h" */
/* #include "matdebug.h" */
/* DEBUG */

/* ---------------------- Conversions --------------------------------------- */
/* Load matrices from values. */

static error loads33 (double result[6],  double a11, double a12, double a13,
double a22, double a23,
double a33) {
if (!result) return ERRnil;
result[0] = a11; result[1] = a12; result[2] = a13;
result[3] = a22; result[4] = a23;
result[5] = a33;
return NoError;
}

error loadcvr  (covariance result, double a11, double a12, double a13,
double a22, double a23,
double a33) {
return loads33 (result, a11, a12, a13,  a22, a23,  a33);
}

error loadwht  (covariance result, double a11, double a12, double a13,
double a22, double a23,
double a33) {
return loads33 (result, a11, a12, a13,  a22, a23,  a33);
}

/* Make a regular matrix from a weight or covariance. */
static error matfms33 (double result[3][3], double *given) {
if (!result || !given) return ERRnil;

result[0][0] = given[0];   result[0][1] = given[1];  result[0][2] = given[2];
result[1][0] = given[1];   result[1][1] = given[3];  result[1][2] = given[4];
result[2][0] = given[2];   result[2][1] = given[4];  result[2][2] = given[5];

return NoError;
}

error matfmwht (double result[3][3], weight given) {
return matfms33 (result, given);
}

error matfmcvr (double result[3][3], covariance given) {
return matfms33 (result, given);
}

/* ---------------------- identities ---------------------------------------- */

/* Set coordinate to additive identity */
error zerocoord (coordinate result) {
if (!result) return ERRnil;
result[0] = 0.0;  result[1] = 0.0; result[2] = 0.0;
return NoError;
}

/* symmetric 3x3 identity */
static error ids33 (double *final) {
if (!final) return ERRnil;
final [0] = 1.0;      final [1] = 0.0;         final[2] = 0.0;
final [3] = 1.0;         final[4] = 0.0;
final[5] = 1.0;
return NoError;
}

/* Set covariance matrix to multiplicitive identity */
error cvrident  (covariance result) { return ids33 ((double *)result); }
/* Set weight matrix to multiplicitive identity */
error whtident  (weight     result) { return ids33 ((double *)result); }

/* ------------------- Add the basic entities ------------------------------- */
error addcoord (coordinate result, coordinate a, coordinate b) {
if (!result || !a || !b) return ERRnil;
result[0] = a[0] + b[0]; result[1] = a[1] + b[1]; result[2] = a[2] + b[2];
return NoError;
}

error subcoord (coordinate result, coordinate a, coordinate b) {
if (!result || !a || !b) return ERRnil;
result[0] = a[0] - b[0]; result[1] = a[1] - b[1]; result[2] = a[2] - b[2];
return NoError;
}

static error adds33 (double result[6], double a[6], double b[6]) {
if (!result || !a || !b) return ERRnil;
result[0] = a[0] + b[0]; result[1] = a[1] + b[1]; result[2] = a[2] + b[2];
result[3] = a[3] + b[3]; result[4] = a[4] + b[4];
result[5] = a[5] + b[5];
return NoError;
}

error addcvr  (covariance result, covariance a, covariance b) {
return adds33 ((double *)result, a, b);
}

error addwht  (weight     result, weight     a, weight     b) {
return adds33 ((double *)result, a, b);
}

/* ---------------- Multiply coordinate by weight --------------------------- */

/* weight by coordinate */
error wxcmult (coordinate result, weight a, coordinate b) {
double x, y, z;  /* Original vector */
/* Note that saving these before use not only makes the addressing
* cleaner, but also removes the restriction that a and result
* be distinct vectors.
*/
if (!result || !a || !b) return ERRnil;

/* We save the original coordinate off here, so that this
* actually works if result and b are the same location.
*/
x = b[0]; y = b[1]; z = b[2];

result[0] = a[0] * x  +  a[1] * y  +  a[2] * z;
result[1] = a[1] * x  +  a[3] * y  +  a[4] * z;
result[2] = a[2] * x  +  a[4] * y  +  a[5] * z;

return NoError;
}

/* ------------ convert between weights and covariances --------------------- */

static error invs33 (double result[6], double given[6]) {
double temp[3][3]; double outtemp[3][3];
error status;
if (!result || !given) return ERRnil;

temp[0][0] = given[0];     temp[0][1] = given[1];    temp[0][2] = given[2];
temp[1][0] = given[1];     temp[1][1] = given[3];    temp[1][2] = given[4];
temp[2][0] = given[2];     temp[2][1] = given[4];    temp[2][2] = given[5];

if ((status = invns (3, outtemp, temp, temp)))  return status;
result[0] =outtemp[0][0]; result[1] =outtemp[0][1]; result[2] =outtemp[0][2];
result[3] =outtemp[1][1]; result[4] =outtemp[1][2];
result[5] =outtemp[2][2];

return NoError;
}

error wht2cvr (covariance result, weight     given) {
return invs33 (result, given);
}

error cvr2wht (weight     result, covariance given) {
return invs33 (result, given);
}

/* ------------------ Invert weight or covariance matrices ------------------ */

error cvrinv (covariance result, covariance given) {
return invs33 (result, given);
}

error whtinv (weight result, weight given) {
return invs33 (result, given);
}

/* ------------------ Scale weight or covariance ---------------------------- */
/*  (used for setting unit weight and covariance)  */
/* Note that a reference variance or reference weight can't be negative or
* zero in any physically meaningful way.
*/

static error scls33 (double out[6], double in[6], double scale) {
if (!out || !in)  return ERRnil;
if (scale == 1.0) return NoError;
out[0] = scale * in[0];   out[1] = scale * in[1];   out[2] = scale * in[2];
out[3] = scale * in[3];   out[4] = scale * in[4];
out[5] = scale * in[5];
if (scale == 0.0) return ERRmeaning;
return NoError;
}

error sclcvr (covariance result, covariance given, double reference) {
return scls33 (result, given, reference);
}

error sclwht   (weight     result, weight     given, double reference) {
return scls33 (result, given, reference);
}

/* ------------------------  Magnitudes ------------------------------------- */

static error det3p (double *result, double given[6]) {
if (!result || !given) return ERRnil;
#define A00 given[0]
#define A01 given[1]
#define A02 given[2]
#define A10 A01
#define A11 given[3]
#define A12 given[4]
#define A20 A02
#define A21 A12
#define A22 given[5]
*result =   A00 * (A11 * A22  -  A21 * A12)
- A01 * (A10 * A22  -  A20 * A12)
+ A02 * (A10 * A21  -  A20 * A11);
return NoError;
}

error detcvr (double *result, covariance given) {
return det3p (result, given);
}
error detwht (double *result, weight     given) {
return det3p (result, given);
}
```