datum.c

给予QT的qps开源最新源码

 
  /* Set WGS84 ellipsoid params */
  WGS84_Parameters(&a_84, &f_84);
  Set_Geocentric_Parameters(a_84, f_84);
  Convert_Geocentric_To_Geodetic(X_WGS84, Y_WGS84, Z_WGS84, &Lat_84, &Lon_84, &Hgt_84);
  Geodetic_Shift_WGS84_To_WGS72(Lat_84, Lon_84, Hgt_84, &Lat_72, &Lon_72,
                                &Hgt_72);
  /* Set WGS72 ellipsoid params */
  WGS72_Parameters(&a_72, &f_72);
  Set_Geocentric_Parameters(a_72, f_72);
  Convert_Geodetic_To_Geocentric(Lat_72, Lon_72, Hgt_72, X, Y, Z);
} /* End Geocentric_Shift_WGS84_To_WGS72 */


long Geocentric_Shift_To_WGS84(const long Index,
                               const double X,
                               const double Y,
                               const double Z,
                               double *X_WGS84,
                               double *Y_WGS84,
                               double *Z_WGS84)

{ /* Begin Geocentric_Shift_To_WGS84 */
  /*
   *  This function shifts a geocentric coordinate (X, Y, Z in meters) relative
   *  to the datum referenced by index to a geocentric coordinate (X, Y, Z in
   *  meters) relative to WGS84.
   *
   *  Index   : Index of source datum                         (input)
   *  X       : X coordinate relative to the source datum     (input)
   *  Y       : Y coordinate relative to the source datum     (input)
   *  Z       : Z coordinate relative to the source datum     (input)
   *  X_WGS84 : X coordinate relative to WGS84                (output)
   *  Y_WGS84 : Y coordinate relative to WGS84                (output)
   *  Z_WGS84 : Z coordinate relative to WGS84                (output)
   */
  Datum_Row *local;
  long error_code = DATUM_NO_ERROR;

  if (Datum_Initialized)
  {
    if ((Index < 1) || (Index > Number_of_Datums))
      error_code |= DATUM_INVALID_SRC_INDEX_ERROR;
    if (!error_code)
    {
      local = Datum_Table[Index-1];
      switch (local->Type)
      {
      case WGS72_Datum:
        {
          Geocentric_Shift_WGS72_To_WGS84(X, Y, Z, X_WGS84, Y_WGS84, Z_WGS84);
          break;
        }
      case WGS84_Datum:
        {          
          *X_WGS84 = X;
          *Y_WGS84 = Y;
          *Z_WGS84 = Z;
          break;
        }
      case Seven_Param_Datum:
        {
          *X_WGS84 = X + local->Parameters[0] + local->Parameters[5] * Y
                     - local->Parameters[4] * Z + local->Parameters[6] * X;
          *Y_WGS84 = Y + local->Parameters[1] - local->Parameters[5] * X
                     + local->Parameters[3] * Z + local->Parameters[6] * Y;
          *Z_WGS84 = Z + local->Parameters[2] + local->Parameters[4] * X
                     - local->Parameters[3] * Y + local->Parameters[6] * Z;
          break;
        }
      case Three_Param_Datum:
        {
          *X_WGS84 = X + local->Parameters[0];
          *Y_WGS84 = Y + local->Parameters[1];
          *Z_WGS84 = Z + local->Parameters[2];
          break;
        }
      } /* End switch */
    }
  }
  else
  {
    error_code |= DATUM_NOT_INITIALIZED_ERROR;
  }
  return (error_code);
} /* End Geocentric_Shift_To_WGS84 */


long Geocentric_Shift_From_WGS84(const double X_WGS84,
                                 const double Y_WGS84,
                                 const double Z_WGS84,
                                 const long Index,
                                 double *X,
                                 double *Y,
                                 double *Z)

