home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: Science / Science.zip / gmt_os2.zip / src / fitcircle.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-03-13  |  16.1 KB  |  454 lines
  1. /*--------------------------------------------------------------------
  2.  *    The GMT-system:    @(#)fitcircle.c    2.13  3/13/95
  3.  *
  4.  *    Copyright (c) 1991-1995 by P. Wessel and W. H. F. Smith
  5.  *    See README file for copying and redistribution conditions.
  6.  *--------------------------------------------------------------------*/
  7. /*
  8.  * fitcircle <lonlatfile> [-L1] [-L2] [-S]
  9.  *
  10.  * Read lon,lat pairs from stdin[file].  Find mean position and pole
  11.  * of best-fit circle through these points.  By default, fit great
  12.  * circle.  If -S, fit small circle.  In this case, fit great circle
  13.  * first, and then search for minimum small circle by bisection.
  14.  *
  15.  * Formally, we want to minimize some norm on the distance between
  16.  * each point and the circle, measured perpendicular to the circle.
  17.  * For both L1 and L2 norms this is a rather intractable problem.
  18.  * (L2 is non-linear, and in L1 it is not clear how to proceed).
  19.  * However, some approximations exist which work well and are simple
  20.  * to compute.  We create a list of x,y,z vectors on the unit sphere,
  21.  * representing the original data.  To find a great circle, do this:
  22.  * For L1:
  23.  *     Find the Fisher mean of these data, call it mean position.
  24.  *    Find the (Fisher) mean of all cross-products between data and
  25.  *        the mean position; call this the pole to the great circle.
  26.  *    Note that the cross-products are proportional to the distance
  27.  *        between datum and mean; hence above average gives data far
  28.  *        from mean larger weight in determining pole.  This is
  29.  *        analogous to fitting line in plane, where data far from
  30.  *        average abcissa have large leverage in determining slope.
  31.  * For L2:
  32.  *    Create 3 x 3 matrix of sums of products of data vector elements.
  33.  *    Find eigenvectors and eigenvalues of this matrix.
  34.  *    Find mean as eigenvector corresponding to max eigenvalue.
  35.  *    Find pole as eigenvector corresponding to min eigenvalue.
  36.  *    Eigenvalue-eigenvector decomposition performed by Jacobi's iterative
  37.  *        method of successive Givens rotations.  Trials suggest that
  38.  *        this converges extremely rapidly (3 sweeps, 9 rotations).
  39.  *
  40.  * To find a small circle, first find the great circle pole and the mean
  41.  * position.  Suppose the small circle pole to lie in the plane containing
  42.  * the mean and great circle pole, and narrow down its location by bisection.
  43.  *
  44.  * Author:    W. H. F. Smith
  45.  * Date:    16 September 1991-1995.
  46.  * Version:    2.0, for GMT release.
  47.  *
  48.  */
  49.  
  50. #include "gmt.h"
  51.  
  52. struct    DATA {
  53.     double    x[3];
  54. }    *data;
  55.  
  56. main (argc,argv)
  57. int    argc;
  58. char    **argv;
  59. {
  60.     int    i, j, k, ix, iy, imin, imax, n_alloc, n_data, n, np, nrots, norm = -1;
  61.     BOOLEAN    error = FALSE, greenwich = FALSE, find_small_circle = FALSE;
  62.  
  63.     double    lonsum, latsum, latlon[2];
  64.     double    meanv[3], cross[3], cross_sum[3], gcpole[3], scpole[3];        /* Extra vectors  */
  65.     double    *a, *lambda, *v, *b, *z;    /* Matrix stuff */
  66.     double    get_small_circle(), rad, *work;
  67.  
  68.     char    buffer[512], format[80];
  69.  
  70.     FILE    *fp = NULL;
  71.  
  72.     argc = gmt_begin (argc, argv);
  73.  
  74.         for (i = 1; i < argc; i++) {
  75.                 if (argv[i][0] == '-') {
  76.                         switch (argv[i][1]) {
  77.             
  78.         
  79.                 /* Common parameters */
  80.             
  81.                 case 'H':
  82.                 case 'V':
  83.                 case ':':
  84.                 case '\0':
  85.                     error += get_common_args (argv[i], 0, 0, 0, 0);
  86.                     break;
  87.                 
  88.                 /* Supplemental parameters */
  89.                 
  90.                                 case 'L':
  91.                     norm = 3;
  92.                                         if (argv[i][2]) norm = atoi(&argv[i][2]);
  93.                                         break;
  94.                                 case 'S':
  95.                     find_small_circle = TRUE;
  96.                                         break;
  97.                                 default:
  98.                                         error = TRUE;
  99.                                         gmt_default_error (argv[i][1]);
  100.                                         break;
  101.                         }
  102.                 }
  103.                 else {
  104.                         if ((fp = fopen(argv[i], "r")) == NULL) {
  105.                                 fprintf (stderr, "fitcircle: Could not open file %s\n", argv[i]);
  106.                                 error = TRUE;
  107.                         }
  108.                 }
  109.         }
  110.  
  111.     if (argc == 1 || gmt_quick) {
  112.         fprintf (stderr, "fitcircle %s - find best-fitting great circle to points on sphere\n\n", GMT_VERSION);
  113.         fprintf(stderr,"usage:  fitcircle [<input_file>] -L[<n>] [-H] [-S] [-V] [-:]\n\n");
  114.         
  115.         if (gmt_quick) exit (-1);
  116.  
  117.         fprintf(stderr,"\tReads from input_file or standard input\n");
  118.         fprintf(stderr,"\t-L specify norm as -L1 or -L2; or use -L or -L3 to give both.\n");
  119.         fprintf (stderr, "\n\tOPTIONS:\n");
  120.         explain_option ('H');
  121.         fprintf(stderr,"\t-S will attempt to fit a small circle rather than a great circle.\n");
  122.         explain_option ('V');
  123.         explain_option (':');
  124.         exit(-1);
  125.     }
  126.  
  127.      if (norm < 1 || norm > 3) {
  128.         fprintf (stderr, "%s: GMT SYNTAX ERROR -L option:  Choose between 1, 2, or 3\n", gmt_program);
  129.         error++;
  130.     }
  131.     if (error) exit (-1);
  132.  
  133.     if (fp == NULL) fp = stdin;
  134.     n_alloc = GMT_CHUNK;
  135.     n_data = 0;
  136.     lonsum = latsum = 0.0;
  137.     sprintf (format, "%s\t%s\n\0", gmtdefs.d_format, gmtdefs.d_format);
  138.     
  139.     data = (struct DATA *) memory (CNULL, n_alloc, sizeof(struct DATA), "fitcircle");
  140.  
  141.     if (gmtdefs.io_header) for (i = 0; i < gmtdefs.n_header_recs; i++) fgets (buffer, 512, fp);
  142.         
  143.     ix = (gmtdefs.xy_toggle) ? 1 : 0;    iy = 1 - ix;        /* Set up which columns have x and y */
  144.  
  145.     while ( (fgets (buffer, 512, fp) ) != NULL) {
  146.  
  147.         if ((sscanf(buffer, "%lf %lf", &latlon[ix], &latlon[iy])) != 2) {
  148.             fprintf(stderr,"fitcircle: Cannot read line %d\n", n_data+1);
  149.             continue;
  150.         }
  151.                 lonsum += latlon[0];
  152.                 latsum += latlon[1];
  153.                 geo_to_cart (&latlon[1], &latlon[0], data[n_data].x, TRUE);
  154.                 n_data++;
  155.  
  156.                 if (n_data == n_alloc) {
  157.                         n_alloc += GMT_CHUNK;
  158.                   data = (struct DATA *) memory ((char *)data, n_alloc, sizeof(struct DATA), "fitcircle");
  159.                 }
  160.         }
  161.         if (fp != stdin) fclose(fp);
  162.           data = (struct DATA *) memory ((char *)data, n_data, sizeof(struct DATA), "fitcircle");
  163.     if (find_small_circle && (norm%2) ) work = (double *) memory(CNULL, n_data, sizeof(double), "fitcircle");
  164.  
  165.     lonsum /= n_data;
  166.     latsum /= n_data;
  167.     if (gmtdefs.verbose) {
  168.         fprintf (stderr, "fitcircle: %d points read, Average Position (Flat Earth): ", n_data);
  169.         fprintf (stderr, format, lonsum, latsum);
  170.     }
  171.     if (lonsum > 180.0) greenwich = TRUE;
  172.  
  173.     /* Get Fisher mean in any case, in order to set L2 mean correctly, if needed.  */
  174.  
  175.  
  176.     meanv[0] = meanv[1] = meanv[2] = 0.0;
  177.  
  178.     for (i = 0; i < n_data; i++) for (j = 0; j < 3; j++) meanv[j] += data[i].x[j];
  179.  
  180.     normalize3v(meanv);
  181.  
  182.     if (norm%2) {
  183.         cart_to_geo (&latsum, &lonsum, meanv, TRUE);
  184.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  185.         printf("L1 Average Position (Fisher's Method):               ");
  186.         printf (format, lonsum, latsum);
  187.  
  188.         cross_sum[0] = cross_sum[1] = cross_sum[2] = 0.0;
  189.         for (i = 0; i < n_data; i++) {
  190.             cross3v(&data[i].x[0], meanv, cross);
  191.             if (cross[2] < 0.0) {
  192.                 cross_sum[0] -= cross[0];
  193.                 cross_sum[1] -= cross[1];
  194.                 cross_sum[2] -= cross[2];
  195.             }
  196.             else {
  197.                 cross_sum[0] += cross[0];
  198.                 cross_sum[1] += cross[1];
  199.                 cross_sum[2] += cross[2];
  200.             }
  201.         }
  202.         normalize3v(cross_sum);
  203.         if (find_small_circle) for (i = 0; i < 3; i++) gcpole[i] = cross_sum[i];
  204.         
  205.         cart_to_geo (&latsum, &lonsum, cross_sum, TRUE);
  206.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  207.         printf("L1 N Hemisphere Great Circle Pole (Cross-Averaged):  ");
  208.         printf (format, lonsum, latsum);
  209.         latsum = -latsum;
  210.         lonsum = d_atan2(-cross_sum[1], -cross_sum[0]) * R2D;
  211.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  212.         printf("L1 S Hemisphere Great Circle Pole (Cross-Averaged):  ");
  213.         printf (format, lonsum, latsum);
  214.         if (find_small_circle) {
  215.             if (gmtdefs.verbose) fprintf(stderr,"Fitting small circle using L1 norm.\n");
  216.             rad = get_small_circle(data, n_data, meanv, gcpole, scpole, 1, work);
  217.             if (rad >= 0.0) {
  218.                 cart_to_geo (&latsum, &lonsum, scpole, TRUE);
  219.                 if (greenwich && lonsum < 0.0) lonsum += 360.0;
  220.                 printf("L1 Small Circle Pole:                                ");
  221.                 printf (format, lonsum, latsum);
  222.                 printf("Distance from Pole to L1 Small Circle (degrees):     %.8lg\n", rad);
  223.             }
  224.         }
  225.     }
  226.  
  227.     if (norm/2) {
  228.  
  229.         n = 3;
  230.         np = n;
  231.  
  232.         a = (double *) memory (CNULL, np*np, sizeof(double), "fitcircle\n");
  233.         lambda = (double *) memory (CNULL, np, sizeof(double), "fitcircle\n");
  234.         b = (double *) memory (CNULL, np, sizeof(double), "fitcircle\n");
  235.         z = (double *) memory (CNULL, np, sizeof(double), "fitcircle\n");
  236.         v = (double *) memory (CNULL, np*np, sizeof(double), "fitcircle\n");
  237.  
  238.         for (i = 0; i < n_data; i++) for (j = 0; j < n; j++) for (k = 0; k < n; k++)
  239.             a[j + k*np] += (data[i].x[j]*data[i].x[k]);
  240.  
  241.         if (jacobi(a, &n, &np, lambda, v, b, z, &nrots)) {
  242.             fprintf(stderr,"fitcircle: Eigenvalue routine failed to converge in 50 sweeps.\n");
  243.             fprintf(stderr,"fitcircle: The reported L2 positions might be garbage.\n");
  244.         }
  245.         if (gmtdefs.verbose) fprintf(stderr,"fitcircle: Eigenvalue routine converged in %d rotations.\n", nrots);
  246.         imax = 0;
  247.         imin = 2;
  248.         if (d_acos (dot3v (v, meanv)) > M_PI_2)
  249.             for (i = 0; i < 3; i++) meanv[i] = -v[imax*np+i];
  250.         else
  251.             for (i = 0; i < 3; i++) meanv[i] = v[imax*np+i];
  252.         cart_to_geo (&latsum, &lonsum, meanv, TRUE);
  253.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  254.         printf("L2 Average Position (Eigenval Method):               ");
  255.         printf (format, lonsum, latsum);
  256.         
  257.         if (v[imin*np+2] < 0.0)    /* Eigvec is in S Hemisphere  */
  258.             for (i = 0; i < 3; i++) gcpole[i] = -v[imin*np+i];
  259.         else
  260.             for (i = 0; i < 3; i++) gcpole[i] = v[imin*np+i];
  261.         
  262.         cart_to_geo (&latsum, &lonsum, gcpole, TRUE);        
  263.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  264.         printf("L2 N Hemisphere Great Circle Pole (Eigenval Method): ");
  265.         printf (format, lonsum, latsum);
  266.         latsum = -latsum;
  267.         lonsum = d_atan2(-gcpole[1], -gcpole[0]) * R2D;
  268.         if (greenwich && lonsum < 0.0) lonsum += 360.0;
  269.         printf("L2 S Hemisphere Great Circle Pole (Eigenval Method): ");
  270.         printf (format, lonsum, latsum);
  271.  
  272.         free ( (char *)v);
  273.         free ( (char *)z);
  274.         free ( (char *)b);
  275.         free ( (char *)lambda);
  276.         free ( (char *)a);
  277.         if (find_small_circle) {
  278.             if (gmtdefs.verbose) fprintf(stderr,"fitcircle: Fitting small circle using L2 norm.\n");
  279.             rad = get_small_circle(data, n_data, meanv, gcpole, scpole, 2, work);
  280.             if (rad >= 0.0) {
  281.                 /* True when small circle fits better than great circle */
  282.                 cart_to_geo (&latsum, &lonsum, scpole, TRUE);
  283.                 if (greenwich && lonsum < 0.0) lonsum += 360.0;
  284.                 printf("L2 Small Circle Pole:                                ");
  285.                 printf (format, lonsum, latsum);
  286.                 printf("Distance from Pole to L2 Small Circle (degrees):     %.8lg\n", rad);
  287.             }
  288.         }
  289.     }
  290.     if (find_small_circle && (norm%2) ) free ((char*)work);
  291.     free ( (char *)data);
  292.     gmt_end (argc, argv);
  293. }
  294.  
  295. double    get_small_circle (data, ndata, center, gcpole, scpole, norm, work)
  296. struct DATA *data;
  297. int    ndata;
  298. double    *center, *gcpole, *scpole, *work;
  299. int    norm;
  300. {
  301.  
  302.     /* Find scpole, the pole to the best-fit small circle, 
  303.         by L(norm) iterative search along arc between
  304.         center and +/- gcpole, the pole to the best fit
  305.         great circle.  */
  306.  
  307.     int    i, j;
  308.     double    temppole[3], a[3], b[3], oldpole[3];
  309.     double    trypos, tryneg, afit, bfit, afactor, bfactor, fit, oldfit;
  310.     double    length_ab, length_aold, length_bold, circle_misfit(), circle_distance;
  311.  
  312.     /* First find out if solution is between center and gcpole,
  313.         or center and -gcpole:  */
  314.  
  315.     for (i = 0; i < 3; i++) temppole[i] = (center[i] + gcpole[i]);
  316.     normalize3v(temppole);
  317.     trypos = circle_misfit(data, ndata, temppole, norm, work, &circle_distance);
  318.  
  319.     for (i = 0; i < 3; i++) temppole[i] = (center[i] - gcpole[i]);
  320.     normalize3v(temppole);
  321.     tryneg = circle_misfit(data, ndata, temppole, norm, work, &circle_distance);
  322.  
  323.     if (tryneg < trypos) {
  324.         for (i = 0; i < 3; i++) a[i] = center[i];
  325.         for (i = 0; i < 3; i++) b[i] = -gcpole[i];
  326.     }
  327.     else {
  328.         for (i = 0; i < 3; i++) a[i] = center[i];
  329.         for (i = 0; i < 3; i++) b[i] = gcpole[i];
  330.     }
  331.  
  332.     /* Now a is at center and b is at pole on correct side.
  333.         Try to bracket a minimum.  Move from b toward
  334.     a in 1 degree steps:  */
  335.  
  336.     afit = circle_misfit(data, ndata, a, norm, work, &circle_distance);
  337.     bfit = circle_misfit(data, ndata, b, norm, work, &circle_distance);
  338.     j = 1;
  339.     do {
  340.         afactor = sin(j * D2R);
  341.         bfactor = cos(j * D2R);
  342.         for (i = 0; i < 3; i++) temppole[i] = (afactor * a[i] + bfactor * b[i]);
  343.         normalize3v(temppole);
  344.         fit = circle_misfit(data, ndata, temppole, norm, work, &circle_distance);
  345.         j++;
  346.     } while (j < 90 && fit > bfit && fit > afit);
  347.  
  348.     if (j == 90) {
  349.         /* Bad news.  There isn't a better fitting pole anywhere.  */
  350.         fprintf(stderr,"fitcircle: Sorry.  Cannot find small circle fitting better than great circle.\n");
  351.         for (i = 0; i < 3; i++) scpole[i] = gcpole[i];
  352.         return(-1.0);
  353.     }
  354.     /* Get here when temppole points to a minimum bracketed by a and b.  */
  355.  
  356.     for (i = 0; i < 3; i++) oldpole[i] = temppole[i];
  357.     oldfit = fit;
  358.  
  359.     /* Now, while not converged, take golden section of wider interval.  */
  360.     length_ab = d_acos (dot3v (a, b));
  361.     length_aold = d_acos (dot3v (a, oldpole));
  362.     length_bold = d_acos (dot3v (b, oldpole));
  363.     do {
  364.         if (length_aold > length_bold) {
  365.             /* Section a_old  */
  366.             for (i = 0; i < 3; i++) temppole[i] = (0.38197*a[i] + 0.61803*oldpole[i]);
  367.             normalize3v(temppole);
  368.             fit = circle_misfit(data, ndata, temppole, norm, work, &circle_distance);
  369.             if (fit < oldfit) {
  370.                 /* Improvement.  b = oldpole, oldpole = temppole  */
  371.                 for (i = 0; i < 3; i++) {
  372.                     b[i] = oldpole[i];
  373.                     oldpole[i] = temppole[i];
  374.                 }
  375.                 oldfit = fit;
  376.             }
  377.             else {
  378.                 /* Not improved.  a = temppole  */
  379.                 for (i = 0; i < 3; i++) a[i] = temppole[i];
  380.             }
  381.         }
  382.         else {
  383.             /* Section b_old  */
  384.             for (i = 0; i < 3; i++) temppole[i] = (0.38197*b[i] + 0.61803*oldpole[i]);
  385.             normalize3v(temppole);
  386.             fit = circle_misfit(data, ndata, temppole, norm, work, &circle_distance);
  387.             if (fit < oldfit) {
  388.                 /* Improvement.  a = oldpole, oldpole = temppole  */
  389.                 for (i = 0; i < 3; i++) {
  390.                     a[i] = oldpole[i];
  391.                     oldpole[i] = temppole[i];
  392.                 }
  393.                 oldfit = fit;
  394.             }
  395.             else {
  396.                 /* Not improved.  b = temppole  */
  397.                 for (i = 0; i < 3; i++) b[i] = temppole[i];
  398.             }
  399.         }
  400.         length_ab = d_acos (dot3v (a, b));
  401.         length_aold = d_acos (dot3v (a, oldpole));
  402.         length_bold = d_acos (dot3v (b, oldpole));
  403.     } while (length_ab > 0.0001);    /* 1 milliradian = 0.05 degree  */
  404.  
  405.     for (i = 0; i < 3; i++) scpole[i] = oldpole[i];
  406.     return(R2D * circle_distance);
  407. }
  408.  
  409. double    circle_misfit(data, ndata, pole, norm, work, circle_distance)
  410. struct DATA *data;
  411. int    ndata;
  412. double    *pole, *work, *circle_distance;
  413. int    norm;
  414. {
  415.     /* Find the L(norm) misfit between a small circle through
  416.         center with pole pole.  Return misfit in radians.  */
  417.  
  418.     double    distance, delta_distance, misfit = 0.0;
  419.     int    i;
  420.  
  421.     /* At first, I thought we could use the center to define
  422.         circle_dist = distance between pole and center.
  423.         Then sum over data {dist[i] - circle_dist}.
  424.         But it turns out that if the data are tightly
  425.         curved, so that they are on a small circle 
  426.         within a few degrees of the pole, then the
  427.         center point is not on the small circle, and
  428.         we cannot use it.  So, we first have to fit
  429.         the circle_dist correctly:  */
  430.  
  431.     if (norm == 1) {
  432.         for (i = 0; i < ndata; i++) work[i] = d_acos (dot3v (&data[i].x[0], pole));
  433.         qsort((char *)work, ndata, sizeof(double), comp_double_asc);
  434.         if (ndata%2)
  435.             *circle_distance = work[ndata/2];
  436.         else
  437.             *circle_distance = 0.5 * (work[(ndata/2)-1] + work[ndata/2]);
  438.     }
  439.     else {
  440.         *circle_distance = 0.0;
  441.         for (i = 0; i < ndata; i++) *circle_distance += d_acos (dot3v (&data[i].x[0], pole));
  442.         *circle_distance /= ndata;
  443.     }
  444.  
  445.     /* Now do each data point:  */
  446.  
  447.     for (i = 0; i < ndata; i++) {
  448.         distance = d_acos (dot3v (&data[i].x[0], pole));
  449.         delta_distance = fabs(*circle_distance - distance);
  450.         misfit += ((norm == 1) ? delta_distance : delta_distance * delta_distance);
  451.     }
  452.     return (norm == 1) ? misfit : sqrt (misfit);
  453. }
  454.