home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: Science / Science.zip / gmt_os2.zip / src / gmt_vector.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-17  |  9.6 KB  |  361 lines
  1. /*--------------------------------------------------------------------
  2.  *    The GMT-system:    @(#)gmt_vector.c    2.5  2/17/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. /* jacobi.c
  9.  *
  10.  * Find eigenvalues & eigenvectors of a square symmetric matrix by Jacobi's
  11.  * method, which is a convergent series of Givens rotations.
  12.  * Modified from Numerical Recipes FORTRAN edition.
  13.  * Returns integer 0 if OK, -1 if failure to converge in MAX_SWEEPS.
  14.  *
  15.  * programmer:    W. H. F. Smith, 7 June, 1991-1995.
  16.  *
  17.  * Caveat Emptor!  Assumes underflows return zero without killing execution.
  18.  * I am not sure what happens if the eigenvalues are degenerate or not distinct.
  19.  */
  20.  
  21. #include "gmt.h"
  22.  
  23. #define MAX_SWEEPS 50
  24.  
  25. int    jacobi(a, n, m, d, v, b, z, nrots)
  26.  
  27. double    a[];    /* Sent.  n by n matrix in full storage mode.
  28.         On return, superdiagonal elements are destroyed.  */
  29. int    *n;    /* Sent.  row and column dimension of a as used.  */
  30. int    *m;    /* Sent.  row and column dimension of a and v as
  31.         allocated, so that a(i,j) is at a[i + (*m)*j].  */
  32. double    d[];    /* Returned.  vector of n eigenvalues of a.  */
  33. double    v[];    /* Returned.  n x n matrix of eigenvectors of a,
  34.         with row dimension m  */
  35. double    b[];    /* Work vector of n elements must be supplied.  */
  36. double    z[];    /* Another work vector of n elements must be supplied.  */
  37. int    *nrots;    /* Returned.  number of Givens rotations performed.  */
  38.  
  39. {
  40.     int    ip, iq, nsweeps, i, j, k;
  41.     double    sum, threshold, g, h, t, theta, c, s, tau, p;
  42.  
  43.  
  44.     /* Begin by initializing v, b, d, and z.  v = identity matrix,
  45.         b = d = diag(a), and z = 0:  */
  46.  
  47.     for (ip = 0; ip < (*n); ip++) {
  48.         for (iq = 0; iq < (*n); iq++) {
  49.             v[ip + (*m)*iq] = 0.0;
  50.         }
  51.         v[ip + (*m)*ip] = 1.0;
  52.         b[ip] = a[ip + (*m)*ip];
  53.         d[ip] = b[ip];
  54.         z[ip] = 0.0;
  55.     }
  56.  
  57.     /* End of initializations.  Set counters and begin:  */
  58.  
  59.     (*nrots) = 0;
  60.     nsweeps = 0;
  61.  
  62.     while (nsweeps < MAX_SWEEPS) {
  63.  
  64.         /* Convergence test:
  65.             Sum off-diagonal elements of upper triangle.
  66.             When sum == 0.0 (underflow !) we have converged.
  67.             In this case, break out of while loop.  */
  68.  
  69.         sum = 0.0;
  70.         for (ip = 0; ip < (*n)-1; ip++) {
  71.             for (iq = ip+1; iq < (*n); iq++) {
  72.                 sum += fabs(a[ip + (*m)*iq]);
  73.             }
  74.         }
  75.         if (sum == 0.0) break;
  76.  
  77.         /* Now we are not converged.
  78.             If (nsweeps < 3) do only the big elements;
  79.                 else do them all.  */
  80.         if (nsweeps < 3) {
  81.             threshold = 0.2 * sum / ( (*n) * (*n) );
  82.         }
  83.         else {
  84.             threshold = 0.0;
  85.         }
  86.  
  87.         /* Now sweep whole upper triangle doing Givens rotations:  */
  88.  
  89.         for (ip = 0; ip < (*n) - 1; ip++) {
  90.             for (iq = ip+1; iq < (*n); iq++) {
  91.  
  92.                 /* After four sweeps, if the off-diagonal
  93.                     element is "small", skip the rotation
  94.                     and just set it to zero.  "Small" is
  95.                     a relative test by addition:  */
  96.  
  97.                 g = 100.0 * fabs(a[ip + (*m)*iq]);
  98.  
  99.                 if ( (nsweeps > 3) && ( (fabs(d[ip])+g) ==  fabs(d[ip]) ) && ( (fabs(d[iq])+g) ==  fabs(d[iq]) ) ) {
  100.                     a[ip + (*m)*iq] = 0.0;
  101.                 }
  102.                 else if (fabs(a[ip + (*m)*iq]) > threshold) {
  103.  
  104.                     h = d[iq] - d[ip];
  105.  
  106.                     if ( (fabs(h)+g) ==  fabs(h) ) {
  107.                         /* I think this could divide by zero if a(i,j) = a(i,i) = a(j,j) = 0.0.
  108.                             Would this occur only in a degenerate matrix?  */
  109.                         t = a[ip + (*m)*iq] / h;
  110.                     }
  111.                     else {
  112.                         theta = 0.5 * h / a[ip + (*m)*iq];
  113.                         t = 1.0 / (fabs(theta) + sqrt(1.0 + theta*theta) );
  114.                         if (theta < 0.0) t = -t;
  115.                     }
  116.  
  117.                     c = 1.0 / sqrt(1.0 + t*t);
  118.                     s = t * c;
  119.                     tau = s / (1.0 + c);
  120.                     
  121.                     h = t * a[ip + (*m)*iq];
  122.                     z[ip] -= h;
  123.                     z[iq] += h;
  124.                     d[ip] -= h;
  125.                     d[iq] += h;
  126.                     a[ip + (*m)*iq] = 0.0;
  127.  
  128.                     for (j = 0; j < ip; j++) {
  129.                         g = a[j + (*m)*ip];
  130.                         h = a[j + (*m)*iq];
  131.                         a[j + (*m)*ip] = g - s * (h + g * tau);
  132.                         a[j + (*m)*iq] = h + s * (g - h * tau);
  133.                     }
  134.                     for (j = ip+1; j < iq; j++) {
  135.                         g = a[ip + (*m)*j];
  136.                         h = a[j + (*m)*iq];
  137.                         a[ip + (*m)*j] = g - s * (h + g * tau);
  138.                         a[j + (*m)*iq] = h + s * (g - h * tau);
  139.                     }
  140.                     for (j = iq+1; j < (*n); j++) {
  141.                         g = a[ip + (*m)*j];
  142.                         h = a[iq + (*m)*j];
  143.                         a[ip + (*m)*j] = g - s * (h + g * tau);
  144.                         a[iq + (*m)*j] = h + s * (g - h * tau);
  145.                     }
  146.  
  147.                     for (j = 0; j < (*n); j++) {
  148.                         g = v[j + (*m)*ip];
  149.                         h = v[j + (*m)*iq];
  150.                         v[j + (*m)*ip] = g - s * (h + g * tau);
  151.                         v[j + (*m)*iq] = h + s * (g - h * tau);
  152.                     }
  153.  
  154.                     (*nrots)++;
  155.                 }
  156.             }
  157.         }
  158.  
  159.         for (ip = 0; ip < (*n); ip++) {
  160.             b[ip] += z[ip];
  161.             d[ip] = b[ip];
  162.             z[ip] = 0.0;
  163.         }
  164.  
  165.         nsweeps++;
  166.     }
  167.  
  168.     /* Get here via break when converged, or when nsweeps == MAX_SWEEPS.
  169.         Sort eigenvalues by insertion:  */
  170.  
  171.     for (i = 0; i < (*n)-1; i++) {
  172.         k = i;
  173.         p = d[i];
  174.         for (j = i+1; j < (*n); j++) {  /* Find max location  */
  175.             if (d[j] >= p) {
  176.                 k = j;
  177.                 p = d[j];
  178.             }
  179.         }
  180.         if (k != i) {  /*  Need to swap value and vector  */
  181.             d[k] = d[i];
  182.             d[i] = p;
  183.             for (j = 0; j < (*n); j++) {
  184.                 p = v[j + (*m)*i];
  185.                 v[j + (*m)*i] = v[j + (*m)*k];
  186.                 v[j + (*m)*k] = p;
  187.             }
  188.         }
  189.     }
  190.  
  191.     /* Return 0 if converged; else print warning and return -1:  */
  192.  
  193.     if (nsweeps == MAX_SWEEPS) {
  194.         fprintf(stderr,"jacobi:  Failed to converge in %d sweeps\n", nsweeps);
  195.         return(-1);
  196.     }
  197.     return(0);
  198. }
  199.  
  200. /*  cartesian_stuff.c  --  bits and pieces for doing spherical trig
  201.  *  in terms of dot and cross products.
  202.  *
  203.  * w. h. f. smith, 16 June. 1989
  204.  *
  205.  */
  206.  
  207. double    dot3v(a,b)
  208. double    *a, *b;
  209. {
  210.     return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
  211. }
  212.  
  213. double    mag3v(a)
  214. double    *a;
  215. {
  216.     return(d_sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]));
  217. }
  218.  
  219. void    normalize3v(a)
  220. double    *a;
  221. {
  222.     double r_length;
  223.     r_length = (mag3v(a));
  224.     if (r_length != 0.0) {
  225.         r_length = 1.0 / r_length;
  226.         a[0] *= r_length;
  227.         a[1] *= r_length;
  228.         a[2] *= r_length;
  229.     }
  230. }
  231.  
  232. void    cross3v(a,b,c)
  233. double    *a, *b, *c;
  234. {
  235.     c[0] = a[1] * b[2] - a[2] * b[1];
  236.     c[1] = a[2] * b[0] - a[0] * b[2];
  237.     c[2] = a[0] * b[1] - a[1] * b[0];
  238. }
  239.  
  240. void    geo_to_cart(alat, alon, a, rads)
  241. double    *alat, *alon, *a;
  242. int    rads;
  243. {
  244.     /* Convert geographic latitude and longitude (alat, alon)
  245.        to a 3-vector of unit length (a).  rads = TRUE if we
  246.        need to convert alat, alon from degrees to radians  */
  247.  
  248.     double cos_lat;
  249.  
  250.     if (rads) {
  251.         *alat *= D2R;
  252.         *alon *= D2R;
  253.     }
  254.  
  255.     cos_lat = cos(*alat);
  256.     a[0] = cos_lat * cos(*alon);
  257.     a[1] = cos_lat * sin(*alon);
  258.     a[2] = sin(*alat);
  259. }
  260.  
  261. void    cart_to_geo(alat, alon, a, rads)
  262. double    *alat, *alon, *a;
  263. int    rads;
  264. {
  265.     /* Convert a 3-vector (a) of unit length into geographic
  266.        coordinates (alat, alon).  rads = TRUE if we want the
  267.        lat and lon converted from radians into degrees.  */
  268.  
  269.     if(rads) {
  270.         *alat = R2D * d_asin(a[2]);
  271.         *alon = R2D * d_atan2(a[1], a[0]);
  272.     }
  273.     else {
  274.         *alat = d_asin(a[2]);
  275.         *alon = d_atan2(a[1],a[0]);
  276.     }
  277. }
  278.  
  279. int fix_up_path (a_lon, a_lat, n, greenwich, step)
  280. double *a_lon[], *a_lat[];
  281. int n;
  282. BOOLEAN greenwich;    /* TRUE if we cross Greenwich */
  283. double step;        /* Add points when step degrees are exceeded */
  284. {
  285.     
  286.     /* Takes pointers to a list of lon/lat pairs and adds auxiliary points if
  287.      * the great circle distance between two given points exceeds
  288.      * <step> spherical degree [ 1 degree].
  289.      */
  290.      
  291.     int i, j, n_tmp, n_insert = 0, n_alloc;
  292.     double *lon_tmp, *lat_tmp, *old;
  293.     double a[3], b[3], x[3], *lon, *lat;
  294.     double c, d, fraction, theta, i_step;
  295.     
  296.     lon = *a_lon;
  297.     lat = *a_lat;
  298.     
  299.     n_alloc = n;
  300.     lon_tmp = (double *) memory (CNULL, n_alloc, sizeof (double), "fix_up_path");
  301.     lat_tmp = (double *) memory (CNULL, n_alloc, sizeof (double), "fix_up_path");
  302.     
  303.     geo_to_cart (&lat[0], &lon[0], a, TRUE);
  304.     lon_tmp[0] = (lon[0] >= M_PI) ? lon[0] - 2.0*M_PI : lon[0];    lat_tmp[0] = lat[0];
  305.     n_tmp = 1;
  306.     if (step <= 0.0) step = 1.0;
  307.     i_step = 1.0 / step;
  308.     
  309.     for (i = 1; i < n; i++) {
  310.         
  311.         geo_to_cart (&lat[i],&lon[i], b, TRUE);
  312.         
  313.         if ((theta = d_acos (dot3v (a, b))) == M_PI) {    /* trouble, no unique great circle */
  314.             fprintf (stderr, "GMT Warning: Two points in input list are antipodal!\n");
  315.         }
  316.         else if ((n_insert = floor (theta * R2D * i_step))) {    /* Must insert n_insert points */
  317.             fraction = step * D2R / theta;
  318.             for (j = 1; j <= n_insert; j++) {
  319.                 c = j * fraction;
  320.                 d = 1 - c;
  321.                 x[0] = a[0] * d + b[0] * c;
  322.                 x[1] = a[1] * d + b[1] * c;
  323.                 x[2] = a[2] * d + b[2] * c;
  324.                 normalize3v (x);
  325.                 cart_to_geo (&lat_tmp[n_tmp], &lon_tmp[n_tmp], x, FALSE);
  326.                 n_tmp++;
  327.                 if (n_tmp == n_alloc) {
  328.                     n_alloc += GMT_CHUNK;
  329.                     lon_tmp = (double *) memory ((char *) lon_tmp, n_alloc, sizeof (double), "fix_up_path");
  330.                     lat_tmp = (double *) memory ((char *) lat_tmp, n_alloc, sizeof (double), "fix_up_path");
  331.                 }
  332.             }
  333.         }
  334.         lon_tmp[n_tmp] = (lon[i] >= M_PI) ? lon[i] - 2.0 * M_PI : lon[i];    lat_tmp[n_tmp] = lat[i];
  335.         n_tmp++;
  336.         if (n_tmp == n_alloc) {
  337.             n_alloc += GMT_CHUNK;
  338.             lon_tmp = (double *) memory ((char *) lon_tmp, n_alloc, sizeof (double), "fix_up_path");
  339.             lat_tmp = (double *) memory ((char *) lat_tmp, n_alloc, sizeof (double), "fix_up_path");
  340.         }
  341.         a[0] = b[0];    a[1] = b[1];    a[2] = b[2];
  342.     }
  343.     lon_tmp = (double *) memory ((char *) lon_tmp, n_tmp, sizeof (double), "fix_up_path");
  344.     lat_tmp = (double *) memory ((char *) lat_tmp, n_tmp, sizeof (double), "fix_up_path");
  345.     
  346.     old = lon;
  347.     lon = lon_tmp;
  348.     free ((char *) old);
  349.     old = lat;
  350.     lat = lat_tmp;
  351.     free ((char *) old);
  352.     for (i = 0; i < n_tmp; i++) {
  353.         lon[i] *= R2D;
  354.         if (!greenwich && lon[i] < 0.0) lon[i] += 360.0;
  355.         lat[i] *= R2D;
  356.     }
  357.     *a_lon = lon;
  358.     *a_lat = lat;
  359.     return (n_tmp);
  360. }        
  361.