/* survey3d.c - Standard three dimensional survey information processing.
* © Copyright 1999 by John Halleck
* All Rights Reserved.
*/
/* 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);
}
This page is http://www.cc.utah.edu/~nahaj/cave/survey/code/c/survey3d.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