geodetic.cc

Go to the documentation of this file.

/* Copyright (C) 2012
   Patrick Eriksson <Patrick.Eriksson@chalmers.se>

   This program is free software; you can redistribute it and/or modify it
   under the terms of the GNU General Public License as published by the
   Free Software Foundation; either version 2, or (at your option) any
   later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
   USA. */




/*===========================================================================
  === File description 
  ===========================================================================*/

/*===========================================================================
  === External declarations
  ===========================================================================*/

#include <cmath>
#include <stdexcept>
#include "geodetic.h"
#include "math_funcs.h"
#include "ppath.h"

extern const Numeric DEG2RAD;
extern const Numeric RAD2DEG;



/*===========================================================================
  === 2D functions
  ===========================================================================*/

// The 2D case is treated as being the 3D x/z-plane. That is, y-coordinate is
// skipped. For simplicity, the angle coordinate is denoted as latitude.
// However, the latitude is here not limited to [-90,90]. It is cyclic and can
// have any value. The input *lat0* is used to shift the output from atan2 with
// n*360 to return what should be the expected latitude. That is, it is assumed
// that no operation moves the latitude more than 180 degrees from the initial
// value *lat0*. Negative zeniath angles are handled, following ARTS definition
// of 2D geometry.



void cart2pol(
            Numeric&   r,
            Numeric&   lat,
      const Numeric&   x,
      const Numeric&   z,
      const Numeric&   lat0,
      const Numeric&   za0 )
{
  r   = sqrt( x*x + z*z );

  // Zenith and nadir cases
  const Numeric absza = abs( za0 );
  if( absza < ANGTOL  ||  absza > 180-ANGTOL  )
    { lat = lat0; }

  else
    { // Latitude inside [0,360]
      lat = RAD2DEG * atan2( z, x );
      // Shift with n*360 to get as close as possible to lat0
      lat = lat - 360.0 * Numeric( round( ( lat - lat0 ) / 360.0 ) );
    }
}




void cart2poslos(
             Numeric&   r,
             Numeric&   lat,
             Numeric&   za,
       const Numeric&   x,
       const Numeric&   z,
       const Numeric&   dx,
       const Numeric&   dz,
       const Numeric&   ppc,
       const Numeric&   lat0,
       const Numeric&   za0 )
{
  r   = sqrt( x*x + z*z );

  // Zenith and nadir cases
  const Numeric absza = abs( za0 );
  if( absza < ANGTOL  ||  absza > 180-ANGTOL  )
    { 
      lat = lat0;
      za  = za0; 
    }

  else
    {
      lat = RAD2DEG * atan2( z, x );

      const Numeric   latrad = DEG2RAD * lat;
      const Numeric   coslat = cos( latrad );
      const Numeric   sinlat = sin( latrad );
      const Numeric   dr     = coslat*dx + sinlat*dz;

      // Use ppc for max accuracy, but dr required to resolve if up- 
      // and downward cases.

      // Another possibility to obtain (absolute value of) za is 
      // RAD2DEG*acos(dr).
      // It is checked that the two ways give consistent results, but
      // occasionally deviate with 1e-4 deg (due to numerical issues).

      za = RAD2DEG * asin( ppc / r );
      if( za0 > 0 )
        {
          if( isnan( za ) )
            { za = 90; }
          else if( dr < 0 )
            { za = 180.0 - za; }
        }
      else
        {
          if( isnan( za ) )
            { za = -90; }
          else if( dr < 0 )
            { za = -180.0 + za; }
          else 
            { za = -za; }
        }
    }
}




void distance2D(
            Numeric&   l,
      const Numeric&   r1,
      const Numeric&   lat1,
      const Numeric&   r2,
      const Numeric&   lat2 )
{
  assert( abs( lat2 - lat1 ) <= 180 );

  Numeric x1, z1, x2, z2;
  pol2cart( x1, z1, r1, lat1 );
  pol2cart( x2, z2, r2, lat2 );

  const Numeric dx = x2 - x1; 
  const Numeric dz = z2 - z1; 
  l = sqrt( dx*dx + dz*dz ); 
}