{ /* Begin Geocentric_Shift_From_WGS84 */
  /*
   *  This function shifts a geocentric coordinate (X, Y, Z in meters) relative
   *  to WGS84 to a geocentric coordinate (X, Y, Z in meters) relative to the
   *  local datum referenced by index.
   *
   *  X_WGS84 : X coordinate relative to WGS84                      (input)
   *  Y_WGS84 : Y coordinate relative to WGS84                      (input)
   *  Z_WGS84 : Z coordinate relative to WGS84                      (input)
   *  Index   : Index of destination datum                          (input)
   *  X       : X coordinate relative to the destination datum      (output)
   *  Y       : Y coordinate relative to the destination datum      (output)
   *  Z       : Z coordinate relative to the destination datum      (output)
   */
  Datum_Row *local;
  long error_code = DATUM_NO_ERROR;

  if (Datum_Initialized)
  {
    if ((Index < 1) || (Index > Number_of_Datums))
      error_code |= DATUM_INVALID_DEST_INDEX_ERROR;
    if (!error_code)
    {
      local = Datum_Table[Index-1];
      switch (local->Type)
      {
      case WGS72_Datum:
        {
          Geocentric_Shift_WGS84_To_WGS72(X_WGS84, Y_WGS84, Z_WGS84, X, Y, Z);
          break;
        }
      case WGS84_Datum:
        {
          *X = X_WGS84;
          *Y = Y_WGS84;
          *Z = Z_WGS84;    
          break;
        }
      case Seven_Param_Datum:
        {
          *X = X_WGS84 - local->Parameters[0] - local->Parameters[5] * Y_WGS84
               + local->Parameters[4] * Z_WGS84 - local->Parameters[6] * X_WGS84;
          *Y = Y_WGS84 - local->Parameters[1] + local->Parameters[5] * X_WGS84
               - local->Parameters[3] * Z_WGS84 - local->Parameters[6] * Y_WGS84;
          *Z = Z_WGS84 - local->Parameters[2] - local->Parameters[4] * X_WGS84
               + local->Parameters[3] * Y_WGS84 - local->Parameters[6] * Z_WGS84;
          break;
        }
      case Three_Param_Datum:
        {
          *X = X_WGS84 - local->Parameters[0];
          *Y = Y_WGS84 - local->Parameters[1];
          *Z = Z_WGS84 - local->Parameters[2];
          break;
        }
      } /* End switch */
    }
  }
  else
  {
    error_code |= DATUM_NOT_INITIALIZED_ERROR;
  }
  return (error_code);
} /* End Geocentric_Shift_From_WGS84 */


long Geocentric_Datum_Shift (const long Index_in,
                             const double X_in,
                             const double Y_in,
                             const double Z_in,
                             const long Index_out,
                             double *X_out,
                             double *Y_out,
                             double *Z_out)

{ /* Begin Geocentric_Datum_Shift */
  /*
   *  This function shifts a geocentric coordinate (X, Y, Z in meters) relative
   *  to the source datum to geocentric coordinate (X, Y, Z in meters) relative
   *  to the destination datum.
   *
   *  Index_in  : Index of source datum                      (input)
   *  X_in      : X coordinate relative to source datum      (input)
   *  Y_in      : Y coordinate relative to source datum      (input)
   *  Z_in      : Z coordinate relative to source datum      (input)
   *  Index_out : Index of destination datum                 (input)
   *  X_out     : X coordinate relative to destination datum (output)
   *  Y_out     : Y coordinate relative to destination datum (output)
   *  Z_out     : Z coordinate relative to destination datum (output)
   */
  long error_code = DATUM_NO_ERROR;
  double X_WGS84;
  double Y_WGS84;
  double Z_WGS84;

  if (Datum_Initialized)
  {
    if ((Index_in < 1) || (Index_in > Number_of_Datums))
      error_code |= DATUM_INVALID_SRC_INDEX_ERROR;
    if ((Index_out < 1) || (Index_out > Number_of_Datums))
      error_code |= DATUM_INVALID_DEST_INDEX_ERROR;
    if (!error_code)
    {
      if (Index_in == Index_out)
      {
        *X_out = X_in;
        *Y_out = Y_in;
        *Z_out = Z_in;  
      }
      else
      {
        Geocentric_Shift_To_WGS84(Index_in, X_in, Y_in, Z_in, &X_WGS84,
                                  &Y_WGS84,&Z_WGS84);
        Geocentric_Shift_From_WGS84(X_WGS84, Y_WGS84, Z_WGS84, Index_out,
                                    X_out, Y_out, Z_out);      
      }
    }
  }
  else
  {
    error_code |= DATUM_NOT_INITIALIZED_ERROR;
  }
  return (error_code);
} /* End Geocentric_Datum_Shift */


void Geodetic_Shift_WGS72_To_WGS84( const double WGS72_Lat,
                                    const double WGS72_Lon,
                                    const double WGS72_Hgt,
                                    double *WGS84_Lat,
                                    double *WGS84_Lon,
                                    double *WGS84_Hgt)

