/*
** Integrated holistic system.
**
** Performs everything to do with my project.
**
** I am not kidding.
**
** Actually, my project had consisted of a set of separate
** programs up until the end of the first semester. I then
** integrated them into one pseudo-library of routines.
** This means that I can access the functions individually
** for testing, or use high-level routines to generate
** example data.
**
** All access is via the command line. Syntax is given
** below for the different modes together with a brief
** description of what they do.
**
** Example command line:
**   integrate <mode> <arguments>
** 
**  calculate <calculation method> <input file> <output file>
**    Calculates affine coordinates from the input correspondence data
**   using the method specified. For more details about the methods
**   currently supported, type "integrate calculate".
**
**  calc-analyse <calculation method> <input file> <reference output>
**    Calculates affine coordinates from the input correspondence data
**   using the method specified. The coordinates are then compared to the
**   given reference coordinates and the average error is then output.
**
**  analyse <affine file> <reference affine coordinates>
**    Compares the affine coordinates with the reference coordinates given
**   and outputs the average error.
**
**  project <3D model file> <yaw> <pitch> <roll> <output correspondences>
**    Projects the given 3D model onto the pair of virtual cameras
**   specified in the model. The second camera is mis-aligned by the given
**   yaw, pitch and roll angles in degrees before the projection is done.
**   This generates an output simulated correspondence file.
**
**  noise <correspondence file> <noise in %> <output correspondences>
**    This adds noise to a correspondence file. The magnitude of this
**   random offset varies from 0 to (noise% * the distance from p0 to p3),
**   for each correspondence point.
**
**  errortest <3D model file> <distortion file> <output error summary>
**    This takes the 3D model and distorts it according to each distortion
**   found in the distortion file. The affine coordinates are then calculated
**   using each method from 0 - 7 and the average error produced by each
**   method is output to the summary file. This is no longer useful, given
**   the alternative of generating triangle files and viewing them in 3D,
**   which is much better than just obtaining a number describing the
**   `average error`.
**
**  rotationmap <3D model file> <calculation method> <yaw +/-> <pitch +/->
**              <image width> <image height> <output files prefix>
**    This distorts the given 3D model file by misaligning the cameras by
**   values from -yaw to +yaw and -pitch to +pitch. The error produced by
**   the calculation method is displayed in an image format, with yaw and
**   pitch along the two axes. The main image is prefix_afErr.pgm; other
**   pictures that convey subsidiary iteration are output as prefix_chi2.pgm,
**   prefix_d1.pgm, prefix_d2.pgm, prefix_dMag.pgm, prefix_numIt.pgm, and
**   prefix_ochi2.pgm.
**
**  deltamap <3D model file> <yaw> <pitch> <image width> <image height>
**           <output PGM filename>
**    This distorts the 3D model file by misaligning the cameras by the given
**   yaw and pitch angles. An image is then made of how well the delta-
**   correction makes the correspondence lines converge for different values
**   of delta. This is useful for verifying that it is possible to find
**   a correct value of delta, and also for validating the theory behind
**   delta-correction.
**
**  warpmap <3D model file> <yaw> <pitch> <image width> <image height>
**          <output PGM filename>
**    This distorts the 3D model file by misaligning the cameras by the given
**   yaw and pitch angles. An image is then made of how well warp-
**   correction makes the correspondence lines converge for different values
**   of the warp. This is useful for verifying that it is possible to find
**   a correct warp, and also for validating the theory behind
**   warp-correction.
**
**  eTtest <test correspondence file>
**    This finds the best point where the correspondence lines converge
**   and how well they converge at that point. Used to verify that the
**   chi^2 estimate is working correctly (you have to keep your eye on it,
**   or else it might do evil things).
**
**  chrisdebug <various args>
**    Do not use this mode. You have been warned.
**
**  gentriangles <3D model file> <yaw> <pitch> <roll> <noise %>
**               <calculation method> <connectivity file>
**               <output triangle file>
**    This takes the 3D model file, misaligns the cameras by (yaw, pitch,
**   roll), and calculates affine coordinates using the specified method.
**   These are used with the connectivity file to generate a set of
**   triangles from the affine point coordinates, which is written as
**   the output distorted triangle file.
**
**  pixelize <input correspondences> <pixel grid spacing> <output file>
**    The input correspondences are snapped to the closest point on a
**   simulated pixel grid constructed on the viewplane. They are then
**   output to a different file.
**
**  triconvert <affine file> <connectivity file> <triangle file>
**    This makes a triangle file from an affine file plus the connections
**   between points.
**
**  Chris Butcher, 29 May 1996
**
*/

#define DEBUGINFO 0
#define IMPORTANTDEBUGINFO 0

#define MAX_NUMPOINTS        20000

#include <string.h>
#include <timers.h>

/* getclock() is used to seed the random number generator */
extern int getclock(int clock_type, struct timespec *tp);

/* mathdefs.h contains vector, matrix definitions and math codes */
#include "mathdefs.h"

/****** FUNCTION PROTOTYPES ******/
/*** Command line analysis ***/
  void print_usage(int argc, char **argv);
  void calculate(int argc, char **argv);
  void calc_analyse(int argc, char **argv);
  void analyse(int argc, char **argv);
  void project(int argc, char **argv);
  void doNoise(int argc, char **argv);
  void errortest(int argc, char **argv);
  void rotationmap(int argc, char **argv);
  void deltamap(int argc, char **argv);
  void warpmap(int argc, char **argv);
  void eTtest(int argc, char **argv);
  void gentriangles(int argc, char **argv);
  void chrisdebug(int argc, char **argv);
  void pixelize(int argc, char **argv);
  void triconvert(int argc, char **argv);

/*** Calculation functions ***/
  /* === Affine Calculation functions - 
              both with/without delta and warp correction === */
  vector *AffineCalc(vector *p_ref1, vector *p_ref2, vector *points1,
                     vector *points2, int approx_method, int do_axes);
  void getAffine(vector p, vector p_prime, vector p_affine);
  void calculate_constant_points(void);

  /* normal delta-correction functions */
  double get_best_eT(double deltaX, double deltaY);
  void adjust_delta(void);
  double steepest_delta(double initial_x, double initial_y);
  void apply_delta(void);
  
  /* new warp-correction functions */
  double get_best_warped_eT(double yaw, double pitch);
  void prepare_warp_matrix(double yaw, double pitch);
  void warp_point(vector thePoint, vector *warped_point);
  void adjust_warp(void);
  double steepest_warp(double initial_x, double initial_y);
  void apply_warp(void);

  /* functions to perform simplex minimization */
  void adjust_simplex_delta(double initialx, double initialy,
                            double (*somefunc_vec)(double *vec));
/* 2D minimization, 3 points comprise the simplex */
#define N_DIM             2
#define N_PTS             N_DIM+1
  double simplex_try_point(double point[N_PTS][N_DIM], double func[N_PTS],
         double pointsum[N_DIM], double (*evalfunc)(double *vec), 
         int hi_point, int *n_evals, double factor);
  double simplex_eT_interface(double *vec);
  void fix(vector toFix);
  void adjust_axes(void);
  void readjust_axes(void);

  /* === Analysis functions - 
               operates on reference and actual affine coordinates === */
  float average_error(vector *refpoints, vector *affpoints);

  /* === Projection functions - 
               project 3D points onto simulated cameras to give
               perfect correspondences === */
  void rotate_project3d(double yaw, double pitch, double rollc, vector vf1,
        vector vf2, vector vt1, vector vt2, vector *points_3d,
        vector **p1_ref, vector **p1_corr, vector **p2_ref, vector **p2_corr);
  void project3d(vector vf1, vector vf2, vector vt1, vector vt2, double roll,
        vector *points_3d, vector **p1_ref, vector **p1_corr, vector **p2_ref,
        vector **p2_corr);
  void project_point(vector origin, vector X_vec, vector Y_vec, vector Z_vec,
        vector thePoint, vector projection);

  /* === Noise functions - 
               adds random error to the input correspondences === */
  void add_noise(double noisepercent, vector *p_ref1, vector *p_corr1,
                 vector *p_ref2, vector *p_corr2);
  void apply_noise(double noisedist, vector toNoisify);

  /* === Rotation functions -
               alters the 3D view so it's no longer a translation === */
  void getRotationMatrix(matrix R, int theAxis, double sintheta,
                         double costheta);

  /* === Rotation-map functions -
               get the error for a given rotation to be used in the errormap */
  double getRotationError(double yaw, double pitch);

  /* === Pixelization -
               maps all correspondence points to a grid on the view plane */
  void map_pixels(double pixel_spacing, vector *p1_ref, vector *p1_correspond,
            vector *p2_ref, vector *p2_correspond);
  void map_to_pixel(double pixel_spacing, vector aPoint);

/*** Support functions ***/
  /* === Memory management === */
  void veccpy(vector *array1, vector *array2, int numvec);
  vector *alloc_vector(int numVectors);
  gray **alloc_pgm(int width, int height);

  /* === File I/O - reads/writes .3D .2D and .affine === */
  void read_3d_file(char *filename, vector vf1, vector vf2, vector vt1,
                    vector vt2, double *rollc, vector **points_3d);
  void write_3d_file(char *filename, vector vf1, vector vf2, vector vt1,
                    vector vt2, double rollc, vector *points_3d);
  void read_correspondence_file(char *filename, vector **p1ref, vector **p2ref,
                    vector **p1, vector **p2);
  void write_correspondence_file(char *filename, vector *p1ref, vector *p2ref,
                    vector *p1, vector *p2);
  vector *read_affine_file(char *filename);
  void write_affine_file(char *filename, vector *pointArray);
  void get_2d_point(FILE *infile, vector xypoint);
  void get_3d_point(FILE *infile, vector xyzpoint);
  void get_nextline(FILE *infile, char *buffer);
  void output_vector(FILE *outfile, vector theVector);
  void write_pgm_file(char *filename, gray **image, int width, int height);

/* This shouldn't be necessary. Unfortunately the ppm libraries have been
   less than stable. */
  void pgm_writepgm( FILE *fp, gray **image, int height, int width, int max,
                     int rawbits);
  int *read_connectivity_file(char *filename, int *num_tri);
  void write_triangle_file(char *filename, vector *pAffine, int *connects,
              int num_tri);

  /* === Debugging output === */
  void fatalError(char *errormessage); 
  void text_print(char *message);
  void scalar_print(char *message, double d);
  void vec_print(char *message, vector v);
  void vec3_print(char *message, vector v);
  void matrix_print(char *message, matrix m);

  /* === Math functions -
          NB distinction between 2D (x,y,w) and 3D (4x4) algebra === */
  /* use this to avoid divide by zero */
  double check0(double checkthis, char *message);
  double vec_angle(vector v1, vector v2, int axis);
  double matrixDet_2d(matrix m);
  void matrixMult_2d(matrix a, matrix b, matrix result);
  void matrixMult_3d(matrix a, matrix b, matrix result);
  void matrixCopy_2d(matrix src, matrix dest);
  void matrixCopy_3d(matrix src, matrix dest);
  void preApply_2d(vector v, matrix m, vector result);
  void postApply_2d(matrix m, vector v, vector result);
  void apply_3d(matrix m, vector v, vector result);

/* Global variables */

/* holds the number of points currently under consideration */
int numpoints;

  /* Affine Calculation global variables */

  /* the reference correspondences */
  vector p0, p0_prime, p1, p1_prime, p2, p2_prime, p3, p3_prime;

  /* used in the delta-convergence process */
  vector e03, eT, delta;
  
  /* matrices to map the 3D representation to the image planes i and i' */
  matrix A, A_prime;

  /* the arrays containing the points */
  vector *pArray, *p_primeArray, *affineArray, *uArray;

  /* which axis is the virtual [remapped] P3? */
  int rotateaxes;

  /* holds the warp matrix */
  matrix warpM;
 
  /* holds the warp parameters */
  vector warp_vec;

  /* should the warped points be printed to stderr? */
  int print_warps;

  /* Rotation-map global variables */
  vector vf1, vf2, vt1, orig_vt2, new_vt2, *points_3d;
  vector *p1_ref, *p2_ref, *p1_corr, *p2_corr, *pAffine, *referenceAffine;
  double rollCoeff;
  int methodnum, orignumpoints;

  /* Delta-correction global variables */
  int n_evals, iterations;
  double trial0chiSquared, trialDchiSquared;

  /* Support globals */
  char buf0[100];            /* miscellaneous buffer for general use */


/*************** COMMAND-LINE ANALYSIS ***************/

/* main() merely selects which function the program is performing. */
int main(int argc, char **argv)
{
  numpoints = 0;
  
  if (argc < 2) {
    /* Enlighten the user as to proper protocol for homage. */
    print_usage(argc, argv);
  } else if (strcmp(argv[1], "calculate") == 0)
    calculate(argc, argv);
  else if (strcmp(argv[1], "calc-analyse") == 0)
    calc_analyse(argc, argv);
  else if (strcmp(argv[1], "analyse") == 0)
    analyse(argc, argv);
  else if (strcmp(argv[1], "project") == 0)
    project(argc, argv);
  else if (strcmp(argv[1], "noise") == 0)
    doNoise(argc, argv);
  else if (strcmp(argv[1], "errortest") == 0)
    errortest(argc, argv);
  else if (strcmp(argv[1], "rotationmap") == 0)
    rotationmap(argc, argv);
  else if (strcmp(argv[1], "deltamap") == 0)
    deltamap(argc, argv);
  else if (strcmp(argv[1], "warpmap") == 0)
    warpmap(argc, argv);
  else if (strcmp(argv[1], "eTtest") == 0)
    eTtest(argc, argv);
  else if (strcmp(argv[1], "gentriangles") == 0)
    gentriangles(argc, argv);
  else if (strcmp(argv[1], "chrisdebug") == 0)
    chrisdebug(argc, argv);
  else if (strcmp(argv[1], "pixelize") == 0)
    pixelize(argc, argv);
  else if (strcmp(argv[1], "triconvert") == 0)
    triconvert(argc, argv);
  else
    print_usage(argc, argv);

  return 0;
}

/* Prints the proper command-line usage of the program */
void print_usage(int argc, char **argv)
{
  fprintf(stderr, "Usage: %s <function>\n", argv[0]);
  fprintf(stderr, "where <function> is one of:\n");
  fprintf(stderr, "  calculate\n");
  fprintf(stderr, "  calc-analyse\n");
  fprintf(stderr, "  analyse\n");
  fprintf(stderr, "  project\n");
  fprintf(stderr, "  noise\n");
  fprintf(stderr, "  errortest\n");
  fprintf(stderr, "  rotationmap\n");
  fprintf(stderr, "  deltamap\n");
  fprintf(stderr, "  warpmap\n");
  fprintf(stderr, "  eTtest\n");
  fprintf(stderr, "  gentriangles\n");
  fprintf(stderr, "  pixelize\n");
  fprintf(stderr, "  triconvert\n");
}

/* "calculate" mode inputs a correspondence file and outputs an affine file */
void calculate(int argc, char **argv)
{
  vector *p1ref, *p2ref, *p1, *p2;
  int calcmode, approx_method, do_axes;

  if ((argc < 5) || (sscanf(argv[2], "%d", &calcmode) != 1)) {
    fprintf(stderr, "Usage: %s calculate <mode> <input file> <output file>\n",
	    argv[0]);
    fprintf(stderr, "Mode: 0 = standard calculation\n");
    fprintf(stderr, "      2 = delta adjustment\n");
    fprintf(stderr, "      4 = simplex delta adjustment\n");
    fprintf(stderr, "      6 = warp adjustment\n");
    fprintf(stderr, "           (add 1 for axis rotation)\n");
    exit(-1);
  }
  if (calcmode < 0 || calcmode > 7) {
    fprintf(stderr, "Error: mode must be from 0 to 7 (entered %s)\n", argv[2]);
    fprintf(stderr, "Mode: 0 = standard calculation\n");
    fprintf(stderr, "      2 = delta adjustment\n");
    fprintf(stderr, "      4 = simplex delta adjustment\n");
    fprintf(stderr, "      6 = warp adjustment\n");
    fprintf(stderr, "           (add 1 for axis rotation)\n");
    exit(-1);
  }
  do_axes = calcmode % 2;
  approx_method = calcmode / 2;

  read_correspondence_file(argv[3], &p1ref, &p2ref, &p1, &p2);
  write_affine_file(argv[4], AffineCalc(p1ref, p2ref, p1, p2, approx_method,
        do_axes));
}
    