/*
void geomtanpoint2d( 
             Numeric&    r_tan,
             Numeric&    lat_tan,
     ConstVectorView    refellipsoid,
       const Numeric&    r,
       const Numeric&    lat,
       const Numeric&    za )
{
  assert( refellipsoid.nelem() == 2 );
  assert( refellipsoid[0] > 0 );
  assert( refellipsoid[1] >= 0 );
  assert( refellipsoid[1] < 1 );
  assert( r > 0 );
  assert( za >= 90 );
                                // e=1e-7 corresponds to that polar radius
  if( refellipsoid[1] < 1e-7 )  // less than 1 um smaller than equatorial 
    {                           // one for the Earth
      r_tan = geometrical_ppc( r, za );
      if( za > 0 )
        { lat_tan = geompath_lat_at_za( za, lat, 90 ); }
      else
        { lat_tan = geompath_lat_at_za( za, lat, -90 ); }
    }

  else
    {
      assert( 0 );  // To be implemented
    }  
}  
*/



void line_circle_intersect(
         Numeric&   x,
         Numeric&   z,
   const Numeric&   xl,
   const Numeric&   zl,
   const Numeric&   dx,
   const Numeric&   dz,
   const Numeric&   xc,
   const Numeric&   zc,
   const Numeric&   r )
{
  const Numeric a = dx*dx + dz*dz;
  const Numeric b = 2*( dx*(xl-xc) + dz*(zl-zc) );
  const Numeric c = xc*xc + zc*zc + xl*xl + zl*zl - 2*(xc*xl + zc*zl) - r*r;
  
  Numeric d = b*b - 4*a*c;
  assert( d > 0 );
  
  const Numeric a2 = 2*a;
  const Numeric b2 = -b / a2;
  const Numeric e  = sqrt( d ) / a2;

  const Numeric l1 = b2 + e;
  const Numeric l2 = b2 - e;

  Numeric l;
  if( l1 < 0 )
    { l = l2; }
  else  if( l2 < 0 )
    { l = l1; }
  else
    { l = min(l1,l2); assert( l>=0 ); }

  x = xl + l*dx;
  z = zl + l*dz;
}




void pol2cart(
            Numeric&   x,
            Numeric&   z,
      const Numeric&   r,
      const Numeric&   lat )
{
  assert( r > 0 );

  const Numeric   latrad = DEG2RAD * lat;

  x = r * cos( latrad );  
  z = r * sin( latrad );
}




void poslos2cart(
             Numeric&   x,
             Numeric&   z,
             Numeric&   dx,
             Numeric&   dz,
       const Numeric&   r,
       const Numeric&   lat,
       const Numeric&   za )
{
  assert( r > 0 );
  assert( za >= -180 && za<=180 );

  const Numeric   latrad = DEG2RAD * lat;
  const Numeric   zarad  = DEG2RAD * za;

  const Numeric   coslat = cos( latrad );
  const Numeric   sinlat = sin( latrad );
  const Numeric   cosza  = cos( zarad );
  const Numeric   sinza  = sin( zarad );

  // This part as pol2cart but uses local variables
  x = r * coslat;  
  z = r * sinlat;

  const Numeric   dr   = cosza;
  const Numeric   dlat = sinza;         // r-term cancel out below

  dx = coslat * dr - sinlat * dlat;
  dz = sinlat * dr + coslat * dlat;
}





/*===========================================================================
  === 3D functions
  ===========================================================================*/