{ /* Begin Geodetic_Shift_WGS72_To_WGS84 */
  /*
   *  This function shifts a geodetic coordinate (latitude, longitude in radians
   *  and height in meters) relative to WGS72 to a geodetic coordinate 
   *  (latitude, longitude in radians and height in meters) relative to WGS84.
   *
   *  WGS72_Lat : Latitude in radians relative to WGS72     (input)
   *  WGS72_Lon : Longitude in radians relative to WGS72    (input)
   *  WGS72_Hgt : Height in meters relative to WGS72        (input)
   *  WGS84_Lat : Latitude in radians relative to WGS84     (output)
   *  WGS84_Lon : Longitude in radians relative to WGS84    (output)
   *  WGS84_Hgt : Height in meters  relative to WGS84       (output)
   */
  double Delta_Lat;
  double Delta_Lon;
  double Delta_Hgt;
  double WGS84_a;       /* Semi-major axis of WGS84 ellipsoid               */
  double WGS84_f;       /* Flattening of WGS84 ellipsoid                    */
  double WGS72_a;       /* Semi-major axis of WGS72 ellipsoid               */
  double WGS72_f;       /* Flattening of WGS72 ellipsoid                    */
  double da;            /* WGS84_a - WGS72_a                                */
  double df;            /* WGS84_f - WGS72_f                                */
  double Q;
  double sin_Lat;
  double sin2_Lat;

  WGS84_Parameters( &WGS84_a, &WGS84_f );  
  WGS72_Parameters( &WGS72_a, &WGS72_f );  
  da = WGS84_a - WGS72_a;
  df = WGS84_f - WGS72_f;
  Q = PI /  648000;
  sin_Lat = sin(WGS72_Lat);
  sin2_Lat = sin_Lat * sin_Lat;

  Delta_Lat = (4.5 * cos(WGS72_Lat)) / (WGS72_a*Q) + (df * sin(2*WGS72_Lat)) / Q;
  Delta_Lat /= SECONDS_PER_RADIAN;
  Delta_Lon = 0.554 / SECONDS_PER_RADIAN;
  Delta_Hgt = 4.5 * sin_Lat + WGS72_a * df * sin2_Lat - da + 1.4;

  *WGS84_Lat = WGS72_Lat + Delta_Lat;
  *WGS84_Lon = WGS72_Lon + Delta_Lon;
  *WGS84_Hgt = WGS72_Hgt + Delta_Hgt;
} /* End Geodetic_Shift_WGS72_To_WGS84 */


void Geodetic_Shift_WGS84_To_WGS72( const double WGS84_Lat,
                                    const double WGS84_Lon,
                                    const double WGS84_Hgt,
                                    double *WGS72_Lat,
                                    double *WGS72_Lon,
                                    double *WGS72_Hgt)

{ /* Begin Geodetic_Shift_WGS84_To_WGS72 */
  /*
   *  This function shifts a geodetic coordinate (latitude, longitude in radians
   *  and height in meters) relative to WGS84 to a geodetic coordinate 
   *  (latitude, longitude in radians and height in meters) relative to WGS72.
   *
   *  WGS84_Lat : Latitude in radians relative to WGS84     (input)
   *  WGS84_Lon : Longitude in radians relative to WGS84    (input)
   *  WGS84_Hgt : Height in meters  relative to WGS84       (input)
   *  WGS72_Lat : Latitude in radians relative to WGS72     (output)
   *  WGS72_Lon : Longitude in radians relative to WGS72    (output)
   *  WGS72_Hgt : Height in meters relative to WGS72        (output)
   */
  double Delta_Lat;
  double Delta_Lon;
  double Delta_Hgt;
  double WGS84_a;       /* Semi-major axis of WGS84 ellipsoid               */
  double WGS84_f;       /* Flattening of WGS84 ellipsoid                    */
  double WGS72_a;       /* Semi-major axis of WGS72 ellipsoid               */
  double WGS72_f;       /* Flattening of WGS72 ellipsoid                    */
  double da;            /* WGS72_a - WGS84_a                                */
  double df;            /* WGS72_f - WGS84_f                                */
  double Q;
  double sin_Lat;
  double sin2_Lat;

  WGS84_Parameters( &WGS84_a, &WGS84_f );  
  WGS72_Parameters( &WGS72_a, &WGS72_f );  
  da = WGS72_a - WGS84_a;
  df = WGS72_f - WGS84_f;
  Q = PI / 648000;
  sin_Lat = sin(WGS84_Lat);
  sin2_Lat = sin_Lat * sin_Lat;

  Delta_Lat = (-4.5 * cos(WGS84_Lat)) / (WGS84_a*Q)
              + (df * sin(2*WGS84_Lat)) / Q;
  Delta_Lat /= SECONDS_PER_RADIAN;
  Delta_Lon = -0.554 / SECONDS_PER_RADIAN;
  Delta_Hgt = -4.5 * sin_Lat + WGS84_a * df * sin2_Lat - da - 1.4;

  *WGS72_Lat = WGS84_Lat + Delta_Lat;
  *WGS72_Lon = WGS84_Lon + Delta_Lon;
  *WGS72_Hgt = WGS84_Hgt + Delta_Hgt;
} /* End Geodetic_Shift_WGS84_To_WGS72 */


void Molodensky_Shift( const double a,
                       const double da,
                       const double f,
                       const double df,
                       const double dx,
                       const double dy,
                       const double dz,
                       const double Lat_in,
                       const double Lon_in,
                       const double Hgt_in,
                       double *Lat_out,
                       double *Lon_out,
                       double *Hgt_out)

{ /* Begin Molodensky_Shift */
  /*
   *  This function shifts geodetic coordinates using the Molodensky method.
   *
   *    a         : Semi-major axis of source ellipsoid in meters  (input)
   *    da        : Destination a minus source a                   (input)
   *    f         : Flattening of source ellipsoid                 (input)
   *    df        : Destination f minus source f                   (input)
   *    dx        : X coordinate shift in meters                   (input)
   *    dy        : Y coordinate shift in meters                   (input)
   *    dz        : Z coordinate shift in meters                   (input)
   *    Lat_in    : Latitude in radians.                           (input)
   *    Lon_in    : Longitude in radians.