/* "calc-analyse" mode inputs a correspondence file, analyses and outputs
       error to stderr */
void calc_analyse(int argc, char **argv)
{
  vector *p1ref, *p2ref, *p1, *p2, *ref_affine;
  int calcmode, approx_method, do_axes;
  float aferror;
  
  if ((argc < 5) || (sscanf(argv[2], "%d", &calcmode) != 1)) {
    fprintf(stderr, "Usage: %s calc-analyse <mode> <input file>
              <reference affine co-ords>\n", argv[0]);
    fprintf(stderr, "Mode: 0 = standard calculation\n");
    fprintf(stderr, "      2 = delta adjustment\n");
    fprintf(stderr, "      4 = simplex delta adjustment\n");
    fprintf(stderr, "      6 = warp adjustment\n");
    fprintf(stderr, "           (add 1 for axis rotation)\n");
    exit(-1);
  }
  if (calcmode < 0 || calcmode > 7) {
    fprintf(stderr, "Error: mode must be from 0 to 7 (entered %s)\n", argv[2]);
    fprintf(stderr, "Mode: 0 = standard calculation\n");
    fprintf(stderr, "      2 = delta adjustment\n");
    fprintf(stderr, "      4 = simplex delta adjustment\n");
    fprintf(stderr, "      6 = warp adjustment\n");
    fprintf(stderr, "           (add 1 for axis rotation)\n");
    exit(-1);
  }
  do_axes = calcmode % 2;
  approx_method = calcmode / 2;

  read_correspondence_file(argv[3], &p1ref, &p2ref, &p1, &p2);
  ref_affine = read_affine_file(argv[4]);

  aferror = average_error(ref_affine, AffineCalc(p1ref, p2ref, p1, p2,
               approx_method, do_axes));
  fprintf(stderr, "Calc-analyse : Average error is %9.5f%%\n", aferror);
}

/* "analyse" mode inputs two .affine files (one real, one perfect) and
      outputs error to stderr */
void analyse(int argc, char **argv)
{
  vector *p_affine, *ref_affine;
  float aferror;
  
  if (argc < 4) {
    fprintf(stderr, "Usage: %s analyse <affine file>
                <reference affine co-ords>\n", argv[0]);
    exit(-1);
  }

  p_affine = read_affine_file(argv[2]);
  ref_affine = read_affine_file(argv[3]);

  aferror = average_error(ref_affine, p_affine);
  fprintf(stderr, "Analyse : Average error is %9.5f%%\n", aferror);
}

/* "project" mode inputs a .3D file and projects it onto the virtual
           camera view planes */
void project(int argc, char **argv)
{
  vector vf1, vf2, vt1, vt2, *points_3d; 
  vector *p1_ref, *p2_ref, *p1_correspond, *p2_correspond;
  double yaw, pitch, roll, dummy;
  
  if ((argc < 7) || (sscanf(argv[3], "%lf", &yaw) != 1)
                 || (sscanf(argv[4], "%lf", &pitch) != 1)
                 || (sscanf(argv[5], "%lf", &roll) != 1)){
    fprintf(stderr, "Usage: %s project <3D model file> <yaw> <pitch>
               <roll> <correspondence file>\n", argv[0]);
    exit(-1);
  }

  read_3d_file(argv[2], vf1, vf2, vt1, vt2, &dummy, &points_3d);
  rotate_project3d(yaw, pitch, roll, vf1, vf2, vt1, vt2, points_3d, &p1_ref,
                   &p1_correspond, &p2_ref, &p2_correspond);
  write_correspondence_file(argv[6], p1_ref, p1_correspond, p2_ref,
                            p2_correspond);
}

/* "noise" mode adds noise to the given correspondence file and outputs
     it again. */
void doNoise(int argc, char **argv)
{
  vector *p1_ref, *p2_ref, *p1_correspond, *p2_correspond;
  double noisepercent;
  
  if ((argc < 5) || (sscanf(argv[3], "%lf", &noisepercent) != 1)) {
    fprintf(stderr, "Usage: %s noise <input correspondence file> <noise in %%>
              <output correspondence file>\n", argv[0]);
    exit(-1);
  }

  read_correspondence_file(argv[2], &p1_ref, &p2_ref, &p1_correspond,
            &p2_correspond);
  add_noise(noisepercent, p1_ref, p1_correspond, p2_ref, p2_correspond);
  write_correspondence_file(argv[4], p1_ref, p1_correspond, p2_ref,
             p2_correspond);
}

/* "errortest" mode distorts a 3D file according to a distortion file
         and writes the errors found to an output file. */
void errortest(int argc, char **argv)
{
  FILE *distortionfile, *errorfile;
  vector vf1, vf2, vt1, orig_vt2, new_vt2, *points_3d;
  vector *p1_ref, *p2_ref, *p1_corr, *p2_corr, *referenceAffine;
  vector *temp_p1ref, *temp_p2ref, *temp_p1corr, *temp_p2corr;
  double orig_rollCoeff, new_rollCoeff, yaw, pitch, roll, noisepercent;
  double affineError[4];
  int i, orignumpoints;
  
  if (argc != 5) {
    fprintf(stderr, "Usage: %s errortest <input 3D points> <distortion file>
             <output error file>\n", argv[0]);
    exit(-1);
  }
  if ((distortionfile = fopen(argv[3], "r")) == NULL) {
    fprintf(stderr, "Can't open distortion file \"%s\"", argv[3]);
    exit(-1);
  }
  if ((errorfile = fopen(argv[4], "w")) == NULL) {
    fprintf(stderr, "Can't open error file \"%s\"", argv[3]);
    exit(-1);
  }
  
  read_3d_file(argv[2], vf1, vf2, vt1, orig_vt2, &orig_rollCoeff, &points_3d);
  orignumpoints = numpoints;
  project3d(vf1, vf2, vt1, orig_vt2, orig_rollCoeff, points_3d, &p1_ref,
            &p1_corr, &p2_ref, &p2_corr);
  referenceAffine = AffineCalc(p1_ref, p2_ref, p1_corr, p2_corr, 0, 0);

  temp_p1ref = alloc_vector(4);
  temp_p2ref = alloc_vector(4);
  temp_p1corr = alloc_vector(numpoints);
  temp_p2corr = alloc_vector(numpoints);
  while (1) {
    get_nextline(distortionfile, buf0);
    if (sscanf(buf0, "%lf %lf %lf %lf", &yaw, &pitch, &roll,
	       &noisepercent) != 4) {
      if (buf0[0] == '~')
	break;
      else
	fprintf(errorfile, "%s", buf0);
    } else {
      printf("Performing analysis (%4.1g %4.1g %4.1g %4.1g) ... ", yaw,
	     pitch, roll, noisepercent);
      fflush(stdout);

      /* Set current view back to original 3D view */
      VSET(new_vt2, orig_vt2);
      new_rollCoeff = orig_rollCoeff; 
      numpoints = orignumpoints;
      rotate_project3d(yaw, pitch, roll, vf1, vf2, vt1, new_vt2, points_3d,
		       &p1_ref, &p1_corr, &p2_ref, &p2_corr);
      if (noisepercent != 0) {
        /* Add noise to correspondences */
	add_noise(noisepercent, p1_ref, p1_corr, p2_ref, p2_corr);
      }

      for (i = 0; i < 4; i++) {
	/* before each test we must restore the original points as
	   delta-correction will change them. */
        veccpy(p1_ref,  temp_p1ref,  4);
        veccpy(p2_ref,  temp_p2ref,  4);
        veccpy(p1_corr, temp_p1corr, numpoints);
        veccpy(p2_corr, temp_p2corr, numpoints);
	affineError[i] = average_error(referenceAffine,
	   AffineCalc(temp_p1ref, temp_p2ref, temp_p1corr, temp_p2corr, i, 0));
      }
      
      fprintf(errorfile, "Y(%4.1f) P(%4.1f) R(%4.1f) N(%4.1f):
              Errors %12.3f,%12.3f,%12.3f,%12.3f%%\n", yaw, pitch, roll,
	      noisepercent, affineError[0], affineError[1], affineError[2],
	      affineError[3]);
      printf("Errors %7.3f,%7.3f,%7.3f,%7.3f%%\n", affineError[0],
	     affineError[1], affineError[2], affineError[3]);
    }
  }
  fclose(errorfile);
}

/* "rotationmap" mode distorts a 3D file multiple times and writes the errors
   found (as a 2D errormap in PGM format) to a file. */
void rotationmap(int argc, char **argv)
{
#define AF_ERR_MAX    10.0
#define ITER_MAX      100
#define CHI2_MAX      0.01
#define DELTA_MAG_MAX 1.0
  
  double yaw, pitch, max_yaw, max_pitch;
  double affineError, numIterations, deltaMagnitude, difference;
  gray **aferr_image, **iter_image, **chi2_image, **ochi2_image,
       **dmag_image, **diff1_image, **diff2_image;
  char filename[100];
  int y, x, im_width, im_height, approx_method;
  
  if (argc != 9) {
    fprintf(stderr, "Usage: %s rotationmap <input 3D points> <method>
             <max yaw +/-> <max pitch +/-> <image width> <image height>
             <output PGM prefix>\n", argv[0]);
    exit(-1);
  }
  if (sscanf(argv[3], "%d", &methodnum) != 1) {
    sprintf(buf0, "rotationmap: can't read method number (%s)", argv[3]);
    fatalError(buf0);
  }
  if (methodnum < 0 || methodnum > 7)
    fatalError("rotationmap: method must be from 0-7");
  if (sscanf(argv[4], "%lf", &max_yaw) != 1) {
    sprintf(buf0, "rotationmap: can't read max yaw (%s)", argv[4]);
    fatalError(buf0);
  }
  approx_method = (methodnum / 2);
  
  if (sscanf(argv[5], "%lf", &max_pitch) != 1) {
    sprintf(buf0, "rotationmap: can't read max pitch (%s)", argv[5]);
    fatalError(buf0);
  }
  if (sscanf(argv[6], "%d", &im_width) != 1) {
    sprintf(buf0, "rotationmap: can't read image width (%s)", argv[6]);
    fatalError(buf0);
  }
  if (sscanf(argv[7], "%d", &im_height) != 1) {
    sprintf(buf0, "rotationmap: can't read image height (%s)", argv[7]);
    fatalError(buf0);
  }
  
  aferr_image = alloc_pgm(im_width, im_height);
  iter_image = alloc_pgm(im_width, im_height);
  chi2_image = alloc_pgm(im_width, im_height);
  ochi2_image = alloc_pgm(im_width, im_height);
  dmag_image = alloc_pgm(im_width, im_height);
  diff1_image = alloc_pgm(im_width, im_height);
  diff2_image = alloc_pgm(im_width, im_height);
  
  read_3d_file(argv[2], vf1, vf2, vt1, orig_vt2, &rollCoeff, &points_3d);
  orignumpoints = numpoints;
  project3d(vf1, vf2, vt1, orig_vt2, rollCoeff, points_3d, &p1_ref, &p1_corr,
            &p2_ref, &p2_corr);
  referenceAffine = AffineCalc(p1_ref, p2_ref, p1_corr, p2_corr, 0, 0);

  for (y = 0; y < im_height; y++) {
    printf("Analysing ... %3d%%\r", (100*y)/im_height);
    fflush(stdout);

    for (x = 0; x < im_width; x++) {
      yaw = max_yaw * (2 * (((float) y) / im_height) - 1.0);
      pitch = max_pitch * (2 * (((float) x) / im_width) - 1.0);
      
      affineError = getRotationError(yaw, pitch)/AF_ERR_MAX;
      switch(approx_method) {
      case 0:
	numIterations = 0;
	deltaMagnitude = 0;
	break;
      case 1:
	numIterations = ((float) iterations)/ITER_MAX;
	deltaMagnitude = sqrt(SQR(delta[X]) + SQR(delta[Y]))/DELTA_MAG_MAX;
	break;
      case 2:
	numIterations = ((float) n_evals)/ITER_MAX;
	deltaMagnitude = sqrt(SQR(delta[X]) + SQR(delta[Y]))/DELTA_MAG_MAX;
	break;
      case 3:
	numIterations = ((float) iterations)/ITER_MAX;
	deltaMagnitude = sqrt(SQR(warp_vec[X]) +
			      SQR(warp_vec[Y]))/DELTA_MAG_MAX;
	break;
      default:
	sprintf(buf0, "rotationmap: approx_method outside range (%d)?!\n",
		approx_method);
	fatalError(buf0);
	exit(-1);
      }
      
      difference = trial0chiSquared - trialDchiSquared;
      
      if (affineError > 1.0)
	aferr_image[y][x] = 255;
      else
	aferr_image[y][x] = (int) (255*affineError);
      if (numIterations > 1.0)
	iter_image[y][x] = 255;
      else
	iter_image[y][x] = (int) (255*numIterations);
      if (trialDchiSquared > CHI2_MAX)
	chi2_image[y][x] = 255;
      else
	chi2_image[y][x] = (int) (255*(trialDchiSquared/CHI2_MAX));
      if (trial0chiSquared > CHI2_MAX)
	ochi2_image[y][x] = 255;
      else
	ochi2_image[y][x] = (int) (255*(trial0chiSquared/CHI2_MAX));
      if (deltaMagnitude > 1.0)
	dmag_image[y][x] = 255;
      else
	dmag_image[y][x] = (int) (255*deltaMagnitude);
      if (difference < 0.0) {
	diff1_image[y][x] = 0;
	diff2_image[y][x] = (int) (255*(-difference));
      } else {
	diff1_image[y][x] = (int) (255*difference);
	diff2_image[y][x] = 0;
      }
    }
  }
  printf("Analysing ... complete!\n");
  sprintf(filename, "%s_afErr.pgm", argv[8]);
  write_pgm_file(filename, aferr_image, im_width, im_height);
  sprintf(filename, "%s_numIt.pgm", argv[8]);
  write_pgm_file(filename, iter_image, im_width, im_height);
  sprintf(filename, "%s_chi2.pgm", argv[8]);
  write_pgm_file(filename, chi2_image, im_width, im_height);
  sprintf(filename, "%s_ochi2.pgm", argv[8]);
  write_pgm_file(filename, ochi2_image, im_width, im_height);
  sprintf(filename, "%s_dMag.pgm", argv[8]);
  write_pgm_file(filename, dmag_image, im_width, im_height);
  sprintf(filename, "%s_d1.pgm", argv[8]);
  write_pgm_file(filename, diff1_image, im_width, im_height);
  sprintf(filename, "%s_d2.pgm", argv[8]);
  write_pgm_file(filename, diff2_image, im_width, im_height);
}

/* "deltamap" mode distorts a 3D file one time and writes the chi-squared
   versus delta (as a 2D errormap in PGM format) to a file. */
void deltamap(int argc, char **argv)
{
#define CHI2_MAX      0.01
#define DELTA_MAG_MAX 1.0
  
  double yaw, pitch;
  double chiSquared;
  gray **delta_image;
  int y, x, im_width, im_height;
  
  if (argc != 8) {
    fprintf(stderr, "Usage: %s deltamap <input 3D points> <yaw> <pitch>
              <image width> <image height> <output PGM filename>\n", argv[0]);
    exit(-1);
  }
  if (sscanf(argv[3], "%lf", &yaw) != 1) {
    sprintf(buf0, "deltamap: can't read yaw (%s)", argv[3]);
    fatalError(buf0);
  }
  if (sscanf(argv[4], "%lf", &pitch) != 1) {
    sprintf(buf0, "deltamap: can't read pitch (%s)", argv[4]);
    fatalError(buf0);
  }
  if (sscanf(argv[5], "%d", &im_width) != 1) {
    sprintf(buf0, "deltamap: can't read image width (%s)", argv[5]);
    fatalError(buf0);
  }
  if (sscanf(argv[6], "%d", &im_height) != 1) {
    sprintf(buf0, "deltamap: can't read image height (%s)", argv[6]);
    fatalError(buf0);
  }
  
  delta_image = alloc_pgm(im_width, im_height);

  read_3d_file(argv[2], vf1, vf2, vt1, new_vt2, &rollCoeff, &points_3d);
  rotate_project3d(yaw, pitch, 0.0, vf1, vf2, vt1, new_vt2, points_3d,
		   &p1_ref, &p1_corr, &p2_ref, &p2_corr);
  
  VSET(p0, p1_ref[0]);
  VSET(p1, p1_ref[1]);
  VSET(p2, p1_ref[2]);
  VSET(p3, p1_ref[3]);
  pArray = p1_corr;

  VSET(p0_prime, p2_ref[0]);
  VSET(p1_prime, p2_ref[1]);
  VSET(p2_prime, p2_ref[2]);
  VSET(p3_prime, p2_ref[3]);
  p_primeArray = p2_corr;
  
  uArray = alloc_vector(numpoints);

  for (y = 0; y < im_height; y++) {
    printf("Deltamapping ... %3d%%\r", (100*y)/im_height);
    fflush(stdout);

    for (x = 0; x < im_width; x++) {
      delta[X] = DELTA_MAG_MAX * (2 * (((float) y) / im_height) - 1.0);
      delta[Y] = DELTA_MAG_MAX * (2 * (((float) x) / im_width ) - 1.0);

      chiSquared = get_best_eT(delta[X], delta[Y]);
      
      if (chiSquared > CHI2_MAX)
	delta_image[y][x] = 255;
      else
	delta_image[y][x] = (int) (255*(chiSquared/CHI2_MAX));
    }
  }
  printf("Deltamapping ... complete!\n");
  write_pgm_file(argv[7], delta_image, im_width, im_height);
}

/* "warpmap" mode distorts a 3D file one time and writes the chi-squared
   versus reverse warp (as a 2D errormap in PGM format) to a file. */
void warpmap(int argc, char **argv)
{
  double warp_chi2_max = 0.15;
  double yaw, pitch;
  double chiSquared, warp_mag;
  gray **warp_image;
  int y, x, im_width, im_height;
  
  if (argc != 9) {
    fprintf(stderr, "Usage: %s warpmap <input 3D points> <yaw> <pitch>
                     <warp magnitude> <image width> <image height>
                     <output PGM filename>\n", argv[0]);
    exit(-1);
  }
  if (sscanf(argv[3], "%lf", &yaw) != 1) {
    sprintf(buf0, "warpmap: can't read yaw (%s)", argv[3]);
    fatalError(buf0);
  }
  if (sscanf(argv[4], "%lf", &pitch) != 1) {
    sprintf(buf0, "warpmap: can't read pitch (%s)", argv[4]);
    fatalError(buf0);
  }
  if (sscanf(argv[5], "%lf", &warp_mag) != 1) {
    sprintf(buf0, "warpmap: can't read warp magnitude (%s)", argv[5]);
    fatalError(buf0);
  }
  if (sscanf(argv[6], "%d", &im_width) != 1) {
    sprintf(buf0, "warpmap: can't read image width (%s)", argv[6]);
    fatalError(buf0);
  }
  if (sscanf(argv[7], "%d", &im_height) != 1) {
    sprintf(buf0, "warpmap: can't read image height (%s)", argv[7]);
    fatalError(buf0);
  }
  
  warp_image = alloc_pgm(im_width, im_height);

  read_3d_file(argv[2], vf1, vf2, vt1, new_vt2, &rollCoeff, &points_3d);
  rotate_project3d(yaw, pitch, 0.0, vf1, vf2, vt1, new_vt2, points_3d,
		   &p1_ref, &p1_corr, &p2_ref, &p2_corr);
  
  VSET(p0, p1_ref[0]);
  VSET(p1, p1_ref[1]);
  VSET(p2, p1_ref[2]);
  VSET(p3, p1_ref[3]);
  pArray = p1_corr;

  VSET(p0_prime, p2_ref[0]);
  VSET(p1_prime, p2_ref[1]);
  VSET(p2_prime, p2_ref[2]);
  VSET(p3_prime, p2_ref[3]);
  p_primeArray = p2_corr;
  
  uArray = alloc_vector(numpoints);

  for (y = 0; y < im_height; y++) {
    printf("Warpmapping ... %3d%%\r", (100*y)/im_height);
    fflush(stdout);

    for (x = 0; x < im_width; x++) {
      warp_vec[X] = warp_mag * (2 * (((float) y) / im_height) - 1.0);
      warp_vec[Y] = warp_mag * (2 * (((float) x) / im_width ) - 1.0);

      chiSquared = get_best_warped_eT(warp_vec[X], warp_vec[Y]);
      
      if (chiSquared > warp_chi2_max)
	warp_image[y][x] = 255;
      else
	warp_image[y][x] = (int) (255*(chiSquared/warp_chi2_max));
    }
  }
  printf("Warpmapping ... complete!\n");
  write_pgm_file(argv[8], warp_image, im_width, im_height);
}

/* "eTtest" mode checks how get_best_eT is working. It's a tricky little devil,
   it knows when your back is turned. */
void eTtest(int argc, char **argv)
{  
  double chiSquared;
  
  if (argc != 3) {
    fprintf(stderr, "Usage: %s eTtest <input correspondences>\n", argv[0]);
    exit(-1);
  }
  
  read_correspondence_file(argv[2], &p1_ref, &p2_ref, &p1_corr, &p2_corr);
  
  VSET(p0, p1_ref[0]);
  VSET(p1, p1_ref[1]);
  VSET(p2, p1_ref[2]);
  VSET(p3, p1_ref[3]);
  pArray = p1_corr;

  VSET(p0_prime, p2_ref[0]);
  VSET(p1_prime, p2_ref[1]);
  VSET(p2_prime, p2_ref[2]);
  VSET(p3_prime, p2_ref[3]);
  p_primeArray = p2_corr;
  
  uArray = alloc_vector(numpoints);

  /* Print out what it says the results are. */
  chiSquared = get_best_eT(0, 0);
  printf("chiSq = %f\n", chiSquared);
  printf("eT = %f, %f\n", eT[X], eT[Y]);
}

/* "gentriangles" mode distorts the given .3D view file with a specified
      pitch, yaw and roll, calculates and outputs a triangle file for fun. */
void gentriangles(int argc, char **argv)
{
  vector vf1, vf2, vt1, vt2, *points_3d; 
  vector *p1_ref, *p2_ref, *p1_corr, *p2_corr, *pAffine;
  double rollCoeff, yaw, pitch, roll, noisepercent;
  int methodnum, approx_method, do_axes;
  int *connections, num_triangles;
  
  if ((argc < 10) ||
      (sscanf(argv[3], "%lf", &yaw)          != 1) ||
      (sscanf(argv[4], "%lf", &pitch)        != 1) ||
      (sscanf(argv[5], "%lf", &roll)         != 1) ||
      (sscanf(argv[6], "%lf", &noisepercent) != 1) ||
      (sscanf(argv[7], "%d",  &methodnum)    != 1)) {
    fprintf(stderr, "Usage: %s gentriangles <input 3D points> <yaw> <pitch>
                     <roll> <noisepercent> <method> <connectivity file>
                     <output triangle file>\n", argv[0]);
    exit(-1);
  }
  
  connections = read_connectivity_file(argv[8], &num_triangles);

  read_3d_file(argv[2], vf1, vf2, vt1, vt2, &rollCoeff, &points_3d);
  rotate_project3d(yaw, pitch, roll, vf1, vf2, vt1, vt2, points_3d, &p1_ref,
		   &p1_corr, &p2_ref, &p2_corr);
  /* Add noise to correspondences */
  add_noise(noisepercent, p1_ref, p1_corr, p2_ref, p2_corr);

  approx_method = methodnum / 2;
  do_axes = methodnum % 2;
  pAffine = AffineCalc(p1_ref, p2_ref, p1_corr, p2_corr, approx_method,
		       do_axes);      

  write_triangle_file(argv[9], pAffine, connections, num_triangles);
  if (approx_method == 1 || approx_method == 2) {
    printf("Delta was (%f, %f)\n", delta[X], delta[Y]);
    printf("By comparison, p0 = (%f, %f), p3 = (%f, %f)\n", p1_ref[0][X],
	   p1_ref[0][Y], p1_ref[3][X], p1_ref[3][Y]);
  } else if (approx_method == 3) {
    printf("Reverse warp found to be (%f, %f)\n", warp_vec[X], warp_vec[Y]);
  }
}

/* Guess what "chrisdebug" mode does. */
void chrisdebug(int argc, char **argv)
{
  double yaw, pitch, x_coeff, y_coeff;
  double chiSquared;
  int mode, orignumpoints;
  vector start_p1, end_p1, warped_p1;

  print_warps = 0;
  
  sscanf(argv[3], "%d",  &mode);
  sscanf(argv[4], "%lf", &yaw);
  sscanf(argv[5], "%lf", &pitch);
  sscanf(argv[6], "%lf", &x_coeff);
  sscanf(argv[7], "%lf", &y_coeff);
  fprintf(stderr, "%s %d %f %f %f %f\n", argv[2], mode, yaw, pitch,
	  x_coeff, y_coeff);
  
  read_3d_file(argv[2], vf1, vf2, vt1, new_vt2, &rollCoeff, &points_3d);
  orignumpoints = numpoints;
  
  project3d(vf1, vf2, vt1, new_vt2, rollCoeff, points_3d, &p1_ref, &p1_corr,
	    &p2_ref, &p2_corr);

  VSET(start_p1, p2_corr[0]);
  start_p1[Z] = 1.0;
  
  VSET(p0, p1_ref[0]);
  VSET(p1, p1_ref[1]);
  VSET(p2, p1_ref[2]);
  VSET(p3, p1_ref[3]);
  pArray = p1_corr;

  VSET(p0_prime, p2_ref[0]);
  VSET(p1_prime, p2_ref[1]);
  VSET(p2_prime, p2_ref[2]);
  VSET(p3_prime, p2_ref[3]);
  p_primeArray = p2_corr;
  
  uArray = alloc_vector(numpoints);

  chiSquared = get_best_warped_eT(0.0, 0.0);

  numpoints = orignumpoints;
  rotate_project3d(yaw, pitch, rollCoeff, vf1, vf2, vt1, new_vt2,
		   points_3d, &p1_ref, &p1_corr, &p2_ref, &p2_corr);

  VSET(end_p1, p2_corr[0]);
  end_p1[Z] = 1.0;
  
  VSET(p0, p1_ref[0]);
  VSET(p1, p1_ref[1]);
  VSET(p2, p1_ref[2]);
  VSET(p3, p1_ref[3]);
  pArray = p1_corr;

  VSET(p0_prime, p2_ref[0]);
  VSET(p1_prime, p2_ref[1]);
  VSET(p2_prime, p2_ref[2]);
  VSET(p3_prime, p2_ref[3]);
  p_primeArray = p2_corr;
  
  uArray = alloc_vector(numpoints);

  
  chiSquared = get_best_warped_eT(0.0, 0.0);
 
  if (mode == 0)
    chiSquared = get_best_eT(x_coeff, y_coeff);
  else {
    warp_vec[X] = x_coeff;
    warp_vec[Y] = y_coeff;
    print_warps = 1;
    apply_warp();
    print_warps = 0;
    printf("Warped by %f, %f\n", warp_vec[X], warp_vec[Y]);

    chiSquared = get_best_warped_eT(0.0, 0.0);

    VSET(warped_p1, p2_corr[0]);

#if DEBUGINFO
    vec_print("start_p1: ", start_p1);
    vec_print("end_p1: ", end_p1);
    vec_print("warped_p1: ", warped_p1);
#endif
  }  
}

/* "pixelize" mode takes a correspondence file and maps all the points to
   a grid on the viewplane. */
void pixelize(int argc, char **argv)
{
  vector *p1_ref, *p2_ref, *p1_correspond, *p2_correspond;
  double pixel_spacing;
  
  if ((argc < 5) || (sscanf(argv[3], "%lf", &pixel_spacing) != 1)) {
    fprintf(stderr, "Usage: %s pixelize <input correspondence file>
                     <pixel spacing> <output correspondence file>\n", argv[0]);
    exit(-1);
  }

  printf("Pixel spacing %f\n", pixel_spacing);
  read_correspondence_file(argv[2], &p1_ref, &p2_ref, &p1_correspond,
			   &p2_correspond);
  map_pixels(pixel_spacing, p1_ref, p1_correspond, p2_ref, p2_correspond);
  write_correspondence_file(argv[4], p1_ref, p1_correspond, p2_ref,
			    p2_correspond);
}

/* "triconvert" mode takes an affine file and a connection file and turns them
   into triangles. */
void triconvert(int argc, char **argv)
{
  vector *affinePoints;
  int *connections, num_tri;
  
  if (argc < 5) {
    fprintf(stderr, "Usage: %s triconvert <input affine file>
                     <connection file> <output triangle file>\n", argv[0]);
    exit(-1);
  }

  affinePoints = read_affine_file(argv[2]);
  connections = read_connectivity_file(argv[3], &num_tri);
  write_triangle_file(argv[4], affinePoints, connections, num_tri);
}  

/************ CALCULATION FUNCTIONS *************/

/*============ AFFINE CALCULATION ==============*/ 

/* AffineCalc() is passed a set of correspondences and control
   parameters (approx_method = 0, 1, 2 or 3, do_axes = 0 or 1)
   and returns a set of affine co-ordinates */
vector *AffineCalc(vector *p_refArray, vector *p_prime_refArray,
		   vector *points1, vector *points2, int approx_method,
		   int do_axes)
{  
  /* Global variables associated with affineCalc:
      vector p0, p1, p2, p3, p0', p1', p2', p3';
      vector e03, eT, delta;
      matrix A, A';
      vector pArray, p_primeArray, affineArray;
      int rotateaxes;

     Uses:
      int numpoints;

     Passed:
      vector *p_refArray (holds p0, p1, p2, p3)
      vector *p_prime_refArray (holds p0', p1', p2', p3')
      vector *points1 (holds numpoints correspondences in image i)
      vector *points2 (holds numpoints correspondences in image i)

     Returns:
      vector *affineArray (holds the 3D co-ordinates of numpoints points)
     */

  int i;
  
  VSET(p0, p_refArray[0]);
  VSET(p1, p_refArray[1]);
  VSET(p2, p_refArray[2]);
  VSET(p3, p_refArray[3]);
  pArray = points1;

  VSET(p0_prime, p_prime_refArray[0]);
  VSET(p1_prime, p_prime_refArray[1]);
  VSET(p2_prime, p_prime_refArray[2]);
  VSET(p3_prime, p_prime_refArray[3]);
  p_primeArray = points2;
  
  uArray = alloc_vector(numpoints);
  affineArray = alloc_vector(numpoints);
  
  trial0chiSquared = get_best_eT(0.0, 0.0);
  switch(approx_method) {
  case 1:
    adjust_delta();
    trialDchiSquared = get_best_eT(delta[X], delta[Y]);
    apply_delta();
    break;
  case 2:
    adjust_simplex_delta(0.0, 0.0, simplex_eT_interface);
    trialDchiSquared = get_best_eT(delta[X], delta[Y]);
    apply_delta();
    break;
  case 3:
    adjust_warp();
    trialDchiSquared = get_best_warped_eT(warp_vec[X], warp_vec[Y]);
    apply_warp();
    break;
  default:
    trialDchiSquared = 0.0;
    break;
  }

  if (do_axes)
    adjust_axes();

  calculate_constant_points();
  for (i = 0; i < numpoints; i++)
    getAffine(pArray[i], p_primeArray[i], affineArray[i]);

  if (do_axes)
    readjust_axes();

  free(uArray);
  return affineArray;
}

/* The heart of the affine calculation - given the global variables
   defining the transformation, this determines the affine co-ordinates
   of a particular correspondence p -> p'. */
void getAffine(vector p, vector p_prime, vector p_affine)
{
  vector I, i, i_prime, vtemp, q;
  line M, M_star, N;
  matrix mtemp;
  double lambda, lambda1, lambda2;
  vector q_p0, p3_p0, p3_e03, q_e03;
  
  /* NB: The debugging info stays. Last time I took it out I regretted it. */

#if DEBUGINFO
  vec_print("Doing p  = ", p);
  vec_print("      p' = ", p_prime);
#endif

  /* Calculate the line M, the intersection of the plane passing
     through the eye O and the line l (from p parallel to p0p3)
     with the plane pi */
  vtemp[X] = p[Y] - e03[Y];
  vtemp[Y] = e03[X] - p[X];
  vtemp[Z] = (p[X]*e03[Y]) - (e03[X]*p[Y]);

#if DEBUGINFO
  vec_print("l: ", vtemp);
#endif

  preApply_2d(vtemp, A, M);

#if DEBUGINFO
  vec_print("M: ", M);
#endif

  /* Calculate the line M*, the intersection of the plane passing
     through the eye O and the line l' (from p' parallel to p0'p3')
     with the plane pi */
  vtemp[X] = p_prime[Y] - e03[Y];
  vtemp[Y] = e03[X] - p_prime[X];
  vtemp[Z] = (p_prime[X]*e03[Y]) - (e03[X]*p_prime[Y]);

#if DEBUGINFO
  vec_print("l': ", vtemp);*/
#endif

  preApply_2d(vtemp, A_prime, M_star);

#if DEBUGINFO
  vec_print("M*: ", M_star);
#endif

  /* Calculate the intersection of M and M*, the projection
       I of P onto pi in the direction P0P3 */
  VCROSS(M, M_star, I);

#if DEBUGINFO
  vec_print("I: ", I);
#endif

  /* Calculate X and Y affine co-ordinates (of I in the plane pi = P0P1P2) */
  p_affine[X] = I[X]/check0(I[Z], "AFFINE-CALC, I[Z]");

#if DEBUGINFO
  scalar_print("X: ", p_affine[X]);
#endif

  p_affine[Y] = I[Y]/check0(I[Z], "AFFINE-CALC, I[Z]");

#if DEBUGINFO
  scalar_print("Y: ", p_affine[Y]);
#endif

  if ((fabs(p_affine[X]) < TINY) && (fabs(p_affine[Y]) < TINY)) {
#if DEBUGINFO
    fprintf(stderr, "X and Y == 0, setting q = p.\n");
#endif
    q[X] = p[X];  
    q[Y] = p[Y];
    q[Z] = p[Z];
  } else {
   
    /* Calculate projections of I in both images (i and i') */
    postApply_2d(A, I, i);
#if DEBUGINFO
    vec_print("i: ", i);
#endif

    postApply_2d(A_prime, I, i_prime);

#if DEBUGINFO
    vec_print("i': ", i_prime);
#endif

    /* Construct lambda = -det(m1)/det(m2) where
	  m1 = ( xp  yp   1  )
	       ( x0' y0'  1  )
	       ( xi' yi' zi' )

	  m2 = ( xp  yp   1  )
	       ( x0  y0   1  )
	       ( xi  yi  zi  ) */
    mtemp[0][0] = p[X];
    mtemp[1][0] = p[Y];
    mtemp[2][0] = 1.0;
    mtemp[0][1] = p0_prime[X];
    mtemp[1][1] = p0_prime[Y];
    mtemp[2][1] = 1.0;
    mtemp[0][2] = i_prime[X];
    mtemp[1][2] = i_prime[Y];
    mtemp[2][2] = i_prime[Z];

#if DEBUGINFO
    matrix_print("mtemp: ", mtemp);*/
#endif

    lambda1 = matrixDet_2d(mtemp);

#if DEBUGINFO
    scalar_print("lambda1: ", lambda1);
#endif

    mtemp[0][1] = p0[X];
    mtemp[1][1] = p0[Y];
    mtemp[0][2] = i[X];
    mtemp[1][2] = i[Y];
    mtemp[2][2] = i[Z];

#if DEBUGINFO
    matrix_print("mtemp: ", mtemp);
#endif

    lambda2 = matrixDet_2d(mtemp);

#if DEBUGINFO
    scalar_print("lambda2: ", lambda2);
#endif

    if (fabs(lambda1 - lambda2) < TINY)
      lambda = 1;
    else
      lambda = -lambda1/check0(lambda2, "AFFINE_CALC: lambda");

#if DEBUGINFO
    scalar_print("lambda: ", lambda);
#endif

    /* Calculate homogeneous co-ordinates of N, the line through
          P parallel to P0I */
    N[X] = lambda*(p0[Y]*i[Z] - i[Y])       
          + (p0_prime[Y]*i_prime[Z] - i_prime[Y]);
    N[Y] = lambda*(i[X] - p0[X]*i[Z])       
          + (i_prime[X] - p0_prime[X]*i_prime[Z]);
    N[Z] = lambda*(p0[X]*i[Y] - i[X]*p0[Y]) 
          + (p0_prime[X]*i_prime[Y] - i_prime[X]*p0_prime[Y]);

#if DEBUGINFO
    vec_print("N: ", N);
#endif

    /* Calculate co-ordinates of q, the projection of the intersection
       of N and P0P3 */
    q[X] = (p3[X] - p0[X])*N[Z] - (p0[X]*p3[Y] - p3[X]*p0[Y])*N[Y];
    q[Y] = (p0[X]*p3[Y] - p3[X]*p0[Y])*N[X] - (p0[Y] - p3[Y])*N[Z];
    q[Z] = (p0[Y] - p3[Y])*N[Y] - (p3[X] - p0[X])*N[X];

#if DEBUGINFO
    vec_print("q: ", q);
#endif

    /* Homogenize co-ordinates of q */
    q[X] /= check0(q[Z], "Q-CALC (x)");
    q[Y] /= check0(q[Z], "Q-CALC (y)");
    q[Z] = 1.0;

#if DEBUGINFO
    vec_print("q(h): ", q);
#endif
  }
  
  /* Calculate Z affine co-ordinate of P */
   /* now we need to get the cross product of the points p0, p3, q and e03 ...
      Now, these points *should* be collinear -> we can take a cross product.
      In practice, if they are not collinear, bad things happen.
      However since we have intersected N with p0p3 to get q and e03 is auto-
      matically on p0p3, they should be collinear in every (ab)normal
      situation.
      
      ***NOTE*** The Z-axis weirdness happens here. It happens because we're
      getting bad values for q (that lie _beyond_ e03 for some reason). Also,
      the fact that we only use the Y-coordinates of q, p0, p3 and e03 won't
      help. I can't think of a way to use the X and Y coordinates unless it's
      by using a dot-product?

      Huzzah! Maybe this will work better ...
      (qp0 . p3p0)*(p3e03 . p3e03) / (p3p0 . p3p0)*(qe03 . p3e03)
      */

/* Memo: This didn't work ... spectacularly.
   q_p0  = sqrt(SQR( q[X] -  p0[X]) + SQR( q[Y] -  p0[Y]));
  p3_p0  = sqrt(SQR(p3[X] -  p0[X]) + SQR(p3[Y] -  p0[Y]));
  p3_e03 = sqrt(SQR(p3[X] - e03[X]) + SQR(p3[Y] - e03[Y]));
   q_e03 = sqrt(SQR( q[X] - e03[X]) + SQR( q[Y] - e03[Y]));*/

  VSUB(q, p0, q_p0);
  VSUB(p3, p0, p3_p0);
  VSUB(p3, e03, p3_e03);
  VSUB(q, e03, q_e03);
  p_affine[Z] = DOT2D(q_p0, p3_p0)*DOT2D(p3_e03, p3_e03);
  p_affine[Z] /= check0(DOT2D(p3_p0, p3_p0)*DOT2D(q_e03, p3_e03),
			"AFFINE_CALC for p[Z]");

/* Leave this here in case the alternative formulation dies horribly.
  p_affine[Z] = (q[Y] - p0[Y])*(p3[Y] - e03[Y]);
  p_affine[Z] /= check0((p3[Y] - p0[Y])*(q[Y] - e03[Y]), "AFFINE_CALC for p[Z]");*/

/* NB: |Z| > 5.0 indicates bad things.

  if (fabs(p_affine[Z]) > 5.0) {
    vec_print("q: ", q);
    scalar_print("q[Z]: ", q_homog);
    vec_print("p0: ", p0);
    vec_print("p3: ", p3);
    vec_print("e03: ", e03);
    fprintf(stderr, "p[Z] %f == Cr(%f,%f,%f,%f)\n", p_affine[Z], q_p0, p3_e03, p3_p0, q_e03);
  }*/

#if DEBUGINFO
  scalar_print("Z: ", p_affine[Z]);
  if (VMAG(p_affine) > 1000) {
    vec_print("large", p_affine);
    SET_VECTOR(p_affine, 0.0, 0.0, 0.0, 1.0);
  }
#endif
  
}

/* calculate_constant_points() determines the defining parameters of the
   transformation and stores them in the designated global variables.
   They are then used for each point to generate the affine co-ords */
void calculate_constant_points(void)
{
  line l1, l2, vline;
  matrix mtemp;
  double lambda, mu, nu;
  double lambda_prime, mu_prime, nu_prime;

  /* This routine calculates all of the constants needed for further
     calculation. e.g. e03 and the matrices A, A' */

  get_line(p0, p3, l1);
  get_line(p0_prime, p3_prime, l2);
  line_intersect(l1, l2, e03);
#if DEBUGINFO
  vec_print("e03: ", e03);
#endif

  /* Construct lambda = -det(m1)/det(m2) where : */
  /* m1 = (  y0-y2       y0'-y2'        y0'-y1'   )
          (  x2-x0       x2'-x0'        x1'-x0'   )
          (x0y2-x2y0  x0'y2'-x2'y0'  x0'y1'-x1'y0') */

  /* m2 = (  y0-y2       y0'-y2'       y0-y1  )
          (  x2-x0       x2'-x0'       x1-x0  )
          (x0y2-x2y0  x0'y2'-x2'y0'  x0y1-x1y0) */
  
  mtemp[0][0] = p0[Y] - p2[Y];
  mtemp[0][1] = p2[X] - p0[X];
  mtemp[0][2] = p0[X]*p2[Y] - p2[X]*p0[Y];
  mtemp[1][0] = p0_prime[Y] - p2_prime[Y];
  mtemp[1][1] = p2_prime[X] - p0_prime[X];
  mtemp[1][2] = p0_prime[X]*p2_prime[Y] - p2_prime[X]*p0_prime[Y];
  mtemp[2][0] = p0_prime[Y] - p1_prime[Y];
  mtemp[2][1] = p1_prime[X] - p0_prime[X];
  mtemp[2][2] = p0_prime[X]*p1_prime[Y] - p1_prime[X]*p0_prime[Y];

#if DEBUGINFO
  matrix_print("mtemp: ", mtemp);
#endif

  lambda = -matrixDet_2d(mtemp);	/* |m1| */
#if DEBUGINFO
  scalar_print("lambda1: ", lambda);
#endif

  mtemp[2][0] = p0[Y] - p1[Y];
  mtemp[2][1] = p1[X] - p0[X];
  mtemp[2][2] = p0[X]*p1[Y] - p1[X]*p0[Y];
#if DEBUGINFO
  matrix_print("mtemp: ", mtemp);
#endif

  lambda /= check0(matrixDet_2d(mtemp), "CONSTANTS (lambda)");	/* / |m2| */
#if DEBUGINFO
  scalar_print("lambda: ", lambda);
#endif

  /* Calculate the vanishing line of pi in the images i and i' (the same) */
  vline[X] = lambda*(p0[Y] - p1[Y])             
          + (p0_prime[Y] - p1_prime[Y]);
  vline[Y] = lambda*(p1[X] - p0[X])             
          + (p1_prime[X] - p0_prime[X]);
  vline[Z] = lambda*(p0[X]*p1[Y] - p1[X]*p0[Y]) 
          + (p0_prime[X]*p1_prime[Y] - p1_prime[X]*p0_prime[Y]);
#if DEBUGINFO
  vec_print("vline: ", vline);
#endif

  /* Calculate the co-efficients of A */
  lambda = 1/check0((vline[X]*p0[X] + vline[Y]*p0[Y] + vline[Z]), 
		    "A-CALC (lambda)");
#if DEBUGINFO
  scalar_print("lambda: ", lambda);
#endif
  mu     = 1/check0((vline[X]*p1[X] + vline[Y]*p1[Y] + vline[Z]),
		    "A-CALC (mu)");
#if DEBUGINFO
  scalar_print("mu: ", mu);
#endif

  nu     = 1/check0((vline[X]*p2[X] + vline[Y]*p2[Y] + vline[Z]),
		    "A-CALC (nu)");
#if DEBUGINFO
  scalar_print("nu: ", nu);
#endif

  /* Calculate A, the projective transformation from pi to image i */
  A[0][0] = mu*p1[X] - lambda*p0[X];
  A[0][1] = mu*p1[Y] - lambda*p0[Y];
  A[0][2] = mu       - lambda;
  A[1][0] = nu*p2[X] - lambda*p0[X];
  A[1][1] = nu*p2[Y] - lambda*p0[Y];
  A[1][2] = nu       - lambda;
  A[2][0] = lambda*p0[X];
  A[2][1] = lambda*p0[Y];
  A[2][2] = lambda;
#if DEBUGINFO
  matrix_print("A: ", A);
#endif

  /* Calculate the co-efficients of A' */
  lambda_prime = 1/check0((vline[X]*p0_prime[X] + vline[Y]*p0_prime[Y]
			   + vline[Z]), "A'-CALC (lambda')");
#if DEBUGINFO
  scalar_print("lambda': ", lambda_prime);
#endif
  mu_prime     = 1/check0((vline[X]*p1_prime[X] + vline[Y]*p1_prime[Y]
			   + vline[Z]), "A'-CALC (mu')");
#if DEBUGINFO
  scalar_print("mu': ", mu_prime);
#endif
  nu_prime     = 1/check0((vline[X]*p2_prime[X] + vline[Y]*p2_prime[Y]
			   + vline[Z]), "A'-CALC (nu')");
#if DEBUGINFO
  scalar_print("nu': ", nu_prime);
#endif

  /* Calculate A', the projective transformation from pi to image i' */
  A_prime[0][0] = mu_prime*p1_prime[X] - lambda_prime*p0_prime[X];
  A_prime[0][1] = mu_prime*p1_prime[Y] - lambda_prime*p0_prime[Y];
  A_prime[0][2] = mu_prime             - lambda_prime;
  A_prime[1][0] = nu_prime*p2_prime[X] - lambda_prime*p0_prime[X];
  A_prime[1][1] = nu_prime*p2_prime[Y] - lambda_prime*p0_prime[Y];
  A_prime[1][2] = nu_prime             - lambda_prime;
  A_prime[2][0] = lambda_prime*p0_prime[X];
  A_prime[2][1] = lambda_prime*p0_prime[Y];
  A_prime[2][2] = lambda_prime;
#if DEBUGINFO
  matrix_print("A': ", A_prime);
#endif
}

/* get_best_eT utilises a least-sqares technique to find the best
 approximate to eT for the given adjustment delta. It returns the (inevitable)
 error associated with this eT. */
double get_best_eT(double deltax, double deltay)
{
  /* This procedure gets the best eT for the given pArray,
     p_primeArray and delta. It returns the sum of d^2,
     otherwise known as chi squared. */
  double chi_squared, mod_u, d;
  double a11, a12, a13, a21, a22, a23;
  double *p, *p_, *u;
  int i;

  a11 = 0;
  a12 = 0;
  a13 = 0;
  a21 = 0;
  a22 = 0;
  a23 = 0;
  for (i = 0; i < numpoints; i++) {
    u = uArray[i];
    p = pArray[i];
    p_ = p_primeArray[i];
    
    u[X] = -(deltay + p_[Y] - p[Y]);
    u[Y] =  (deltax + p_[X] - p[X]);
    mod_u = sqrt(u[X]*u[X] + u[Y]*u[Y]);
    u[X] /= check0(mod_u, "mod u");
    u[Y] /= check0(mod_u, "mod u");
    
    a11 += 2*u[X]*u[X];
    a12 += 2*u[X]*u[Y];
    a13 += 2*u[X]*(u[X]*p[X] + u[Y]*p[Y]);
    a21 += 2*u[Y]*u[X];
    a22 += 2*u[Y]*u[Y];
    a23 += 2*u[Y]*(u[X]*p[X] + u[Y]*p[Y]);
  }   

  /* Okay, we now have a set of equations of the form
        a11eTx + a12eTy = a13
	a21eTx + a22eTy = a23

	These give
	   eTy = (a13a21 - a11a23)/(a12a21 - a11a22)
	   eTx = (a13 - a12eTy)/a11       */
#if DEBUGINFO
  fprintf(stderr, "Trying to solve : %16.11feTx + %16.11feTy = %16.11f\n",
	  a11, a12, a13);
  fprintf(stderr, "                  %16.11feTx + %16.11feTy = %16.11f\n",
	  a21, a22, a23);
#endif
  
  if (a11 == 0) {
    eT[Y] = a13/check0(a12, "non-determinative equations for eT!");
    eT[X] = (a23 - a22*eT[Y])/check0(a21,
				     "non-determinative equations for eT!");
  } else {
    eT[Y] = (a13*a21 - a11*a23)/check0(a12*a21 - a11*a22, "eT.y");
    eT[X] = (a13 - a12*eT[Y])/check0(a11, "a11");
  }
  
  chi_squared = 0.0;

  for (i = 0; i < numpoints; i++) {
    p = pArray[i];
    u = uArray[i];
    
    d = u[X]*(p[X] - eT[X]) + u[Y]*(p[Y] - eT[Y]);
    chi_squared += d*d;
  }
  
  return chi_squared;
}

/* get_best_warped_eT utilises the same least-sqares technique to find best
 approximation to eT for the warp yaw, pitch, (roll eventually?). It
 returns the error associated with this eT. */
double get_best_warped_eT(double yaw, double pitch)
{
  /* This procedure gets the best eT for the given pArray,
     p_primeArray and rotation. It returns the sum of d^2,
     otherwise known as chi squared. */
  double chi_squared, mod_u, d;
  double a11, a12, a13, a21, a22, a23;
  double *p, *p_, *u;
  vector warped_p_;
  int i;

  prepare_warp_matrix(yaw, pitch); /* precalculate the warp matrix */
  a11 = 0;
  a12 = 0;
  a13 = 0;
  a21 = 0;
  a22 = 0;
  a23 = 0;
  for (i = 0; i < numpoints; i++) {
    u = uArray[i];
    p = pArray[i];
    p_ = p_primeArray[i];

    warp_point(p_, &warped_p_);    /* warp the points in image 2 to match */
    u[X] = -(warped_p_[Y] - p[Y]);
    u[Y] =  (warped_p_[X] - p[X]);
    mod_u = sqrt(u[X]*u[X] + u[Y]*u[Y]);
    u[X] /= check0(mod_u, "mod u");
    u[Y] /= check0(mod_u, "mod u");
    
    a11 += 2*u[X]*u[X];
    a12 += 2*u[X]*u[Y];
    a13 += 2*u[X]*(u[X]*p[X] + u[Y]*p[Y]);
    a21 += 2*u[Y]*u[X];
    a22 += 2*u[Y]*u[Y];
    a23 += 2*u[Y]*(u[X]*p[X] + u[Y]*p[Y]);
  }   

  /* Okay, we now have a set of equations of the form
        a11eTx + a12eTy = a13
	a21eTx + a22eTy = a23

	These give
	   eTy = (a13a21 - a11a23)/(a12a21 - a11a22)
	   eTx = (a13 - a12eTy)/a11       */

#if DEBUGINFO
  fprintf(stderr, "Trying to solve : %16.11feTx + %16.11feTy = %16.11f\n",
	  a11, a12, a13);
  fprintf(stderr, "                  %16.11feTx + %16.11feTy = %16.11f\n",
	  a21, a22, a23);
#endif

  eT[Y] = (a13*a21 - a11*a23)/check0(a12*a21 - a11*a22, "eT.y");
  eT[X] = (a13 - a12*eT[Y])/check0(a11, "a11");
  
  chi_squared = 0.0;

  for (i = 0; i < numpoints; i++) {
    p = pArray[i];
    u = uArray[i];
    
    d = u[X]*(p[X] - eT[X]) + u[Y]*(p[Y] - eT[Y]);
    chi_squared += d*d;
  }
  
  return chi_squared;
}

void prepare_warp_matrix(double yaw, double pitch)
{
  matrix tempR;

  getRotationMatrix(tempR,Y,sin(-  yaw*M_PI/180.0),cos(- yaw*M_PI/180.0));
  getRotationMatrix(warpM,X,sin(-pitch*M_PI/180.0),cos(pitch*M_PI/180.0));
  
  matrixMult_3d(tempR, warpM, warpM);

#if DEBUGINFO
  matrix_print("warpM: ", warpM);
#endif
}

void warp_point(vector thePoint, vector *warped_point)
{
  vector tempresult;
  
  tempresult[X] = warpM[0][0]*thePoint[X] + warpM[1][0]*thePoint[Y]
    + warpM[2][0]*thePoint[Z];
  tempresult[Y] = warpM[0][1]*thePoint[X] + warpM[1][1]*thePoint[Y]
    + warpM[2][1]*thePoint[Z];
  tempresult[Z] = warpM[0][2]*thePoint[X] + warpM[1][2]*thePoint[Y]
    + warpM[2][2]*thePoint[Z];

  if (tempresult[Z] < 0) {
    /* NB: This is a Very Bad Thing but it will be silent unless
       DEBUGINFO is on. This may not be wise, but the warnings were
       bugging me. */
#if DEBUGINFO
    fprintf(stderr, "warp_point: point behind viewplane!!!\n");
    vec_print("w_p: ", tempresult);
#endif
  }
  (*warped_point)[X] = tempresult[X] / check0(tempresult[Z],
					      "warp_point: w_p[Z]");
  (*warped_point)[Y] = tempresult[Y] / check0(tempresult[Z], 
					      "warp_point: w_p[Z]");
  (*warped_point)[Z] = 1.0;
}

/* adjust_delta tries five steepest_delta calls starting at different
   procedures and returns the answer from the one which gives the
   smallest chi^2. NB: this is a Cheap Hack(TM) but Kevin likes it. */
void adjust_delta(void)
{
#define DELTA_INITIAL_VARIATION    0.05
  double founddelta[5][2];
  double foundchi2[5];
  double bestchi2;
  int i, bestdelta;

  bestchi2 = 999999.9;
  bestdelta = 0;
  for (i = 0; i < 5; i++) {
    switch(i) {
    case 0:
      foundchi2[i] = steepest_delta(0, 0);
      break;
    case 1:
      foundchi2[i] = steepest_delta(DELTA_INITIAL_VARIATION, 0);
      break;
    case 2:
      foundchi2[i] = steepest_delta(-DELTA_INITIAL_VARIATION, 0);
      break;
    case 3:
      foundchi2[i] = steepest_delta(0, DELTA_INITIAL_VARIATION);
      break;
    case 4:
      foundchi2[i] = steepest_delta(0, -DELTA_INITIAL_VARIATION);
      break;
    }
    founddelta[i][X] = delta[X];
    founddelta[i][Y] = delta[Y];
    if (foundchi2[i] < bestchi2) {
      bestchi2 = foundchi2[i];
      bestdelta = i;
    }
  }
  delta[X] = founddelta[bestdelta][X];
  delta[Y] = founddelta[bestdelta][Y];
} 

/* steepest_delta() performs an iterative technique upon delta in order
   to determine the best approximation so that the rotational motion
   of the camera may be accounted for. It uses a steepest-descent algorithm
   to do so, and the aim is to find the value of delta that minimizes
   the return value of get_best_eT(). (may be accomplished without any
   prior knowledge of what get_best_eT() actually does) */
double steepest_delta(double initial_x, double initial_y)
{
#define GRAD_EPSILON        1e-14
#define CHI2_TOLERANCE      0.0001
#define MAX_ITERATIONS      100
#define INITIAL_STEP        0.1
  
  double oldchi_square, chi_square;
  double step_size, grad_size;
  vector chi_grad, lastdelta;
  
  lastdelta[X] = 0;
  lastdelta[Y] = 0;
  delta[X] = initial_x;
  delta[Y] = initial_y;
  grad_size = 1.0;
  step_size = INITIAL_STEP;
  iterations = 0;
  chi_square = 999999999999.9; /* large number */
#if DEBUGINFO
  text_print("Adjusting delta ...");
#endif

  do {
    iterations++;
    oldchi_square = chi_square;
    chi_square = get_best_eT(delta[X], delta[Y]);
    while (chi_square > oldchi_square) {
      /* if we take a bad step, reduce the step size and
	 try again from the old delta. */
#if DEBUGINFO
      fprintf(stderr, "      chi^2(%20.17g) step(%10.7g) delta(%10.7f,%10.7f)
               REJECTING\n", chi_square, step_size, delta[X], delta[Y]);
#endif
      step_size /= 2;
      delta[X] = lastdelta[X] - step_size * (chi_grad[X] / grad_size);
      delta[Y] = lastdelta[Y] - step_size * (chi_grad[Y] / grad_size);
      chi_square = get_best_eT(delta[X], delta[Y]);
    }

#if DEBUGINFO
    fprintf(stderr, "%4d: chi^2(%20.17g) step(%10.7g) delta(%10.7f,%10.7f)
                     eT(%10.7f,%10.7f)", iterations, chi_square, step_size,
	             delta[X], delta[Y], eT[X], eT[Y]);
#endif

    if ((fabs(chi_square - oldchi_square)/chi_square < CHI2_TOLERANCE) &&
	(step_size < INITIAL_STEP/1024)) {        /* if not improving much, and quite close, stop! */
#if DEBUGINFO
      fprintf(stderr, "\n");
#endif
      break;
    }

    chi_grad[X] = (chi_square - get_best_eT((delta[X] - GRAD_EPSILON),
					    delta[Y]))/GRAD_EPSILON;
    chi_grad[Y] = (chi_square - get_best_eT(delta[X], (delta[Y]
					    - GRAD_EPSILON)))/GRAD_EPSILON;
    grad_size = sqrt(SQR(chi_grad[X]) + SQR(chi_grad[Y]));

    if (grad_size == 0) {
      /* we've found a minimum, stop. Shouldn't be any maximums, so safe. */
#if DEBUGINFO
      fprintf(stderr, "\n");
#endif
      break;
      }

#if DEBUGINFO
    fprintf(stderr, " chi'(%10.7g,%10.7g)\n", chi_grad[X], chi_grad[Y]);
#endif
    
    /* this code does delta -= step_size*chi_grad */
    lastdelta[X] = delta[X];
    lastdelta[Y] = delta[Y];
    delta[X] -= step_size * (chi_grad[X]/grad_size);
    delta[Y] -= step_size * (chi_grad[Y]/grad_size);

  } while (iterations < MAX_ITERATIONS);
  
#if DEBUGINFO
  fprintf(stderr, "%d iterations: delta (%20.17f,%20.17f)\n",
	  i, delta[X], delta[Y]);
#endif

  return chi_square;
}

/* adjust_warp tries five steepest_warp calls starting at different
   points and returns the answer from the one which gives the smallest
   chi^2. NB: this is a Cheap Hack(TM) but Kevin likes it. */
void adjust_warp(void)
{
#define WARP_INITIAL_VARIATION    1.0
  double foundwarp[5][2];
  double foundchi2[5];
  double bestchi2;
  int i, bestwarp;

  bestchi2 = 99999999999.9;
  bestwarp = 0;
  for (i = 0; i < 5; i++) {
    switch(i) {
    case 0:
      foundchi2[i] = steepest_warp(0, 0);
      break;
    case 1:
      foundchi2[i] = steepest_warp(WARP_INITIAL_VARIATION, 0);
      break;
    case 2:
      foundchi2[i] = steepest_warp(-WARP_INITIAL_VARIATION, 0);
      break;
    case 3:
      foundchi2[i] = steepest_warp(0, WARP_INITIAL_VARIATION);
      break;
    case 4:
      foundchi2[i] = steepest_warp(0, -WARP_INITIAL_VARIATION);
      break;
    }
    foundwarp[i][X] = warp_vec[X];
    foundwarp[i][Y] = warp_vec[Y];
    if (foundchi2[i] < bestchi2) {
      bestchi2 = foundchi2[i];
      bestwarp = i;
    }
  }
  warp_vec[X] = foundwarp[bestwarp][X];
  warp_vec[Y] = foundwarp[bestwarp][Y];
} 

/* steepest_warp() performs an iterative technique upon the warp in order
   to determine the best approximation so that the rotational motion of
   the camera may be accounted for. It uses a steepest-descent algorithm
   to do so, and the aim is to find the rotational warp that minimizes
   the return value of get_best_warped_eT(). (may be accomplished without
   any prior knowledge of what get_best_warped_eT() actually does) */
double steepest_warp(double initial_x, double initial_y)
{
#define WARP_GRAD_EPSILON        1e-06
#define WARP_CHI2_TOLERANCE      0.0001
#define WARP_MAX_ITERATIONS      100
#define WARP_INITIAL_STEP        10.0
  
  double oldchi_square, chi_square;
  double step_size, grad_size;
  vector chi_grad, last_warp;
  
  last_warp[X] = 0;
  last_warp[Y] = 0;
  warp_vec[X] = initial_x;  /* yaw */
  warp_vec[Y] = initial_y;  /* pitch */
  grad_size = 1.0;
  step_size = WARP_INITIAL_STEP;
  iterations = 0;
  chi_square = 99999999999.9; /* large number */
#if DEBUGINFO
  text_print("Adjusting the warp ...");
#endif
  
  do {
    iterations++;
    oldchi_square = chi_square;
    chi_square = get_best_warped_eT(warp_vec[X], warp_vec[Y]);
    while (chi_square > oldchi_square) {
      /* if we take a bad step, reduce the step size and
	 try again from the old warp. */
#if DEBUGINFO
      fprintf(stderr, "      chi^2(%20.17g) step(%10.7g) warp(%10.7f,%10.7f) 
                       REJECTING\n", chi_square, step_size, warp_vec[X],
	              warp_vec[Y]);
#endif
      step_size /= 2;
      warp_vec[X] = last_warp[X] - step_size * (chi_grad[X] / grad_size);
      warp_vec[Y] = last_warp[Y] - step_size * (chi_grad[Y] / grad_size);
      chi_square = get_best_warped_eT(warp_vec[X], warp_vec[Y]);
    }

#if DEBUGINFO
    fprintf(stderr, "%4d: chi^2(%20.17g) step(%10.7g) warp(%10.7f,%10.7f)
                     eT(%10.7f,%10.7f)", iterations, chi_square, step_size,
	            warp_vec[X], warp_vec[Y], eT[X], eT[Y]);
#endif
    
    if ((fabs(chi_square - oldchi_square)/chi_square < WARP_CHI2_TOLERANCE) &&
	(step_size < WARP_INITIAL_STEP/1024)) {
      /* if not improving much, and quite close, stop! */
      
#if DEBUGINFO
      fprintf(stderr, "\n");
#endif
	
      break;
    }

    chi_grad[X] = (chi_square - get_best_warped_eT((warp_vec[X]
                 - WARP_GRAD_EPSILON),warp_vec[Y]))/WARP_GRAD_EPSILON;
    chi_grad[Y] = (chi_square - get_best_warped_eT(warp_vec[X],(warp_vec[Y]
                 - WARP_GRAD_EPSILON)))/WARP_GRAD_EPSILON;
    grad_size = sqrt(SQR(chi_grad[X]) + SQR(chi_grad[Y]));

    if (grad_size == 0) {
      /* we've found a minimum, stop. */
#if DEBUGINFO
      fprintf(stderr, "\n");
#endif
      break;
      }

#if DEBUGINFO
      fprintf(stderr, " chi'(%10.7g,%10.7g)\n", chi_grad[X], chi_grad[Y]);
#endif
    
    /* warp_vec -= step_size*chi_grad */

    last_warp[X] = warp_vec[X];
    last_warp[Y] = warp_vec[Y];
    warp_vec[X] -= step_size * (chi_grad[X]/grad_size);
    warp_vec[Y] -= step_size * (chi_grad[Y]/grad_size);
  } while (iterations < WARP_MAX_ITERATIONS);

#if IMPORTANTDEBUGINFO
  fprintf(stderr, "%d iterations: warp_vec (%20.17f,%20.17f)\n", iterations, warp_vec[X], warp_vec[Y]);
#endif

  return chi_square;
}

double simplex_eT_interface(double *vec)
{
  return get_best_eT(vec[X], vec[Y]);
}

/* adjust_simplex_delta uses the simplex method in two dimensions to minimize
   the value of chi^2 on delta in 2D. Initial value is given. */
void adjust_simplex_delta(double initialx, double initialy, double (*evalfunc)(double *vec))
{
#define N_DIM             2
#define N_PTS             N_DIM+1
#define EVALUATE_MAX      5000
#define ALPHA             1.0
#define BETA              0.5
#define GAMMA             2.0
#define DESIRED_TOLERANCE 0.001
#define INITIAL_DELTA     0.02
  int i, j, lo_point, hi_point, nexthi_point;
  double pointsum[N_DIM];
  double point[N_PTS][N_DIM];
  double func[N_PTS];
  double sum, current_tolerance, attempt_func, hi_value;

  /* initialise number of f evals to zero */
  n_evals = 0;

  /* initial points form an equilateral triangle around initial_xy */     
  point[0][X] = initialx;
  point[0][Y] = initialy + INITIAL_DELTA;
  point[1][X] = initialx + INITIAL_DELTA*(sqrt(3.0)/2.0);
  point[1][Y] = initialy - INITIAL_DELTA*(0.5);
  point[2][X] = initialx - INITIAL_DELTA*(sqrt(3.0)/2.0);
  point[2][Y] = initialy - INITIAL_DELTA*(0.5);
  
  /* for each dimension (X, Y) */
  for (j = 0; j < N_DIM; j++) {
    /* take the sum of the X or Y co-ords of all 3 points 
       and store it in pointsum[X, Y] */
    for (sum = 0.0, i = 0; i < N_PTS; i++)         
      sum += point[i][j];
    pointsum[j] = sum;
  }
  
  /* initialise the array func[i] to hold function values at the points */
  for (i = 0; i < N_PTS; i++) {
    func[i] = (*evalfunc)(point[i]);
  }
  
  /* loop: begin a new iteration */
  for (;;) {
    
    /* loop over points to determine the ones with the lowest and */
    /* highest f values. Initialise lo_p to 0, hi_p to the bigger */
    /* of p0 and p1, nexthi_p to the lower of p0 and p1. */
    lo_point = 0;
    if (func[0] > func[1]) {
      hi_point = 0;
      nexthi_point = 1;
    } else {
      hi_point = 1;
      nexthi_point = 0;
    }
    
    /* actually perform the above loop. Update lo_p, hi_p, nexthi_p if */
    /* necessary. Upon exit these three values are correct. */
    for (i = 0; i < N_PTS; i++) {
      if (func[i] <= func[lo_point])
	lo_point = i;
      if (func[i] > func[hi_point]) {
	nexthi_point = hi_point;
	hi_point = i;
      } else if (func[i] > func[nexthi_point])
	nexthi_point = i;
    }

    /* compute the current fractional tolerance. Return the best value as */
    /* delta[X,Y] if we're within the desired tolerance OR we've exceeded */
    /* the maximum number of evaluations allowed. */
    current_tolerance = 2.0 * fabs(func[hi_point] - func[lo_point])
                        / (fabs(func[hi_point]) + fabs(func[lo_point]));
    if ((current_tolerance < DESIRED_TOLERANCE) ||
	(n_evals >= EVALUATE_MAX)) {
      delta[X] = point[lo_point][X];
      delta[Y] = point[lo_point][Y];
      break;
    }
    
    /* try a reflection through the highest point */
    attempt_func = simplex_try_point(point, func, pointsum, evalfunc,
				     hi_point, &n_evals, -ALPHA);
    /* if the new point is better than the best point, 
       try to extrapolate more in that direction */
    if (attempt_func <= func[lo_point]){
      attempt_func = simplex_try_point(point, func, pointsum, evalfunc,
				       hi_point, &n_evals, GAMMA);
    } else if (attempt_func >= func[nexthi_point]) {
      /* if it's worse than the second worst point, 
	 save the worst value */
      hi_value = func[hi_point];
      /* look for a lower point between the two tried */
      attempt_func = simplex_try_point(point, func, pointsum, evalfunc,
				       hi_point, &n_evals, BETA);
      /* if unsuccessful, contract around the lowest point */
      if (attempt_func >= hi_value) {
	/* for every point not the lowest, contract */
	for (i = 0; i < N_PTS; i++) {
	  if (i != lo_point) {
	    /* calculate new point */
	    for (j = 0; j < N_DIM; j++) {
	      pointsum[j] = 0.5*(point[i][j] + point[lo_point][j]);
	      point[i][j] = pointsum[j];
	    }
	    /* calculate function value at new point */
	    func[i] = (*evalfunc)(pointsum);
	  }
	}
	n_evals += N_PTS - 1;

	/* recompute pointsum[] as at start */
	for (j = 0; j < N_DIM; j++) {
	  for (sum = 0.0, i = 0; i < N_PTS; i++)         
	    sum += point[i][j];                   
	  pointsum[j] = sum;                      
	}
      }

    } else {
      /* If we've found a smaller point between the two tried,  
       update function evals and go back to next iteration */
      --n_evals;
    }
  }
  /* the main for loop never terminates normally, only through break; */
}

/* simplex_try_point extrapolates by a factor (factor) through the face
   of the simplex across from the high point, and if it finds a better
   point replaces the high point with the new (extrapolated) point. */
double simplex_try_point(double point[N_PTS][N_DIM], double func[N_PTS],
       double pointsum[N_DIM], double (*evalfunc)(double *vec), int hi_point,
       int *n_evals, double factor)
{
  int j;
  double trypoint[N_DIM];
  double factor1, factor2, tryvalue;

  /* these factors used to construct the extrapolated point */
  factor1 = (1.0 - factor)/N_DIM;
  factor2 = factor1 - factor;

  /* construct the extrapolated point */
  for (j = 0; j < N_DIM; j++)
    trypoint[j] = pointsum[j]*factor1 - point[hi_point][j]*factor2;

  /* evaluate the function at the extrapolated point */
  tryvalue = (*evalfunc)(trypoint);
  (*n_evals)++;

  /* if it's better than the current worst point */
  if (tryvalue < func[hi_point]) {
    func[hi_point] = tryvalue;
    for (j = 0; j < N_DIM; j++) {
      pointsum[j] += trypoint[j] - point[hi_point][j];
      point[hi_point][j] = trypoint[j];
    }
  }
  
  return tryvalue;
}
  
/* apply_delta() takes the delta that has been found by the successive
   approximation technique and pre-processes the correspondences
   accordingly. It uses the procedure fix(v) to apply the delta correction
   to the vector v. */
void apply_delta(void)
{
  int i;

  fix(p0_prime);
  fix(p1_prime);
  fix(p2_prime);
  fix(p3_prime);
  for (i = 0; i < numpoints; i++)
    fix(p_primeArray[i]);
}

/* apply_warp() takes the warp that has been found by the successive
   approximation technique and adjusts the correspondences accordingly. */
void apply_warp(void)
{
  int i;

  prepare_warp_matrix(warp_vec[X], warp_vec[Y]);
  
  warp_point(p0_prime, &p0_prime);
  warp_point(p1_prime, &p1_prime);
  warp_point(p2_prime, &p2_prime);
  warp_point(p3_prime, &p3_prime);
  for (i = 0; i < numpoints; i++)
    warp_point(p_primeArray[i], &p_primeArray[i]);
}

/* fix() adjusts the vector v to account for the rotational camera
   motion by using delta. */
void fix(vector toFix)
{
  toFix[X] += delta[X];
  toFix[Y] += delta[Y];
}

/* adjust_axes() determines the axis which has the most parallax on it
   between the two images, and rotates the reference points p0-3
   and p'0-3 to make this axis p0-p3 while preserving the handedness
   of the co-ordinate system */
void adjust_axes(void)
{
  int bestAxis;
  line l1, l2;
  vector e1, e2, e3, tempvect;
  double d1, d2, d3;

  get_line(p0, p1, l1);
  get_line(p0_prime, p1_prime, l2);
  line_intersect(l1, l2, e1);

  get_line(p0, p2, l1);
  get_line(p0_prime, p2_prime, l2);
  line_intersect(l1, l2, e2);

  get_line(p0, p3, l1);
  get_line(p0_prime, p3_prime, l2);
  line_intersect(l1, l2, e3);
  
  d1 = SQR(e1[X]) + SQR(e1[Y]);
  d2 = SQR(e2[X]) + SQR(e2[Y]);
  d3 = SQR(e3[X]) + SQR(e3[Y]);
  
  /* We look for the **closest** vanishing point, which is the one with
     the **most** parallax on it. */
  if (d1 < d3)
    if (d1 < d2)
      bestAxis = 1;
    else
      bestAxis = 2;
  else
    if (d2 < d3)
      bestAxis = 2;
    else
      bestAxis = 3;

  switch (bestAxis) {
  case 0:{
    text_print("ERROR! BestAxis == 0!");
    rotateaxes = 0;
    break;
  }
  case 1:{
    /* rotate p3 -> p2 -> p1 -> p3 so that old p1 is now in p3 position */
    VSET(tempvect, p3);
    VSET(p3, p1);
    VSET(p1, p2);
    VSET(p2, tempvect);

    VSET(tempvect, p3_prime);
    VSET(p3_prime, p1_prime);
    VSET(p1_prime, p2_prime);
    VSET(p2_prime, tempvect);

    rotateaxes = 1;
    break;
  }
  case 2: {
    /* rotate p3 -> p1 -> p2 -> p3 so that old p2 is now in p3 position */
    VSET(tempvect, p3);
    VSET(p3, p2);
    VSET(p2, p1);
    VSET(p1, tempvect);

    VSET(tempvect, p3_prime);
    VSET(p3_prime, p2_prime);
    VSET(p2_prime, p1_prime);
    VSET(p1_prime, tempvect);

    rotateaxes = 2;
    break;
  }
  case 3: {
    /* no rotation necessary, p3 already has good parallax */
    rotateaxes = 3;
    break;
  }
  }
}

/* readjust_axes() restores the affine coordinates back to the original
 axes that were presented to the program. (undoes the effects of
 adjust_axes) */
void readjust_axes(void)
{
  int i;
  vector tempvect;

  for (i = 0; i < numpoints; i++) {
    VSET(tempvect, affineArray[i]);
    switch(rotateaxes){
    case 0:{
      text_print("ERROR! rotateaxes == 0");
      break;
    }
    case 1:{
      affineArray[i][X] = tempvect[Z];
      affineArray[i][Y] = tempvect[X];
      affineArray[i][Z] = tempvect[Y];
      break;
    }
    case 2:{
      affineArray[i][X] = tempvect[Y];
      affineArray[i][Y] = tempvect[Z];
      affineArray[i][Z] = tempvect[X];
      break;
    }
    case 3:{
      /* no adjustment necessary */
      break;
    }
    }
  }
}     

/*======== ANALYSIS FUNCTIONS =========*/ 

/* average_error() returns the average error in % between the specified
 reference and actual affine co-ordinates. */
float average_error(vector *refpoints, vector *affpoints)
{
  float totError, maxValue;
  int i, index;

  maxValue = 0.0;
  for (i = 0; i < numpoints; i++)
    for (index = X; index <= Z; index++)
    if (maxValue < fabs(refpoints[i][index]))
      maxValue = fabs(refpoints[i][index]);
  if (maxValue == 0)
    fatalError("average_error: maxvalue of 0.0");
  
  totError = 0.0;
  for(i = 0; i < numpoints; i++)
    for (index = X; index <= Z; index++)
      totError += fabs(affpoints[i][X] - refpoints[i][X]);

  return (100.0*totError/maxValue) / (numpoints*3);
}

/*====== PROJECTION FUNCTIONS ======*/ 
/* rotate_project3d() takes a specified 3D view and a set of 3D points
   which are then projected onto the virtual image plane using a 3D->2D
   projection matrix for each view. */
void rotate_project3d(double yaw, double pitch, double rollc, vector vf1,
  vector vf2, vector vt1, vector vt2, vector *points_3d, vector **p1_ref,
  vector **p1_corr, vector **p2_ref, vector **p2_corr)
{
  vector view_dir, up_dir, right_dir;
  vector new_viewdir, new_updir, new_rightdir;
  double cos_pitch, cos_yaw, sin_pitch, sin_yaw;
  int i;

  numpoints -= 4;                    /* adjust for the reference points */
  *p1_ref = alloc_vector(4);
  *p2_ref = alloc_vector(4);
  *p1_corr = alloc_vector(numpoints);
  *p2_corr = alloc_vector(numpoints);

  /* I had enormous problems with trying to rotate the points around so that
     the view vector ended up along the Z axis. So now, I'm creating a new
     basis system with Z along the view vector, Y along its upvector and X
     along its rightvector.
       Maybe it will work better. Then again ...  */

  /* First for the first camera ... */
  VSUB(vt1, vf1, view_dir); 
  VTIMES(1.0/check0(VMAG(view_dir), "view_dir in project3d"),
	 view_dir, view_dir);
  SET_VECTOR(up_dir, 0.0, 0.0, 1.0, 1.0);
  VCROSS(view_dir, up_dir, right_dir);
  VTIMES(1.0/check0(VMAG(right_dir), "right_dir in project3d"),
	 right_dir, right_dir);
  VCROSS(right_dir, view_dir, up_dir);
  VTIMES(1.0/check0(VMAG(up_dir), "up_dir in project3d"), up_dir, up_dir);

#if DEBUGINFO
  vec_print("view_dir:", view_dir);
  vec_print("right_dir:", right_dir);
  vec_print("up_dir:", up_dir);
  scalar_print("v_d . r_d:", VDOT(view_dir, right_dir));
  scalar_print("v_d . u_d:", VDOT(view_dir, up_dir));
  scalar_print("r_d . u_d:", VDOT(right_dir, up_dir));
#endif

  /* Now that we have the basis system for the first camera worked out,
     use it to determine the projections of the 3D points
     (in the first camera). */
  for (i = 0; i < 4; i++)
    project_point(vf1, right_dir, up_dir, view_dir,
		  points_3d[i], (*p1_ref)[i]);

  for (i = 0; i < numpoints; i++)
    project_point(vf1, right_dir, up_dir, view_dir,
		  points_3d[i + 4], (*p1_corr)[i]);

  /* Now for the second camera ... */
  VSUB(vt2, vf2, view_dir); 
  VTIMES(1.0/check0(VMAG(view_dir), "view_dir in project3d"),
	 view_dir, view_dir);
  SET_VECTOR(up_dir, 0.0, 0.0, 1.0, 1.0);
  VCROSS(view_dir, up_dir, right_dir);
  VTIMES(1.0/check0(VMAG(right_dir), "right_dir in project3d"),
	 right_dir, right_dir);
  VCROSS(right_dir, view_dir, up_dir);
  VTIMES(1.0/check0(VMAG(up_dir), "up_dir in project3d"), up_dir, up_dir);

#if DEBUGINFO
  vec_print("view_dir:", view_dir);
  vec_print("right_dir:", right_dir);
  vec_print("up_dir:", up_dir);
  scalar_print("v_d . r_d:", VDOT(view_dir, right_dir));
  scalar_print("v_d . u_d:", VDOT(view_dir, up_dir));
  scalar_print("r_d . u_d:", VDOT(right_dir, up_dir));
#endif

  /* we have the basis system for the unrotated camera, now rotate
     it as a rigid body */
  cos_pitch = cos(pitch*M_PI/180.0);
  cos_yaw = cos(yaw*M_PI/180.0);
  sin_pitch = sin(pitch*M_PI/180.0);
  sin_yaw = sin(yaw*M_PI/180.0);
  
  /* new view = cos(yaw)cos(pitch)view + cos(pitch)sin(yaw)right
     + sin(pitch)up */
  VTIMES(cos_yaw*cos_pitch, view_dir, new_viewdir);
  VFRACADD(new_viewdir, cos_pitch*sin_yaw, right_dir);
  VFRACADD(new_viewdir, sin_pitch, up_dir);

  /* new up = cos(pitch)up - sin(pitch)cos(yaw)view
     - sin(pitch)sin(yaw)right */
  VTIMES(cos_pitch, up_dir, new_updir);
  VFRACADD(new_updir, -sin_pitch*cos_yaw, view_dir);
  VFRACADD(new_updir, -sin_pitch*sin_yaw, right_dir);

  /* new right = cos(yaw)right - sin(yaw)view */
  VTIMES(cos_yaw, right_dir, new_rightdir);
  VFRACADD(new_rightdir, -sin_yaw, view_dir);
  
  /* renormalise the modified view, up and right vectors */
  VTIMES(1.0/check0(VMAG(new_viewdir), "new_viewdir in project3d"),
	 new_viewdir, new_viewdir);
  VTIMES(1.0/check0(VMAG(new_updir), "new_updir in project3d"),
	 new_updir, new_updir);
  VTIMES(1.0/check0(VMAG(new_rightdir), "new_rightdir in project3d"),
	 new_rightdir, new_rightdir);

#if DEBUGINFO
  vec_print("new view_dir:", new_viewdir);
  vec_print("new right_dir:", new_rightdir);
  vec_print("new up_dir:", new_updir);
  scalar_print("new v_d . r_d:", VDOT(new_viewdir, new_rightdir));
  scalar_print("new v_d . u_d:", VDOT(new_viewdir, new_updir));
  scalar_print("new r_d . u_d:", VDOT(new_rightdir, new_updir));
#endif
  
  /* Now that we have the (rotated) basis system for the second camera
     worked out, use it to determine the projections of the 3D points
     (in the second camera). */
  for (i = 0; i < 4; i++)
    project_point(vf2, new_rightdir, new_updir, new_viewdir,
		  points_3d[i], (*p2_ref)[i]);

  for (i = 0; i < numpoints; i++)
    project_point(vf2, new_rightdir, new_updir, new_viewdir,
		  points_3d[i + 4], (*p2_corr)[i]);

  /* ********* NB *********** Must apply the roll coefficient here,
     don't forget */
}

/* project3d() takes a specified 3D view and a set of 3D points which
   are then projected onto the virtual image plane using a 3D->2D
   projection matrix for each view. */
void project3d(vector vf1, vector vf2, vector vt1, vector vt2,
	       double rollc, vector *points_3d, vector **p1_ref,
	       vector **p1_corr, vector **p2_ref, vector **p2_corr)
{
  vector view_dir, up_dir, right_dir;
  int i;

  numpoints -= 4;                    /* adjust for the reference points */
  *p1_ref = alloc_vector(4);
  *p2_ref = alloc_vector(4);
  *p1_corr = alloc_vector(numpoints);
  *p2_corr = alloc_vector(numpoints);

  /* I had enormous problems with trying to rotate the points around so that
     the view vector ended up along the Z axis. So now, I'm creating a new
     basis system with Z along the view vector, Y along its upvector and X
     along its rightvector. This allows direct projection without any
     time-consuming rotation for every point.
       Maybe it will work better. */

  /* First for the first camera ... */
  VSUB(vt1, vf1, view_dir); 
  VTIMES(1.0/check0(VMAG(view_dir), "view_dir in project3d"), view_dir,
	 view_dir);
  SET_VECTOR(up_dir, 0.0, 0.0, 1.0, 1.0);
  VCROSS(view_dir, up_dir, right_dir);
  VTIMES(1.0/check0(VMAG(right_dir), "right_dir in project3d"), right_dir,
	 right_dir);
  VCROSS(right_dir, view_dir, up_dir);
  VTIMES(1.0/check0(VMAG(up_dir), "up_dir in project3d"), up_dir, up_dir);
#if DEBUGINFO
  vec_print("view_dir:", view_dir);
  vec_print("right_dir:", right_dir);
  vec_print("up_dir:", up_dir);
  scalar_print("v_d . r_d:", VDOT(view_dir, right_dir));
  scalar_print("v_d . u_d:", VDOT(view_dir, up_dir));
  scalar_print("r_d . u_d:", VDOT(right_dir, up_dir));
#endif

  /* Now that we have the basis system for the first camera worked out,
     use it to determine the projections of the 3D points
     (in the first camera). */
  for (i = 0; i < 4; i++)
    project_point(vf1, right_dir, up_dir, view_dir,
		  points_3d[i], (*p1_ref)[i]);

  for (i = 0; i < numpoints; i++)
    project_point(vf1, right_dir, up_dir, view_dir,
		  points_3d[i + 4], (*p1_corr)[i]);

  /* Now for the second camera ... */
  VSUB(vt2, vf2, view_dir); 
  VTIMES(1.0/check0(VMAG(view_dir), "view_dir in project3d"),
	 view_dir, view_dir);
  SET_VECTOR(up_dir, 0.0, 0.0, 1.0, 1.0);
  VCROSS(view_dir, up_dir, right_dir);
  VTIMES(1.0/check0(VMAG(right_dir), "right_dir in project3d"),
	 right_dir, right_dir);
  VCROSS(right_dir, view_dir, up_dir);
  VTIMES(1.0/check0(VMAG(up_dir), "up_dir in project3d"),
	 up_dir, up_dir);

  /* Now that we have the basis system for the second camera worked out,
     use it to determine the projections of the 3D points
     (in the second camera). */
  for (i = 0; i < 4; i++)
    project_point(vf2, right_dir, up_dir, view_dir,
		  points_3d[i],(*p2_ref)[i]);

  for (i = 0; i < numpoints; i++)
    project_point(vf2, right_dir, up_dir, view_dir,
		  points_3d[i + 4], (*p2_corr)[i]);

  /* ********* NB *********** Must apply the roll coefficient here,
     don't forget */
}

void project_point(vector origin, vector X_vec, vector Y_vec, vector Z_vec,
		   vector thePoint, vector projection)
{
  vector relPoint;

  VSUB(thePoint, origin, relPoint);
  
  projection[X] = VDOT(relPoint, X_vec);
  projection[Y] = VDOT(relPoint, Y_vec);
  projection[Z] = VDOT(relPoint, Z_vec);

  projection[X] /= check0(projection[Z], "projection[Z]");
  projection[Y] /= check0(projection[Z], "projection[Z]");
  projection[Z] = 1.0;
}

/*========== NOISE FUNCTIONS ========*/ 

/* add_noise() seeds the random number generator from the system timer
   and adds random noise to all of the input correspondences. This
   noise is measured in percent of the distance p0 <-> p3 ...
   e.g. 1.0 means the maximum variation in the X and Y axes is 1%
   of the p0-p3 distance */
void add_noise(double noisepercent, vector *p1_ref, vector *p1_corr,
	       vector *p2_ref, vector *p2_corr)
{
  double noisedist;
  struct timespec mytime;
  int i;

  /* compute noise distance that's necessary for noisepercent% */
  noisedist = sqrt(SQR(p1_ref[3][X] - p1_ref[0][X])
		   + SQR(p1_ref[3][Y] - p1_ref[0][Y]));
#if DEBUGINFO
  fprintf(stderr, "Initial noisedist %16.11f\n", noisedist);
#endif

  noisedist *= noisepercent/100.0;

#if DEBUGINFO
  fprintf(stderr, "Noise +/- term is %16.11f\n", noisedist);
#endif
  
  /* initialise random number generator */
  getclock(TIMEOFDAY, &mytime);
  srand48(mytime.tv_nsec);

#if DEBUGINFO
  fprintf(stderr, "Random number generator seeded :
                    first %13.11f\n", drand48());
#endif
  
  for (i = 0; i < 4; i++) {
    apply_noise(noisedist, p1_ref[i]);
    apply_noise(noisedist, p2_ref[i]);
  }
  for (i = 0; i < numpoints; i++) {
    apply_noise(noisedist, p1_corr[i]);
    apply_noise(noisedist, p2_corr[i]);
  }
}

/* apply_noise() applies noise of magnitude noisedist to the
   given correspondence vector. */
void apply_noise(double noisedist, vector toNoisify)
{
  toNoisify[X] += noisedist*2*(drand48() - 0.5);
  toNoisify[Y] += noisedist*2*(drand48() - 0.5);
}

/*======== ROTATION FUNCTIONS =========*/ 

/* getRotationMatrix places a matrix in R that will rotate about the
   given axis by an amount specified by sin(theta) and cos(theta)
   [supplied to avoid unnecessary calculation] */
void getRotationMatrix(matrix R, int theAxis, double sintheta, double costheta)
{
  /* This returns a matrix that should rotate about the given
     axis by the given amount (specified by sin(theta) and cos(theta)). */
  int axis2, axis3;
  
  switch (theAxis) {
    case X: {
      axis2 = Y;
      axis3 = Z;
      break;
    }
    case Y: {
      axis2 = X;
      axis3 = Z;
      break;
    }
    case Z: {
      axis2 = X;
      axis3 = Y;
      break;
    }
    default: {
      fprintf(stderr, "Can't rotate about axis %d!\n", theAxis);
      exit(-1);
    }
  }
  
  R[axis2]  [axis2]   = costheta;
  R[axis3]  [axis2]   = -sintheta;
  R[theAxis][axis2]   = 0.0;
  R[3]      [axis2]   = 0.0;
  R[axis2]  [axis3]   = sintheta;
  R[axis3]  [axis3]   = costheta;
  R[theAxis][axis3]   = 0.0;
  R[3]      [axis3]   = 0.0;
  R[axis2]  [theAxis] = 0.0;
  R[axis3]  [theAxis] = 0.0;
  R[theAxis][theAxis] = 1.0;
  R[3]      [theAxis] = 0.0;
  R[axis2]  [3]       = 0.0;
  R[axis3]  [3]       = 0.0;
  R[theAxis][3]       = 0.0;
  R[3]      [3]       = 1.0;
}

/*======== ROTATION-MAP FUNCTIONS ========*/ 

/* Uses the global variables defined for rotationmap() to return the
   error for a certain picture. */
double getRotationError(double yaw, double pitch)
{
  int do_axes, approx_method;

  /* Set current view back to original 3D view */
  VSET(new_vt2, orig_vt2);
  numpoints = orignumpoints;
  free(p1_ref);
  free(p2_ref);
  free(p1_corr);
  free(p2_corr);
  rotate_project3d(yaw, pitch, 0.0, vf1, vf2, vt1, new_vt2, points_3d,
		   &p1_ref, &p1_corr, &p2_ref, &p2_corr);
  
  free(pAffine);
  do_axes = methodnum % 2;
  approx_method = methodnum / 2;
  pAffine = AffineCalc(p1_ref, p2_ref, p1_corr, p2_corr, approx_method,
		       do_axes);      
  return average_error(referenceAffine, pAffine);
}  

/*========= PROJECTION FUNCTIONS =========*/

/* Takes a correspondence file and maps each point in it to the
   nearest place in a grid placed on the view-plane. */
void map_pixels(double pixel_spacing, vector *p1_ref, vector *p1_correspond,
		vector *p2_ref, vector *p2_correspond)
{
  int i;

  for (i = 0; i < 4; i++) {
    map_to_pixel(pixel_spacing, p1_ref[i]);
    map_to_pixel(pixel_spacing, p2_ref[i]);
  }
  for (i = 0; i < numpoints; i++) {
    if (i == 0) {
      vec_print("before 0:", p1_correspond[i]);
      map_to_pixel(pixel_spacing, p1_correspond[i]);
      vec_print("after 0:", p1_correspond[i]);
    } else {
      map_to_pixel(pixel_spacing, p1_correspond[i]);
    }
    map_to_pixel(pixel_spacing, p2_correspond[i]);
  }
}

/* Maps a point to the nearest place in a grid on the view-plane,
   spacing pixel_spacing. */
void map_to_pixel(double pixel_spacing, vector aPoint)
{
  int pixel_x, pixel_y;

  pixel_x = (int) (fabs(aPoint[X] / pixel_spacing) + 0.5);
  if (aPoint[X] < 0)
    pixel_x = -pixel_x;

  pixel_y = (int) (fabs(aPoint[Y] / pixel_spacing) + 0.5);
  if (aPoint[Y] < 0)
    pixel_y = -pixel_y;

  aPoint[X] = pixel_x * pixel_spacing;
  aPoint[Y] = pixel_y * pixel_spacing;
}

/************ SUPPORT FUNCTIONS ***********/

/*======= MEMORY ALLOCATION =========*/ 

/* This copies numvec vectors from array 1 to array 2. */
void veccpy(vector *array1, vector *array2, int numvec)
{
  int i;

  for (i = 0; i < numvec; i++)
    VSET(array2[i], array1[i]);
}

/* alloc_vector() returns a pointer to a block of memory big enough for
 n vectors, using calloc. */
vector *alloc_vector(int numVectors)
{
  vector *tempalloc;

  if (numVectors > MAX_NUMPOINTS)
    fatalError("Error: exceeded MAX_NUMPOINTS!");
  
   if ((tempalloc = (vector *)calloc(numVectors, sizeof(vector))) == NULL) {
    sprintf(buf0, "alloc_vector: can't allocate %d vectors", numVectors);
    fatalError(buf0);
  }
  return tempalloc;
}

/* alloc_pgm() returns a pointer to a list of row pointers to a
   pgm image of the given size */
gray **alloc_pgm(int width, int height)
{
  gray *image, **rows;
  int i;

  /* Allocate space for image and row pointer arrays */
  if ((image = (gray *)calloc(width*height, sizeof(gray))) == NULL)
    fatalError("alloc_pgm: cannot allocate image data");

  if ((rows = (gray **)calloc(height, sizeof(gray *))) == NULL)
    fatalError("alloc_pgm: cannot allocate row pointers");
  
  /* Set up image row pointers */
  for (i = 0; i < height; i++)
    rows[i] = &image[i * width] ;
  
  return rows;
}

/*======== FILE INPUT/OUTPUT ========*/

/* Read in the given .3D file */
void read_3d_file(char *filename, vector vf1, vector vf2, vector vt1,
		  vector vt2, double *rollc, vector **points_3d)
{
  FILE *infile;
  int i;

  if ((infile = fopen(filename, "r")) == NULL) {
    sprintf(buf0, "read_3d_file: can't open %s", filename);
    fatalError(buf0);
  }
  get_3d_point(infile, vf1);
  get_3d_point(infile, vt1);
  get_3d_point(infile, vf2);
  get_3d_point(infile, vt2);
  
  get_nextline(infile, buf0);
  if (sscanf(buf0, "%lf", rollc) < 1) {
    sprintf(buf0, "read_3d_file: corrupt rollcoeff in %s", filename);
    fatalError(buf0);
  }

  get_nextline(infile, buf0);
  if (sscanf(buf0, "%d", &numpoints) < 1) {
    sprintf(buf0, "read_3d_file: corrupt numpoints in %s", filename);
    fatalError(buf0);
  }

  if (numpoints > MAX_NUMPOINTS)
    fatalError("Error: exceeded MAX_NUMPOINTS!");
  
   *points_3d = alloc_vector(numpoints);
  for (i = 0; i < numpoints; i++)
    get_3d_point(infile, (*points_3d)[i]);

  fclose(infile);
}

/* Write out a .3D file */
void write_3d_file(char *filename, vector vf1, vector vf2, vector vt1,
		   vector vt2, double rollc, vector *points_3d)
{
  FILE *outfile;
  int i;

  if ((outfile = fopen(filename, "w")) == NULL) {
    sprintf(buf0, "write_3d_file: can't open file %s", filename);
    fatalError(buf0);
  }
  fprintf(outfile, "; This is a 3D model for generating sample
                    test data that\n");
  fprintf(outfile, "; has pure 2D co-ordinates.\n");
  fprintf(outfile, "\n");
  fprintf(outfile, "; View From 1 :\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", vf1[X], vf1[Y], vf1[Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; View To 1 :\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", vt1[X], vt1[Y], vt1[Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; View From 2 :\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", vf2[X], vf2[Y], vf2[Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; View To 2 :\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", vt2[X], vt2[Y], vt2[Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; Roll Clockwise (degrees)\n");
  fprintf(outfile, "%16.11f\n", rollc);
  fprintf(outfile, "\n");
  fprintf(outfile, "; Number of Points to Project\n");
  fprintf(outfile, "%d\n", numpoints);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P0\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", points_3d[0][X],
                    points_3d[0][Y], points_3d[0][Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P1\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", points_3d[1][X],
                    points_3d[1][Y], points_3d[1][Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P2\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", points_3d[2][X],
	  points_3d[2][Y], points_3d[2][Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P3\n");
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", points_3d[3][X],
	  points_3d[3][Y], points_3d[3][Z]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; Data points\n");
  for (i = 4; i < numpoints; i++)
    fprintf(outfile, "%16.11f %16.11f %16.11f\n", points_3d[i][X],
	    points_3d[i][Y], points_3d[i][Z]);
  fclose(outfile);
}

/* Read in the given .2D file */
void read_correspondence_file(char *filename, vector **p1ref,
			      vector **p2ref, vector **p1, vector **p2)
{
  FILE *infile;
  int i;

  if ((infile = fopen(filename, "r")) == NULL) {
    sprintf(buf0, "read_correspondence_file: can't open %s", filename);
    fatalError(buf0);
  }
  *p1ref = alloc_vector(4);
  *p2ref = alloc_vector(4);
  get_2d_point(infile, (*p1ref)[0]);
  get_2d_point(infile, (*p2ref)[0]);
  get_2d_point(infile, (*p1ref)[1]);
  get_2d_point(infile, (*p2ref)[1]);
  get_2d_point(infile, (*p1ref)[2]);
  get_2d_point(infile, (*p2ref)[2]);
  get_2d_point(infile, (*p1ref)[3]);
  get_2d_point(infile, (*p2ref)[3]);

  get_nextline(infile, buf0);
  if (sscanf(buf0, "%d", &numpoints) < 1) {
    sprintf(buf0, "read_correspondence_file:
                   corrupt number of correspondences in %s", filename);
    fatalError(buf0);
  }
  if (numpoints > MAX_NUMPOINTS)
    fatalError("Error: exceeded MAX_NUMPOINTS!");
  
   *p1 = alloc_vector(numpoints);
  *p2 = alloc_vector(numpoints);
  for (i = 0; i < numpoints; i++) {
    get_2d_point(infile, (*p1)[i]);
    get_2d_point(infile, (*p2)[i]);
  }
  fclose(infile);
}

/* Write out a .2D file */
void write_correspondence_file(char *filename, vector *p1_ref,
	     vector *p1_cond, vector *p2_ref, vector *p2_cond)
{
  FILE *outfile;
  int i;

  if ((outfile = fopen(filename, "w")) == NULL) {
    sprintf(buf0, "write_correspondence_file: can't open file %s", filename);
    fatalError(buf0);
  }
  fprintf(outfile, ";These are simulated 2D co-ordinates for a set\n");
  fprintf(outfile, ";of 3D points.\n");
  fprintf(outfile, "\n");
  fprintf(outfile, "; P0 : the basis of the axes\n");
  fprintf(outfile, "%16.11f %16.11f\n", p1_ref[0][X], p1_ref[0][Y]);
  fprintf(outfile, "%16.11f %16.11f\n", p2_ref[0][X], p2_ref[0][Y]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P1 : the X affine axis\n");
  fprintf(outfile, "%16.11f %16.11f\n", p1_ref[1][X], p1_ref[1][Y]);
  fprintf(outfile, "%16.11f %16.11f\n", p2_ref[1][X], p2_ref[1][Y]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P2 : the Y affine axis\n");
  fprintf(outfile, "%16.11f %16.11f\n", p1_ref[2][X], p1_ref[2][Y]);
  fprintf(outfile, "%16.11f %16.11f\n", p2_ref[2][X], p2_ref[2][Y]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; P3 : the Z affine axis\n");
  fprintf(outfile, "%16.11f %16.11f\n", p1_ref[3][X], p1_ref[3][Y]);
  fprintf(outfile, "%16.11f %16.11f\n", p2_ref[3][X], p2_ref[3][Y]);
  fprintf(outfile, "\n");
  fprintf(outfile, "; The number of correspondences found\n");
  fprintf(outfile, "%d\n", numpoints);
  fprintf(outfile, "\n");
  fprintf(outfile, "; The correspondences, each in the form\n");
  fprintf(outfile, "; <p.x       p.y>\n");
  fprintf(outfile, "; <p_prime.x p_prime.y>\n");
  fprintf(outfile, "\n");
  for (i = 0; i < numpoints; i++) {
    fprintf(outfile, "%16.11f %16.11f\n", p1_cond[i][X], p1_cond[i][Y]);
    fprintf(outfile, "%16.11f %16.11f\n", p2_cond[i][X], p2_cond[i][Y]);
    fprintf(outfile, "\n");
  }
  fclose(outfile);
}

/* Read in an affine co-ordinate (.affine) file */
vector *read_affine_file(char *filename)
{
  FILE *infile;
  vector *thepoints;
  int i, newnumpoints;

  if ((infile = fopen(filename, "r")) == NULL) {
    sprintf(buf0, "read_affine_file: can't open %s", filename);
    fatalError(buf0);
  }
  get_nextline(infile, buf0);
  if (sscanf(buf0, "%d", &newnumpoints) != 1)
    fatalError("read_affine_file: corrupt numpoints");

  if (newnumpoints != numpoints && numpoints != 0)
    fatalError("read_affine_file: differing number of points");
  
  if (newnumpoints > MAX_NUMPOINTS)
    fatalError("Error: exceeded MAX_NUMPOINTS!");
  
  numpoints = newnumpoints;
  thepoints = alloc_vector(numpoints);

  for (i = 0; i < numpoints; i++)
    get_3d_point(infile, thepoints[i]);
  fclose(infile);
  
  return thepoints;
}

/* Read in a connectivity (.con) file */
int *read_connectivity_file(char *filename, int *num_tri)
{
  FILE *infile;
  int *connects;
  int i;

  if ((infile = fopen(filename, "r")) == NULL) {
    sprintf(buf0, "read_connectivity_file: can't open %s", filename);
    fatalError(buf0);
  }
  get_nextline(infile, buf0);
  if (sscanf(buf0, "%d", num_tri) != 1)
    fatalError("read_connectivity_file: corrupt num_tri");

  if ((connects = (int *)calloc(3 * (*num_tri + 1), sizeof(int))) == NULL) {
    sprintf(buf0, "read_connectivity_file: can't allocate %d triangles",
	    *num_tri);
    fatalError(buf0);
  }

  for (i = 0; i < *num_tri; i++) {
    get_nextline(infile, buf0);
    if (sscanf(buf0, "%d %d %d", &connects[3*i + 0], &connects[3*i + 1],
	       &connects[3*i + 2]) < 3)
      fatalError("read_connectivity_file: can't read next triangle");
  }
  fclose(infile);
  
  return connects;
}

/* Read in a connectivity (.con) file */
void write_triangle_file(char *filename, vector *pAffine, int *connects,
             int num_tri)
{
  FILE *outfile;
  int i;
  
  if ((outfile = fopen(filename, "w")) == NULL) {
    sprintf(buf0, "write_triangle_file: can't write to %s", filename);
    fatalError(buf0);
  }


  fprintf(outfile, "%d\n", num_tri);
  for (i = 0; i < num_tri; i++) {
    output_vector(outfile, pAffine[connects[3*i + 0]]);
    output_vector(outfile, pAffine[connects[3*i + 1]]);
    output_vector(outfile, pAffine[connects[3*i + 2]]);
  }

  fclose(outfile);
}

/* Write out vector to a file */
void output_vector(FILE *outfile, vector theVector)
{
  fprintf(outfile, "%16.11f %16.11f %16.11f\n", theVector[X],
	  theVector[Y], theVector[Z]);
}

/* Write out a .affine file */
void write_affine_file(char *filename, vector *pointArray)
{
  FILE *outfile;
  int i;

  if ((outfile = fopen(filename, "w")) == NULL) {
    sprintf(buf0, "write_affine_file: can't open file %s", filename);
    fatalError(buf0);
  }
  fprintf(outfile, ";****Affine co-ordinate output****\n");
  fprintf(outfile, "\n; Number of points is :\n");
  fprintf(outfile, "%d\n", numpoints);
  fprintf(outfile, "\n; Point data follows ...\n");
  for (i = 0; i < numpoints; i++)
    fprintf(outfile, "%16.11f %16.11f %16.11f\n", pointArray[i][X],
	    pointArray[i][Y], pointArray[i][Z]);
  fclose(outfile);
}

/* Read in an XY point from the given file */
void get_2d_point(FILE *infile, vector xypoint)
{
  get_nextline(infile, buf0);
  if (sscanf(buf0, "%lf %lf", &xypoint[X], &xypoint[Y]) < 2)
    fatalError("get_2d_point: can't read next XYpoint");
  xypoint[Z] = 1.0;
  xypoint[W] = 1.0;
}

/* Read in an XYZ point from the given file */
void get_3d_point(FILE *infile, vector xyzpoint)
{
  get_nextline(infile, buf0);
  if (sscanf(buf0, "%lf %lf %lf", &xyzpoint[X], &xyzpoint[Y],
	     &xyzpoint[Z]) < 2)
    fatalError("get_3d_point: can't read next XYZpoint");
  xyzpoint[W] = 1.0;
}

/* Get the next non-comment, non-blank line from the file */
void get_nextline(FILE *infile, char *buffer)
{
  do {
    if ((buffer = fgets(buffer, 95, infile)) == NULL)
      fatalError("get_nextline: unexpected file termination");
  } while (buffer[0] == ';' || strlen(buffer) < 2);
}

/* Write to the given filename an image in raw PGM format */
void write_pgm_file(char *filename, gray **image, int width, int height)
{
  FILE *outfile;

  if ((outfile = fopen(filename, "w")) == NULL) {
    sprintf(buf0, "write_pgm_file: can't open file %s", filename);
    fatalError(buf0);
  }
  pgm_writepgm(outfile, image, height, width, 255, 1);
  fclose(outfile);
}
  
/* pgm_writepgm()

NB: This is Kevin's PGM reader. You can tell because I would never call my
code cheap. Mine is ... inexpensive.

This is a cheap implementation of the pgm library's writepgm call.  For
documentation on parameters, see libpgm(3).  Note:  Setting the "max" value
to anything other than 255 can cause weird behaviour in programs that use the
output image.  
*/
void pgm_writepgm( FILE *fp, gray **image, int height, int width,
         int max, int rawbits)
    {
    /* Write the header */
    if ( fprintf( fp, "P5\n") < 0 )
	{
	fprintf(stderr,"Write Magic Number error.\n");
	exit( -1 );
	}

    if ( fprintf( fp, "%d %d\n", width, height ) < 0 )
	{
	fprintf(stderr,"Write image size error.\n");
	exit( -1 );
	}

    if ( fprintf( fp, "%d\n", max ) < 0 )
	{
	fprintf(stderr,"Write max gray level error.\n");
	exit( -1 );
	}

    /* Write the data */
    if (fwrite( image[0], sizeof(gray) * width, height, fp ) != height )
	{
	fprintf(stderr,"Write data error.\n");
	exit(-1);
	}
}

/*========= DEBUGGING OUTPUT =======*/ 

/* Print out the error message and exit the program */
void fatalError(char *errormessage)
{
  fprintf(stderr, "\n\nFatal error encountered :\n%s\n", errormessage);
  fprintf(stderr, "Note: this is generally bad.\n");
  exit(-1);
}

/* Print the text as debugging information */
void text_print(char *message)
{
  fprintf(stderr, "%s\n", message);
}

/* Print the value as debugging information */
void scalar_print(char *message, double d)
{
  fprintf(stderr, "%10s  %16.11f\n", message, d);
}

/* Print the vector as debugging information */
void vec_print(char *message, vector v)
{
  fprintf(stderr, "%10s (%16.11f %16.11f %16.11f %16.11f)\n", message,
	  v[X], v[Y], v[Z], v[W]);
}

/* Print the vector as debugging information */
void vec3_print(char *message, vector v)
{
  fprintf(stderr, "%10s (%16.11f %16.11f %16.11f)\n", message,
	  v[X], v[Y], v[Z]);
}

/* Print the matrix as debugging information */
void matrix_print(char *message, matrix m)
{
  fprintf(stderr, "%10s (%16.11f %16.11f %16.11f %16.11f)\n", message,
	  m[0][0], m[1][0], m[2][0], m[3][0]);
  fprintf(stderr, "           (%16.11f %16.11f %16.11f %16.11f)\n",
	  m[0][1], m[1][1], m[2][1], m[3][1]);
  fprintf(stderr, "           (%16.11f %16.11f %16.11f %16.11f)\n",
	  m[0][2], m[1][2], m[2][2], m[3][2]);
  fprintf(stderr, "           (%16.11f %16.11f %16.11f %16.11f)\n",
	  m[0][3], m[1][3], m[2][3], m[3][3]);
}

/*========= MATH FUNCTIONS =========*/ 

/* Gets the angle between two vectors, relative to rotation about an axis */
double vec_angle(vector v1, vector v2, int axis)
{
  int other1, other2;
  double result, magaccum;
  
  other1 = (axis + 1) % 3;
  other2 = (axis + 2) % 3;
  printf("axes %d, %d\n", other1, other2);
  result = v1[other1]*v2[other1] + v1[other2]*v2[other2];
  magaccum =  sqrt(v1[other1]*v1[other1] + v1[other2]*v1[other2])
            * sqrt(v2[other1]*v2[other1] + v2[other2]*v2[other2]);
  result /= check0(magaccum, "vec_angle: along axis!");
  printf("val %f\n", result);

  result = acos(result)*180.0/M_PI;
  printf("ang %f\n", result);
  return result;
}

/* These mathematics functions are all reasonably straightforward. */
double matrixDet_2d(matrix m)
{
  double sum;

  sum  = m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1]);
  sum -= m[1][0]*(m[0][1]*m[2][2] - m[0][2]*m[2][1]);
  sum += m[2][0]*(m[0][1]*m[1][2] - m[0][2]*m[1][1]);

  return sum;
}

void matrixMult_2d(matrix a, matrix b, matrix result)
{
  int i, j, k;
  double sum;
  matrix temp;

  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++) {
      sum = 0.0;
      for (k = 0; k < 3; k++)
        sum += a[i][k] * b[k][j];
      temp[i][j] = sum;
    }
  matrixCopy_2d(temp, result);
}

void matrixMult_3d(matrix a, matrix b, matrix result)
{
  int i, j, k;
  double sum;
  matrix temp;

  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++) {
      sum = 0.0;
      for (k = 0; k < 4; k++)
        sum += a[k][j] * b[i][k];
      temp[i][j] = sum;
    }
  matrixCopy_3d(temp, result);
}

void matrixCopy_2d(matrix src, matrix dest)
{
  double *sptr = (double *)src;
  double *dptr = (double *)dest;
  int i;

  for (i = 0; i < 9; i++)
    dptr[i] = sptr[i];
}

void matrixCopy_3d(matrix src, matrix dest)
{
  double *sptr = (double *)src;
  double *dptr = (double *)dest;
  int i;

  for (i = 0; i < 16; i++)
    dptr[i] = sptr[i];
}

void preApply_2d(vector v, matrix m, vector result)
{
  int i, j;
  vector temp;

  for (i = 0; i < 3; i++) {
    temp[i] = 0.0;
    for (j = 0; j < 3; j++)
      temp[i] += v[j]*m[i][j];
  }
  for (i = 0; i < 3; i++)
    result[i] = temp[i];
}

void postApply_2d(matrix m, vector v, vector result)
{
  int i, j;
  vector temp;

  for (j = 0; j < 3; j++) {
    temp[j] = 0.0;
    for (i = 0; i < 3; i++)
      temp[j] += v[i]*m[i][j];
  }
  for (i = 0; i < 3; i++)
    result[i] = temp[i];
}

void apply_3d(matrix m, vector v, vector result)
{
  int i;
  vector temp;

  temp[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0] + v[3]*m[3][0];
  temp[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1] + v[3]*m[3][1];
  temp[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2] + v[3]*m[3][2];
  temp[3] = v[0]*m[0][3] + v[1]*m[1][3] + v[2]*m[2][3] + v[3]*m[3][3];
  
  for (i = 0; i < 4; i++)
    result[i] = temp[i];
}

double check0(double checkthis, char *message)
{
  if (checkthis == 0) {
#if DEBUGINFO
    fprintf(stderr, "Zero divisor replaced : %s\n", message);
#else
    if (strcmp(message, "AFFINE_CALC: lambda") != 0)
      fprintf(stderr, "Zero divisor replaced : %s\n", message);
#endif
    return 1/10000000000.0;
  } else
    return checkthis;
}