void cart2poslos(
             Numeric&   r,
             Numeric&   lat,
             Numeric&   lon,
             Numeric&   za,
             Numeric&   aa,
       const Numeric&   x,
       const Numeric&   y,
       const Numeric&   z,
       const Numeric&   dx,
       const Numeric&   dy,
       const Numeric&   dz,
       const Numeric&   ppc,
       const Numeric&   lat0,
       const Numeric&   lon0,
       const Numeric&   za0,
       const Numeric&   aa0 )
{
  r   = sqrt( x*x + y*y + z*z );

  // Zenith and nadir cases
  if( za0 < ANGTOL  ||  za0 > 180-ANGTOL  )
    { 
      lat = lat0;
      lon = lon0;
      za  = za0; 
      aa  = aa0; 
    }

  else
    {
      lat = RAD2DEG * asin( z / r );
      lon = RAD2DEG * atan2( y, x );

      bool ns_case = false;
      bool lon_flip = false;

      // Make sure that lon is maintained for N-S cases (if not 
      // starting on a pole)
      if( ( abs(aa0) < ANGTOL  ||  abs(180-aa0) < ANGTOL )  && 
                                             abs( lat0 ) <= POLELAT )
        {
          ns_case = true;
          // Check that not lon changed with 180 deg
          if( abs(lon-lon0) < 1 )
            { lon = lon0; }
          else
            {
              lon_flip = true;
              if( lon0 > 0 )
                { lon = lon0 - 180; }
              else
                { lon = lon0 + 180; }
            }
        }

      const Numeric   latrad = DEG2RAD * lat;
      const Numeric   lonrad = DEG2RAD * lon;
      const Numeric   coslat = cos( latrad );
      const Numeric   sinlat = sin( latrad );
      const Numeric   coslon = cos( lonrad );
      const Numeric   sinlon = sin( lonrad );
      const Numeric   dr     = coslat*coslon*dx + coslat*sinlon*dy + sinlat*dz;

      // Use ppc for max accuracy, but dr required to resolve if up- 
      // and downward cases
      za = RAD2DEG * asin( ppc / r );
      if( isnan( za ) )
        { za = 90; }
      if( dr < 0 )
        { za = 180.0 - za; }

      // The difference below can at least be 3e-6 for tangent points 
      assert( abs( za - RAD2DEG*acos(dr) ) < 1e-4 );

      // For lat = +- 90 the azimuth angle gives the longitude along which 
      // the LOS goes
      if( abs( lat ) >= POLELAT )      
        { aa = RAD2DEG * atan2( dy, dx ); }

      // N-S cases, not starting at a pole
      else if( ns_case )
        { 
          if( !lon_flip )
            { aa = aa0; }
          else
            {
              if( abs(aa0) < ANGTOL )
                { aa = 180; }
              else
                { aa = 0; }
            }
        }

      else
        {
          const Numeric   dlat = -sinlat*coslon/r*dx - sinlat*sinlon/r*dy + 
                                                             coslat/r*dz;
          const Numeric   dlon = -sinlon/coslat/r*dx + coslon/coslat/r*dy;

          aa = RAD2DEG * acos( r * dlat / sin( DEG2RAD * za ) );

          if( isnan( aa ) )
            {
              if( dlat >= 0 )
                { aa = 0; }
              else
                { aa = 180; }
            }
          else if( dlon < 0 )
            { aa = -aa; }
        }
    }
}




void cart2sph(
             Numeric&   r,
             Numeric&   lat,
             Numeric&   lon,
       const Numeric&   x,
       const Numeric&   y,
       const Numeric&   z,
       const Numeric&   lat0,
       const Numeric&   lon0,
       const Numeric&   za0,
       const Numeric&   aa0 )
{
  r   = sqrt( x*x + y*y + z*z );

  // Zenith and nadir cases
  if( za0 < ANGTOL  ||  za0 > 180-ANGTOL  )
    { 
      lat = lat0;
      lon = lon0;
    }

  else
    {
      lat = RAD2DEG * asin( z / r );
      lon = RAD2DEG * atan2( y, x );

      // Make sure that lon is maintained for N-S cases (if not 
      // starting on a pole)
      if( ( abs(aa0) < ANGTOL  ||  abs(180-aa0) < ANGTOL )  && 
                                             abs( lat0 ) <= POLELAT )
        {
          // Check that not lon changed with 180 deg
          if( abs(lon-lon0) < 1 )
            { lon = lon0; }
          else
            {
              if( lon0 > 0 )
                { lon = lon0 - 180; }
              else
                { lon = lon0 + 180; }
            }
        }
    }
}




void distance3D(
            Numeric&   l,
      const Numeric&   r1,
      const Numeric&   lat1,
      const Numeric&   lon1,
      const Numeric&   r2,
      const Numeric&   lat2,
      const Numeric&   lon2 )
{
  Numeric x1, y1, z1, x2, y2, z2;
  sph2cart( x1, y1, z1, r1, lat1, lon1 );
  sph2cart( x2, y2, z2, r2, lat2, lon2 );

  const Numeric dx = x2 - x1; 
  const Numeric dy = y2 - y1; 
  const Numeric dz = z2 - z1; 
  l = sqrt( dx*dx + dy*dy + dz*dz ); 
}




void geompath_tanpos_3d( 
             Numeric&    r_tan,
             Numeric&    lat_tan,
             Numeric&    lon_tan,
             Numeric&    l_tan,
       const Numeric&    r,
       const Numeric&    lat,
       const Numeric&    lon,
       const Numeric&    za,
       const Numeric&    aa,
       const Numeric&    ppc )
{
  assert( za >= 90 );
  assert( r >= ppc );

  Numeric   x, y, z, dx, dy, dz; 

  poslos2cart( x, y, z, dx, dy, dz, r, lat, lon, za, aa );

  l_tan = sqrt( r*r - ppc*ppc );

  cart2sph( r_tan, lat_tan, lon_tan, x+dx*l_tan, y+dy*l_tan, z+dz*l_tan,
            lat, lon, za, aa );
}




/*
void geomtanpoint( 
             Numeric&    r_tan,
             Numeric&    lat_tan,
             Numeric&    lon_tan,
     ConstVectorView    refellipsoid,
       const Numeric&    r,
       const Numeric&    lat,
       const Numeric&    lon,
       const Numeric&    za,
       const Numeric&    aa )
{
  assert( refellipsoid.nelem() == 2 );
  assert( refellipsoid[0] > 0 );
  assert( refellipsoid[1] >= 0 );
  assert( refellipsoid[1] < 1 );
  assert( r > 0 );
  assert( za >= 90 );

  if( refellipsoid[1] < 1e-7 )        // e=1e-7 corresponds to that polar radius
    {                                 // less than 1 um smaller than equatorial 
      Numeric   x, y, z, dx, dy, dz;   // one for the Earth

      poslos2cart( x, y, z, dx, dy, dz, r, lat, lon, za, aa );
   
      const Numeric ppc   = r * sin( DEG2RAD * abs(za) );
      const Numeric l_tan = sqrt( r*r - ppc*ppc );
   
      cart2sph( r_tan, lat_tan, lon_tan, x+dx*l_tan, y+dy*l_tan, z+dz*l_tan );
    }

  else
    {
      // Equatorial and polar radii squared:
      const Numeric a2 = refellipsoid[0]*refellipsoid[0];
      const Numeric b2 = a2 * ( 1 - refellipsoid[1]*refellipsoid[1] ); 

      Vector X(3), xunit(3), yunit(3), zunit(3);

      poslos2cart( X[0], X[1], X[2], xunit[0], xunit[1], xunit[2], 
                                                         r, lat, lon, za, aa );
      cross( zunit, xunit, X );
      unitl( zunit );                // Normalisation of length to 1

      cross( yunit, zunit, xunit );
      unitl( yunit );                // Normalisation of length to 1

            Numeric x   = X[0];
            Numeric y   = X[1];
      const Numeric w11 = xunit[0];
      const Numeric w12 = yunit[0];
      const Numeric w21 = xunit[1];
      const Numeric w22 = yunit[1];
      const Numeric w31 = xunit[2];
      const Numeric w32 = yunit[2];

      const Numeric yr = X * yunit;
      const Numeric xr = X * xunit;

      const Numeric A = (w11*w11 + w21*w21)/a2 + w31*w31/b2;
      const Numeric B = 2.0*((w11*w12 + w21*w22)/a2 + (w31*w32)/b2);
      const Numeric C = (w12*w12 + w22*w22)/a2 + w32*w32/b2;

      if( B == 0.0 )
        { x = 0.0; }
      else 
        { 
          const Numeric K      = -2.0*A/B; 
          const Numeric factor = 1.0/(A+(B+C*K)*K);
          x = sqrt(factor);
          y = K*x;
        }

      const Numeric dist1 = (xr-X[0])*(xr-X[0]) + (yr-y)*(yr-y);
      const Numeric dist2 = (xr+X[0])*(xr+X[0]) + (yr+y)*(yr+y);
        
      if( dist1 > dist2 )
        { x = -x; }

      cart2sph( r_tan, lat_tan, lon_tan, w11*x + w12*yr, w21*x + w22*yr,
                                                         w31*x + w32*yr );
    }
}
*/




void line_sphere_intersect(
         Numeric&   x,
         Numeric&   y,
         Numeric&   z,
   const Numeric&   xl,
   const Numeric&   yl,
   const Numeric&   zl,
   const Numeric&   dx,
   const Numeric&   dy,
   const Numeric&   dz,
   const Numeric&   xc,
   const Numeric&   yc,
   const Numeric&   zc,
   const Numeric&   r )
{
  const Numeric a = dx*dx + dy*dy + dz*dz;
  const Numeric b = 2*( dx*(xl-xc) + dy*(yl-yc) + dz*(zl-zc) );
  const Numeric c = xc*xc + yc*yc + zc*zc + 
                    xl*xl + yl*yl + zl*zl - 2*(xc*xl + yc*yl + zc*zl) - r*r;
  
  Numeric d = b*b - 4*a*c;
  assert( d > 0 );
  
  const Numeric a2 = 2*a;
  const Numeric b2 = -b / a2;
  const Numeric e  = sqrt( d ) / a2;

  const Numeric l1 = b2 + e;
  const Numeric l2 = b2 - e;

  Numeric l;
  if( l1 < 0 )
    { l = l2; }
  else  if( l2 < 0 )
    { l = l1; }
  else
    { l = min(l1,l2); assert( l>=0 ); }

  x = xl + l*dx;
  y = yl + l*dy;
  z = zl + l*dz;
}




void latlon_at_aa(
         Numeric&   lat2,
         Numeric&   lon2,
   const Numeric&   lat1,
   const Numeric&   lon1,
   const Numeric&   aa,
   const Numeric&   ddeg )
{
  // Code from http://www.movable-type.co.uk/scripts/latlong.html
  // (but with short-cuts, such as asin(sin(lat2)) = lat2)
  // Note that lat1 here is another latitude
  
  const Numeric dang   = DEG2RAD * ddeg;
  const Numeric cosdang= cos( dang );
  const Numeric sindang= sin( dang );
  const Numeric latrad = DEG2RAD * lat1;
  const Numeric coslat = cos( latrad );
  const Numeric sinlat = sin( latrad );
  const Numeric aarad  = DEG2RAD * aa;

  lat2   = sinlat*cosdang + coslat*sindang*cos(aarad);
  lon2   = lon1 + RAD2DEG*( atan2( sin(aarad)*sindang*coslat,
                                   cosdang-sinlat*lat2 ) );
  lat2 = RAD2DEG * asin( lat2 );
}




void los2xyz( 
         Numeric&   za, 
         Numeric&   aa, 
   const Numeric&   r1,
   const Numeric&   lat1,    
   const Numeric&   lon1,
   const Numeric&   x1, 
   const Numeric&   y1, 
   const Numeric&   z1, 
   const Numeric&   x2, 
   const Numeric&   y2, 
   const Numeric&   z2 )
{
  Numeric dx = x2-x1, dy = y2-y1, dz = z2-z1;
  const Numeric ldxyz = sqrt( dx*dx + dy*dy + dz*dz );
  dx /= ldxyz;   
  dy /= ldxyz;   
  dz /= ldxyz;

  // All below extracted from 3D version of cart2poslos:
  const Numeric   latrad = DEG2RAD * lat1;
  const Numeric   lonrad = DEG2RAD * lon1;
  const Numeric   coslat = cos( latrad );
  const Numeric   sinlat = sin( latrad );
  const Numeric   coslon = cos( lonrad );
  const Numeric   sinlon = sin( lonrad );

  const Numeric   dr     = coslat*coslon*dx    + coslat*sinlon*dy    + sinlat*dz;
  const Numeric   dlat   = -sinlat*coslon/r1*dx - sinlat*sinlon/r1*dy + 
                                                                   coslat/r1*dz;
  const Numeric   dlon   = -sinlon/coslat/r1*dx + coslon/coslat/r1*dy;

  za = RAD2DEG * acos( dr );
  aa = RAD2DEG * acos( r1 * dlat / sin( DEG2RAD * za ) );
  if( isnan( aa ) )
    {
      if( dlat >= 0 )
        { aa = 0; }
      else
        { aa = 180; }
    }
  else if( dlon < 0 )
    { aa = -aa; }
}




void poslos2cart(
             Numeric&   x,
             Numeric&   y,
             Numeric&   z,
             Numeric&   dx,
             Numeric&   dy,
             Numeric&   dz,
       const Numeric&   r,
       const Numeric&   lat,
       const Numeric&   lon,
       const Numeric&   za,
       const Numeric&   aa )
{
  assert( r > 0 );
  assert( abs( lat ) <= 90 );
  assert( abs( lon ) <= 360 );
  assert( za >= 0 && za<=180 );

  // lat = +-90 
  // For lat = +- 90 the azimuth angle gives the longitude along which the
  // LOS goes
  if( abs( lat ) > POLELAT )
    {
      const Numeric   s = sign( lat );

      x = 0;
      y = 0;
      z = s * r;

      dz = s * cos( DEG2RAD * za );
      dx = sin( DEG2RAD * za );
      dy = dx * sin( DEG2RAD * aa );
      dx = dx * cos( DEG2RAD * aa );
    }

  else
    {
      const Numeric   latrad = DEG2RAD * lat;
      const Numeric   lonrad = DEG2RAD * lon;
      const Numeric   zarad  = DEG2RAD * za;
      const Numeric   aarad  = DEG2RAD * aa;

      const Numeric   coslat = cos( latrad );
      const Numeric   sinlat = sin( latrad );
      const Numeric   coslon = cos( lonrad );
      const Numeric   sinlon = sin( lonrad );
      const Numeric   cosza  = cos( zarad );
      const Numeric   sinza  = sin( zarad );
      const Numeric   cosaa  = cos( aarad );
      const Numeric   sinaa  = sin( aarad );

      // This part as sph2cart but uses local variables
      x = r * coslat;   // Common term for x and y
      y = x * sinlon;
      x = x * coslon;
      z = r * sinlat;

      const Numeric   dr   = cosza;
      const Numeric   dlat = sinza * cosaa;         // r-term cancel out below
      const Numeric   dlon = sinza * sinaa / coslat; 

      dx = coslat*coslon * dr - sinlat*coslon * dlat - coslat*sinlon * dlon;
      dz =        sinlat * dr +        coslat * dlat;
      dy = coslat*sinlon * dr - sinlat*sinlon * dlat + coslat*coslon * dlon;
    }
}




Numeric pos2refell_r(
       const Index&     atmosphere_dim,
       ConstVectorView  refellipsoid,
       ConstVectorView  lat_grid,
       ConstVectorView  lon_grid,
       ConstVectorView  rte_pos )
{
  if( atmosphere_dim == 1 )
    { return refellipsoid[0]; }
  else
    {
      assert( rte_pos.nelem() > 1 );

      bool inside = true;

      if( rte_pos[1] < lat_grid[0] ||  rte_pos[1] > last(lat_grid) )
        { inside = false; }
      else if( atmosphere_dim == 3 )
        {
          assert( rte_pos.nelem() == 3 );
          if( rte_pos[2] < lon_grid[0] ||  rte_pos[2] > last(lon_grid) )
            { inside = false; }
        }
          
      if( inside )
        {
          GridPos gp_lat;
          gridpos( gp_lat, lat_grid, rte_pos[1] );
          return refell2d( refellipsoid, lat_grid, gp_lat );
        }
      else
        { return refell2r( refellipsoid, rte_pos[1] ); }
    }
}




Numeric refell2r(
       ConstVectorView  refellipsoid,
       const Numeric&   lat )
{
  assert( refellipsoid.nelem() == 2 );
  assert( refellipsoid[0] > 0 );
  assert( refellipsoid[1] >= 0 );
  assert( refellipsoid[1] < 1 );

  if( refellipsoid[1] < 1e-7 )  // e=1e-7 corresponds to that polar radius  
    {                           // less than 1 um smaller than equatorial 
      return refellipsoid[0];   // one for the Earth
    }

  else
    {
      const Numeric   c = 1 - refellipsoid[1]*refellipsoid[1];
      const Numeric   b = refellipsoid[0] * sqrt( c );
      const Numeric   v = DEG2RAD * lat;
      const Numeric   ct = cos( v );
      const Numeric   st = sin( v );
      
      return b / sqrt( c*ct*ct + st*st );
    }
}




Numeric refell2d(
       ConstVectorView  refellipsoid,
       ConstVectorView  lat_grid,
       const GridPos    gp )
{
  if( gp.fd[0] == 0 )
    return refell2r(refellipsoid,lat_grid[gp.idx]);
  else if( gp.fd[0] == 1 )
    return refell2r(refellipsoid,lat_grid[gp.idx+1]);
  else
    return gp.fd[1] * refell2r(refellipsoid,lat_grid[gp.idx]) +
           gp.fd[0] * refell2r(refellipsoid,lat_grid[gp.idx+1]);
}       




Numeric sphdist(
   const Numeric&   lat1,
   const Numeric&   lon1,
   const Numeric&   lat2,
   const Numeric&   lon2 )
{
  // Equations taken from http://www.movable-type.co.uk/scripts/latlong.html
  const Numeric slat = sin( DEG2RAD*(lat2-lat1) / 2.0 );
  const Numeric slon = sin( DEG2RAD*(lon2-lon1) / 2.0 );
  const Numeric a = slat*slat + cos(DEG2RAD*lat1)*cos(DEG2RAD*lat2)*slon*slon;

  return RAD2DEG * 2 * atan2( sqrt(a), sqrt(1-a) );
}       






void sph2cart(
            Numeric&   x,
            Numeric&   y,
            Numeric&   z,
      const Numeric&   r,
      const Numeric&   lat,
      const Numeric&   lon )
{
  assert( r > 0 );
  assert( abs( lat ) <= 90 );
  assert( abs( lon ) <= 360 );

  const Numeric   latrad = DEG2RAD * lat;
  const Numeric   lonrad = DEG2RAD * lon;

  x = r * cos( latrad );   // Common term for x and z
  y = x * sin( lonrad );
  x = x * cos( lonrad );
  z = r * sin( latrad );
}





/*===========================================================================
  === coordinate transformations
  ===========================================================================*/


void lon_shiftgrid(
            Vector&    longrid_out,
      ConstVectorView  longrid_in,
      const Numeric    lon )
{
    longrid_out = longrid_in;
    if (longrid_in[longrid_in.nelem()-1] >= lon+360.)
      longrid_out += -360.;
    else if (longrid_in[0] <= lon-360.)
      longrid_out += 360.;
}