text: //============================================================================
  //
  //  This file is part of GPSTk, the GPS Toolkit.
  //
  //  The GPSTk is free software; you can redistribute it and/or modify
  //  it under the terms of the GNU Lesser General Public License as published
  //  by the Free Software Foundation; either version 3.0 of the License, or
  //  any later version.
  //
  //  The GPSTk 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 Lesser General Public License for more details.
  //
  //  You should have received a copy of the GNU Lesser General Public
  //  License along with GPSTk; if not, write to the Free Software Foundation,
  //  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  //  
  //  Copyright 2004, The University of Texas at Austin
  //
  //============================================================================
  
  //============================================================================
  //
  //This software developed by Applied Research Laboratories at the University of
  //Texas at Austin, under contract to an agency or agencies within the U.S. 
  //Department of Defense. The U.S. Government retains all rights to use,
  //duplicate, distribute, disclose, or release this software. 
  //
  //Pursuant to DoD Directive 523024 
  //
  // DISTRIBUTION STATEMENT A: This software has been approved for public 
  //                           release, distribution is unlimited.
  //
  //=============================================================================
  
  /**
   * @file Position.cpp
   * class gpstk::Position encapsulates 3-D positions, both geographic positions,
   *    expressed as geodetic (with respect to any geoid), geocentric or
   *    Earth-centered, Earth-fixed (cartesian) coordinates, as well as ordinary
   *    positions defined by spherical or cartesian coordinates. Position inherits
   *    from class Triple.
   */
  
  #include "Position.hpp"
  #include "WGS84Ellipsoid.hpp"
  #include "GNSSconstants.hpp"    // for TWO_PI, etc
  #include "GNSSconstants.hpp"             // for RAD_TO_DEG, etc
  #include "MiscMath.hpp"             // for RSS, SQRT
  
  namespace gpstk
  {
  
     using namespace std;
     using namespace StringUtils;
  
     // ----------- Part  1: coordinate systems --------------------------------
        // Labels for coordinate systems supported by Position
     static const char *SystemNames[] = {
        "Unknown",
        "Geodetic",
        "Geocentric",
        "Cartesian",
        "Spherical"};
  
        // return string giving name of coordinate system
     string Position::getSystemName()
        throw()
     { return SystemNames[system]; }
  
     // ----------- Part  2: tolerance -----------------------------------------
        // One millimeter tolerance.
     const double Position::ONE_MM_TOLERANCE = 0.001;
        // One centimeter tolerance.
     const double Position::ONE_CM_TOLERANCE = 0.01;
        // One micron tolerance.
     const double Position::ONE_UM_TOLERANCE = 0.000001;
  
        // Default tolerance for time equality in meters.
     double Position::POSITION_TOLERANCE = Position::ONE_MM_TOLERANCE;
  
        // Sets the tolerance for output and comparisons, for this object only.
        // See the constants in this file (e.g. ONE_MM_TOLERANCE)
        // for some easy to use tolerance values.
        // @param tol Tolerance in meters to be used by comparison operators.
     Position& Position::setTolerance(const double tol)
        throw()
     {
        tolerance = tol;
        return *this;
     }
  
     // ----------- Part  3: member functions: constructors --------------------
     //
        // Default constructor.
     Position::Position()
        throw()
     {
        WGS84Ellipsoid WGS84;
        initialize(0.0,0.0,0.0,Unknown,&WGS84);
     }
  
     Position::Position(const double& a,
                        const double& b,
                        const double& c,
                        Position::CoordinateSystem s,
                        EllipsoidModel *ell,
                        ReferenceFrame frame)
        throw(GeometryException)
     {
        try {
           initialize(a,b,c,s,ell,frame);
        }
        catch(GeometryException& ge) {
           GPSTK_RETHROW(ge);
        }
     }
  
     Position::Position(const double ABC[3],
                        CoordinateSystem s,
                        EllipsoidModel *ell,
                        ReferenceFrame frame)
        throw(GeometryException)
     {
        double a=ABC[0];
        double b=ABC[1];
        double c=ABC[2];
        try {
           initialize(a,b,c,s,ell,frame);
        }
        catch(GeometryException& ge) {
           GPSTK_RETHROW(ge);
        }
     }
  
     Position::Position(const Triple& ABC,
                        CoordinateSystem s,
                        EllipsoidModel *ell,
                        ReferenceFrame frame)
        throw(GeometryException)
     {
        double a=ABC[0];
        double b=ABC[1];
        double c=ABC[2];
        try {
           initialize(a,b,c,s,ell,frame);
        }
        catch(GeometryException& ge) {
           GPSTK_RETHROW(ge);
        }
     }
  
     Position::Position(const Xvt& xvt)
        throw()
     {
        double a=xvt.x[0];
        double b=xvt.x[1];
        double c=xvt.x[2];
        initialize(a,b,c,Cartesian, NULL, xvt.frame);
     }
  
     // ----------- Part  4: member functions: arithmetic ----------------------
     //
     Position& Position::operator-=(const Position& right)
        throw()
     {
        Position r(right);
        CoordinateSystem savesys=system;    // save the original system
  
           // convert to cartestian and difference there
        transformTo(Cartesian);
        r.transformTo(Cartesian);
  
        for(int i=0; i<3; i++)
           theArray[i] -= r.theArray[i];
  
        transformTo(savesys);               // transform back to the original system
        return *this;
     }
  
     Position& Position::operator+=(const Position& right)
        throw()
     {
        Position r(right);
        CoordinateSystem savesys=system;    // save the original system
  
           // convert to cartestian and difference there
        transformTo(Cartesian);
        r.transformTo(Cartesian);
  
        for(int i=0; i<3; i++)
           theArray[i] += r.theArray[i];
  
        transformTo(savesys);               // transform back to the original system
        return *this;
     }
  
     Position operator-(const Position& left,
                              const Position& right)
        throw()
     {
        Position l(left),r(right);
           // convert both to Cartesian
        l.transformTo(Position::Cartesian);
        r.transformTo(Position::Cartesian);
           // difference
        l -= r;
  
        return l;
     }
  
     Position operator+(const Position& left,
                              const Position& right)
        throw()
     {
        Position l(left),r(right);
           // convert both to Cartesian
        l.transformTo(Position::Cartesian);
        r.transformTo(Position::Cartesian);
           // add
        l += r;
  
        return l;
     }
  
     // ----------- Part  5: member functions: comparisons ---------------------
     //
        // Equality operator. Returns false if ell values differ.
     bool Position::operator==(const Position &right) const
        throw()
     {
        if(AEarth != right.AEarth || eccSquared != right.eccSquared)
           return false;
        if(right.getReferenceFrame() != refFrame)
           return false;   //Unknown frames are considered the same.
        if(range(*this,right) < tolerance)
           return true;
        else
           return false;
     }
  
        // Inequality operator. Returns true if ell values differ.
     bool Position::operator!=(const Position &right) const
        throw()
     {
        return !(operator==(right));
     }
  
     // ----------- Part  6: member functions: coordinate transformations ------
     //
        // Transform coordinate system. Does nothing if sys already matches the
        // current value of member CoordinateSystem 'system'.
        // @param sys coordinate system into which *this is to be transformed.
        // @return *this
     Position Position::transformTo(CoordinateSystem sys)
        throw()
     {
        if(sys == Unknown || sys == system) return *this;
  
        // this copies geoid information and tolerance
        Position target(*this);
  
        // transform target.theArray and set target.system
        switch(system) {
           case Unknown:
              return *this;
           case Geodetic:
              // --------------- Geodetic to ... ------------------------
              switch(sys) {
                 case Unknown: case Geodetic: return *this;
                 case Geocentric:
                    convertGeodeticToGeocentric(*this,target,AEarth,eccSquared);
                    target.system = Geocentric;
                    break;
                 case Cartesian:
                    convertGeodeticToCartesian(*this,target,AEarth,eccSquared);
                    target.system = Cartesian;
                    break;
                 case Spherical:
                    convertGeodeticToGeocentric(*this,target,AEarth,eccSquared);
                    target.theArray[0] = 90 - target.theArray[0];   // geocen -> sph
                    target.system = Spherical;
                    break;
              }
              break;
           case Geocentric:
              // --------------- Geocentric to ... ----------------------
              switch(sys) {
                 case Unknown: case Geocentric: return *this;
                 case Geodetic:
                    convertGeocentricToGeodetic(*this,target,AEarth,eccSquared);
                    target.system = Geodetic;
                    break;
                 case Cartesian:
                    convertGeocentricToCartesian(*this,target);
                    target.system = Cartesian;
                    break;
                 case Spherical:
                    target.theArray[0] = 90 - target.theArray[0];   // geocen -> sph
                    target.system = Spherical;
                    break;
              }
              break;
           case Cartesian:
              // --------------- Cartesian to ... -----------------------
              switch(sys) {
                 case Unknown: case Cartesian: return *this;
                 case Geodetic:
                    convertCartesianToGeodetic(*this,target,AEarth,eccSquared);
                    target.system = Geodetic;
                    break;
                 case Geocentric:
                    convertCartesianToGeocentric(*this,target);
                    target.system = Geocentric;
                    break;
                 case Spherical:
                    convertCartesianToSpherical(*this,target);
                    target.system = Spherical;
                    break;
              }
              break;
           case Spherical:
              // --------------- Spherical to ... -----------------------
              switch(sys) {
                 case Unknown: case Spherical: return *this;
                 case Geodetic:
                    theArray[0] = 90 - theArray[0];   // sph -> geocen
                    convertGeocentricToGeodetic(*this,target,AEarth,eccSquared);
                    target.system = Geodetic;
                    break;
                 case Geocentric:
                    target.theArray[0] = 90 - target.theArray[0];   // sph -> geocen
                    target.system = Geocentric;
                    break;
                 case Cartesian:
                    convertSphericalToCartesian(*this,target);
                    target.system = Cartesian;
                    break;
              }
              break;
        }  // end switch(system)
  
        *this = target;
        return *this;
     }
  
     // ----------- Part  7: member functions: get -----------------------------
     //
     // These routines retrieve coordinate values in all coordinate systems.
     // Note that calling these will transform the Position to another coordinate
     // system if that is required.
     //
     
     const ReferenceFrame& Position::getReferenceFrame() const
        throw()
     {   
        return refFrame;
     }
     
        // Get X coordinate (meters)
     double Position::X() const
        throw()
     {
        if(system == Cartesian)
           return theArray[0];
        Position t(*this);
        t.transformTo(Cartesian);
        return t.theArray[0];
     }
  
        // Get Y coordinate (meters)
     double Position::Y() const
        throw()
     {
        if(system == Cartesian)
           return theArray[1];
        Position t(*this);
        t.transformTo(Cartesian);
        return t.theArray[1];
     }
  
        // Get Z coordinate (meters)
     double Position::Z() const
        throw()
     {
        if(system == Cartesian)
           return theArray[2];
        Position t(*this);
        t.transformTo(Cartesian);
        return t.theArray[2];
     }
  
        // Get geodetic latitude (degrees North).
     double Position::geodeticLatitude() const
        throw()
     {
        if(system == Geodetic)
           return theArray[0];
        Position t(*this);
        t.transformTo(Geodetic);
        return t.theArray[0];
     }
  
        // Get geocentric latitude (degrees North),
        // equal to 90 degress - theta in regular spherical coordinates.
     double Position::geocentricLatitude() const
        throw()
     {
        if(system == Geocentric)
           return theArray[0];
        Position t(*this);
        t.transformTo(Geocentric);
        return t.theArray[0];
     }
  
        // Get spherical coordinate theta in degrees
     double Position::theta() const
        throw()
     {
        if(system == Spherical)
           return theArray[0];
        Position t(*this);
        t.transformTo(Spherical);
        return t.theArray[0];
     }
  
        // Get spherical coordinate phi in degrees
     double Position::phi() const
        throw()
     {
        if(system == Spherical)
           return theArray[1];
        Position t(*this);
        t.transformTo(Spherical);
        return t.theArray[1];
     }
  
        // Get longitude (degrees East),
        // equal to phi in regular spherical coordinates.
     double Position::longitude() const
        throw()
     {
        if(system != Cartesian)
           return theArray[1];
        Position t(*this);
        t.transformTo(Spherical);
        return t.theArray[1];
     }
  
        // Get radius or distance from the center of Earth (meters),
        // Same as radius in spherical coordinates.
     double Position::radius() const
        throw()
     {
        if(system == Spherical || system == Geocentric)
           return theArray[2];
        Position t(*this);
        t.transformTo(Spherical);
        return t.theArray[2];
     }
  
        // Get height above ellipsoid (meters) (Geodetic).
     double Position::height() const
        throw()
     {
        if(system == Geodetic)
           return theArray[2];
        Position t(*this);
        t.transformTo(Geodetic);
        return t.theArray[2];
     }
  
     // ----------- Part  8: member functions: set -----------------------------
     //
     void Position::setReferenceFrame(const ReferenceFrame& frame)
        throw()
     {
        refFrame = frame;
     }
     
        /**
        * Set the ellipsoid values for this Position given a ellipsoid.
        * @param ell  Pointer to the EllipsoidModel.
        * @throw      GeometryException if input is NULL.
        */
     void Position::setEllipsoidModel(const EllipsoidModel *ell)
        throw(GeometryException)
     {
        if(!ell)
        {
           GeometryException ge("Given EllipsoidModel pointer is NULL.");
           GPSTK_THROW(ge);
        }
        AEarth = ell->a();
        eccSquared = ell->eccSquared();
     }
  
        // Set the Position given geodetic coordinates, system is set to Geodetic.
        // @param lat geodetic latitude in degrees North
        // @param lon geodetic longitude in degrees East
        // @param ht height above the ellipsoid in meters
        // @return a reference to this object.
        // @throw GeometryException on invalid input
     Position& Position::setGeodetic(const double lat,
                                     const double lon,
                                     const double ht,
                                     const EllipsoidModel *ell)
        throw(GeometryException)
     {
        if(lat > 90 || lat < -90)
        {
           GeometryException ge("Invalid latitude in setGeodetic: "
                                   + StringUtils::asString(lat));
           GPSTK_THROW(ge);
        }
        theArray[0] = lat;
  
        theArray[1] = lon;
        if(theArray[1] < 0)
           theArray[1] += 360*(1+(unsigned long)(theArray[1]/360));
        else if(theArray[1] >= 360)
           theArray[1] -= 360*(unsigned long)(theArray[1]/360);
  
        theArray[2] = ht;
  
        if(ell) {
           AEarth = ell->a();
           eccSquared = ell->eccSquared();
        }
        system = Geodetic;
  
        return *this;
     }
  
        // Set the Position given geocentric coordinates, system is set to Geocentric
        // @param lat geocentric latitude in degrees North
        // @param lon geocentric longitude in degrees East
        // @param rad radius from the Earth's center in meters
        // @return a reference to this object.
        // @throw GeometryException on invalid input
     Position& Position::setGeocentric(const double lat,
                                       const double lon,
                                       const double rad)
        throw(GeometryException)
     {
        if(lat > 90 || lat < -90)
        {
           GeometryException ge("Invalid latitude in setGeocentric: "
                                   + StringUtils::asString(lat));
           GPSTK_THROW(ge);
        }
        if(rad < 0)
        {
           GeometryException ge("Invalid radius in setGeocentric: "
                                            + StringUtils::asString(rad));
           GPSTK_THROW(ge);
        }
        theArray[0] = lat;
        theArray[1] = lon;
        theArray[2] = rad;
  
        if(theArray[1] < 0)
           theArray[1] += 360*(1+(unsigned long)(theArray[1]/360));
        else if(theArray[1] >= 360)
           theArray[1] -= 360*(unsigned long)(theArray[1]/360);
        system = Geocentric;
  
        return *this;
     }
  
        // Set the Position given spherical coordinates, system is set to Spherical
        // @param theta angle from the Z-axis (degrees)
        // @param phi angle from the X-axis in the XY plane (degrees)
        // @param rad radius from the center in meters
        // @return a reference to this object.
        // @throw GeometryException on invalid input
     Position& Position::setSpherical(const double theta,
                                      const double phi,
                                      const double rad)
        throw(GeometryException)
     {
        if(theta < 0 || theta > 180)
        {
           GeometryException ge("Invalid theta in setSpherical: "
                                   + StringUtils::asString(theta));
           GPSTK_THROW(ge);
        }
        if(rad < 0)
        {
           GeometryException ge("Invalid radius in setSpherical: "
                                            + StringUtils::asString(rad));
           GPSTK_THROW(ge);
        }
  
        theArray[0] = theta;
        theArray[1] = phi;
        theArray[2] = rad;
  
        if(theArray[1] < 0)
           theArray[1] += 360*(1+(unsigned long)(theArray[1]/360));
        else if(theArray[1] >= 360)
           theArray[1] -= 360*(unsigned long)(theArray[1]/360);
        system = Spherical;
  
        return *this;
     }
  
        // Set the Position given ECEF coordinates, system is set to Cartesian.
        // @param X ECEF X coordinate in meters.
        // @param Y ECEF Y coordinate in meters.
        // @param Z ECEF Z coordinate in meters.
        // @return a reference to this object.
     Position& Position::setECEF(const double X,
                                 const double Y,
                                 const double Z)
        throw()
     {
        theArray[0] = X;
        theArray[1] = Y;
        theArray[2] = Z;
        system = Cartesian;
        return *this;
     }
  
     // ----------- Part 9: member functions: setToString, printf -------------
     //
        // setToString, similar to scanf, this function takes a string and a
        // format describing string in order to define Position
        // values.  The parameters it can take are listed below and
        // described above with the printf() function.
        //
        // The specification must be sufficient to define a Position.
        // The following table lists combinations that give valid
        // Positions. Anything more or other combinations will give
        // unknown (read as: "bad") results so don't try it.  Anything
        // less will throw an exception.
        //
        // @code
        //  %X %Y %Z  (cartesian or ECEF)
        //  %x %y %z  (cartesian or ECEF)
        //  %a %l %r  (geocentric)
        //  %A %L %h  (geodetic)
        //  %t %p %r  (spherical)
        // @endcode
        //
        // So
        // @code
        // pos.setToString("123.4342,9328.1982,-128987.399", "%X,%Y,%Z");
        // @endcode
        //
        // works but
        //
        // @code
        // pos.setToString("123.4342,9328.1982", "%X,%Y");
        // @endcode
        // doesn't work (incomplete specification because it doesn't specify
        // a Position).
        //
        // Whitespace is unimportant here, the function will handle it.
        // The caller must ensure that that the extra characters in
        // the format string (ie '.' ',') are in the same relative
        // location as they are in the actual string, see the example above.
        //
        // @param str string from which to get the Position coordinates
        // @param fmt format to use to parse \c str.
        // @throw GeometryException if \c fmt is an incomplete or invalid specification
        // @throw StringException if an error occurs manipulating the
        // \c str or \c fmt strings.
        // @return a reference to this object.
     Position& Position::setToString(const std::string& str,
                                     const std::string& fmt)
        throw(GeometryException,StringUtils::StringException)
     {
        try {
              // make an object to return (so we don't fiddle with *this
              // until it's necessary)
           Position toReturn;
  
              // flags indicated these defined
           bool hX=false, hY=false, hZ=false;
           bool hglat=false, hlon=false, hht=false;
           bool hclat=false, hrad=false;
           bool htheta=false, hphi=false;
              // store input values
           double x=0.0, y=0.0, z=0.0, glat=0.0, lon=0.0, ht=0.0, clat=0.0,
                  rad=0.0, theta=0.0, phi=0.0;
              // copy format and input string to parse
           string f = fmt;
           string s = str;
           stripLeading(s);
           stripTrailing(f);
  
              // parse strings...  As we process each part, it's removed from both
              // strings so when we reach 0, we're done
           while ( (s.size() > 0) && (f.size() > 0) )
           {
                 // remove everything in f and s up to the first % in f
                 // (these parts of the strings must be identical or this will break
                 // after it tries to remove it!)
              while ( (s.length() != 0) && (f.length() != 0) && (f[0] != '%') )
              {
                    // remove that character now and other whitespace
                 s.erase(0,1);
                 f.erase(0,1);
                 stripLeading(s);
                 stripLeading(f);
              }
  
                 // check just in case we hit the end of either string...
              if ( (s.length() == 0) || (f.length() == 0) )
                 break;
  
                 // lose the '%' in f...
              f.erase(0,1);
  
                 // if the format string is like %03f, get '3' as the field
                 // length.
              string::size_type fieldLength = string::npos;
  
              if (!isalpha(f[0]))
              {
                 fieldLength = asInt(f);
  
                    // remove everything else up to the next character
                    // (in "%03f", that would be 'f')
                 while ((!f.empty()) && (!isalpha(f[0])))
                    f.erase(0,1);
                 if (f.empty())
                    break;
              }
  
                 // finally, get the character that should end this field, if any
              char delimiter = 0;
              if (f.size() > 1)
              {
                 if (f[1] != '%')
                 {
                    delimiter = f[1];
  
                    if (fieldLength == string::npos)
                       fieldLength = s.find(delimiter,0);
                 }
                    // if the there is no delimiter character and the next field
                    // is another part of the time to parse, assume the length
                    // of this field is 1
                 else if (fieldLength == string::npos)
                 {
                    fieldLength = 1;
                 }
              }
  
                 // figure out the next string to be removed.  if there is a field
                 // length, use that first
              string toBeRemoved = s.substr(0, fieldLength);
  
                 // based on char at f[0], we know what to do...
              switch (f[0])
              {
            //%x   X() (meters)
            //%y   Y() (meters)
            //%z   Z() (meters)
            //%X   X()/1000 (kilometers)
            //%Y   Y()/1000 (kilometers)
            //%Z   Z()/1000 (kilometers)
                 case 'X':
                    x = asDouble(toBeRemoved) * 1000;
                    hX = true;
                    break;
                 case 'x':
                    x = asDouble(toBeRemoved);
                    hX = true;
                    break;
                 case 'Y':
                    y = asDouble(toBeRemoved) * 1000;
                    hY = true;
                    break;
                 case 'y':
                    y = asDouble(toBeRemoved);
                    hY = true;
                    break;
                 case 'Z':
                    z = asDouble(toBeRemoved) * 1000;
                    hZ = true;
                    break;
                 case 'z':
                    z = asDouble(toBeRemoved);
                    hZ = true;
                    break;
            //%A   geodeticLatitude() (degrees North)
            //%a   geocentricLatitude() (degrees North)
                 case 'A':
                    glat = asDouble(toBeRemoved);
                    if(glat > 90. || glat < -90.) {
                       InvalidRequest f(
                             "Invalid geodetic latitude for setTostring: "
                             + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    hglat = true;
                    break;
                 case 'a':
                    clat = asDouble(toBeRemoved);
                    if(clat > 90. || clat < -90.) {
                       InvalidRequest f(
                             "Invalid geocentric latitude for setTostring: "
                             + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    hclat = true;
                    break;
            //%L   longitude() (degrees East)
            //%l   longitude() (degrees East)
            //%w   longitude() (degrees West)
            //%W   longitude() (degrees West)
                 case 'L':
                 case 'l':
                    lon = asDouble(toBeRemoved);
                    if(lon < 0)
                       lon += 360*(1+(unsigned long)(lon/360));
                    else if(lon >= 360)
                       lon -= 360*(unsigned long)(lon/360);
                    hlon = true;
                    break;
                 case 'w':
                 case 'W':
                    lon = 360.0 - asDouble(toBeRemoved);
                    if(lon < 0)
                       lon += 360*(1+(unsigned long)(lon/360));
                    else if(lon >= 360)
                       lon -= 360*(unsigned long)(lon/360);
                    hlon = true;
                    break;
            //%t   theta() (degrees)
            //%T   theta() (radians)
                 case 't':
                    theta = asDouble(toBeRemoved);
                    if(theta > 180. || theta < 0.) {
                       InvalidRequest f("Invalid theta for setTostring: "
                                                  + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    htheta = true;
                    break;
                 case 'T':
                    theta = asDouble(toBeRemoved) * RAD_TO_DEG;
                    if(theta > 90. || theta < -90.) {
                       InvalidRequest f("Invalid theta for setTostring: "
                                                  + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    htheta = true;
                    break;
            //%p   phi() (degrees)
            //%P   phi() (radians)
                 case 'p':
                    phi = asDouble(toBeRemoved);
                    if(phi < 0)
                       phi += 360*(1+(unsigned long)(phi/360));
                    else if(phi >= 360)
                       phi -= 360*(unsigned long)(phi/360);
                    hphi = true;
                    break;
                 case 'P':
                    phi = asDouble(toBeRemoved) * RAD_TO_DEG;
                    if(phi < 0)
                       phi += 360*(1+(unsigned long)(phi/360));
                    else if(phi >= 360)
                       phi -= 360*(unsigned long)(phi/360);
                    hphi = true;
                    break;
            //%r   radius() meters
            //%R   radius()/1000 kilometers
            //%h   height() meters
            //%H   height()/1000 kilometers
                 case 'r':
                    rad = asDouble(toBeRemoved);
                    if(rad < 0.0) {
                       InvalidRequest f("Invalid radius for setTostring: "
                                                  + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    hrad = true;
                    break;
                 case 'R':
                    rad = asDouble(toBeRemoved) * 1000;
                    if(rad < 0.0) {
                       InvalidRequest f("Invalid radius for setTostring: "
                                                  + toBeRemoved);
                       GPSTK_THROW(f);
                    }
                    hrad = true;
                    break;
                 case 'h':
                    ht = asDouble(toBeRemoved);
                    hht = true;
                    break;
                 case 'H':
                    ht = asDouble(toBeRemoved) * 1000;
                    hht = true;
                    break;
                 default: // do nothing
                    break;
              }
                 // remove the part of s that we processed
              stripLeading(s,toBeRemoved,1);
  
                 // remove the character we processed from f
              f.erase(0,1);
  
                 // check for whitespace again...
              stripLeading(f);
              stripLeading(s);
  
           }
  
           if ( s.length() != 0  )
           {
                 // throw an error - something didn't get processed in the strings
              InvalidRequest fe(
                 "Processing error - parts of strings left unread - " + s);
              GPSTK_THROW(fe);
           }
  
           if (f.length() != 0)
           {
                 // throw an error - something didn't get processed in the strings
              InvalidRequest fe(
                 "Processing error - parts of strings left unread - " + f);
              GPSTK_THROW(fe);
           }
  
              // throw if the specification is incomplete
           if ( !(hX && hY && hZ) && !(hglat && hlon && hht) &&
                !(hclat && hlon && hrad) && !(htheta && hphi && hrad)) {
              InvalidRequest fe("Incomplete specification for setToString");
              GPSTK_THROW(fe);
           }
  
              // define the Position toReturn
           if(hX && hY && hZ)
              toReturn.setECEF(x,y,z);
           else if(hglat && hlon && hht)
              toReturn.setGeodetic(glat,lon,ht);
           else if(hclat && hlon && hrad)
              toReturn.setGeocentric(clat,lon,rad);
           else if(htheta && hphi && hrad)
              toReturn.setSpherical(theta,phi,rad);
  
           *this = toReturn;
           return *this;
        }
        catch(gpstk::Exception& exc)
        {
           GeometryException ge(exc);
           ge.addText("Failed to convert string to Position");
           GPSTK_THROW(ge);
        }
        catch(std::exception& exc)
        {
           GeometryException ge(exc.what());
           ge.addText("Failed to convert string to Position");
           GPSTK_THROW(ge);
        }
     }
  
        // Format this Position into a string.
        //
        // Generate and return a string containing formatted
        // Position coordinates, formatted by the specification \c fmt.
        //
        // \li \%x   X() (meters)
        // \li \%y   Y() (meters)
        // \li \%z   Z() (meters)
        // \li \%X   X()/1000 (kilometers)
        // \li \%Y   Y()/1000 (kilometers)
        // \li \%Z   Z()/1000 (kilometers)
        // \li \%A   geodeticLatitude() (degrees North)
        // \li \%a   geocentricLatitude() (degrees North)
        // \li \%L   longitude() (degrees East)
        // \li \%l   longitude() (degrees East)
        // \li \%w   longitude() (degrees West)
        // \li \%W   longitude() (degrees West)
        // \li \%t   theta() (degrees)
        // \li \%T   theta() (radians)
        // \li \%p   phi() (degrees)
        // \li \%P   phi() (radians)
        // \li \%r   radius() meters
        // \li \%R   radius()/1000 kilometers
        // \li \%h   height() meters
        // \li \%H   height()/1000 kilometers
        //
        // @param fmt format to use for this time.
        // @return a string containing this Position in the
        // representation specified by \c fmt.
     std::string Position::printf(const char *fmt) const
        throw(StringUtils::StringException)
     {
        string rv = fmt;
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?x"),
                            string("xf"), X());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?y"),
                            string("yf"), Y());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?z"),
                            string("zf"), Z());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?X"),
                            string("Xf"), X()/1000);
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?Y"),
                            string("Yf"), Y()/1000);
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?Z"),
                            string("Zf"), Z()/1000);
  
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?A"),
                            string("Af"), geodeticLatitude());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?a"),
                            string("af"), geocentricLatitude());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?L"),
                            string("Lf"), longitude());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?l"),
                            string("lf"), longitude());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?w"),
                            string("wf"), 360-longitude());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?W"),
                            string("Wf"), 360-longitude());
  
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?t"),
                            string("tf"), theta());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?T"),
                            string("Tf"), theta()*DEG_TO_RAD);
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?p"),
                            string("pf"), phi());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?P"),
                            string("Pf"), phi()*DEG_TO_RAD);
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?r"),
                            string("rf"), radius());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?R"),
                            string("Rf"), radius()/1000);
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?h"),
                            string("hf"), height());
        rv = formattedPrint(rv, string("%[ 0-]?[[:digit:]]*(\\.[[:digit:]]+)?H"),
                            string("Hf"), height()/1000);
        return rv;
     }
  
        // Returns the string that operator<<() would print.
     string Position::asString() const
        throw(StringUtils::StringException)
     {
        ostringstream o;
        o << *this;
        return o.str();
     }
  
     // ----------- Part 10: functions: fundamental conversions ---------------
     //
        // Fundamental conversion from spherical to cartesian coordinates.
        // @param trp (input): theta, phi, radius
        // @param xyz (output): X,Y,Z in units of radius
        // Algorithm references: standard geometry.
     void Position::convertSphericalToCartesian(const Triple& tpr,
                                                Triple& xyz)
        throw()
     {
        double st=::sin(tpr[0]*DEG_TO_RAD);
        xyz[0] = tpr[2]*st*::cos(tpr[1]*DEG_TO_RAD);
        xyz[1] = tpr[2]*st*::sin(tpr[1]*DEG_TO_RAD);
        xyz[2] = tpr[2]*::cos(tpr[0]*DEG_TO_RAD);
     }
  
        // Fundamental routine to convert cartesian to spherical coordinates.
        // @param xyz (input): X,Y,Z
        // @param trp (output): theta, phi (deg), radius in units of input
        // Algorithm references: standard geometry.
     void Position::convertCartesianToSpherical(const Triple& xyz,
                                                Triple& tpr)
        throw()
     {
        tpr[2] = RSS(xyz[0],xyz[1],xyz[2]);
        if(tpr[2] <= Position::POSITION_TOLERANCE/5) { // zero-length Cartesian vector
           tpr[0] = 90;
           tpr[1] = 0;
           return;
        }
        tpr[0] = ::acos(xyz[2]/tpr[2]);
        tpr[0] *= RAD_TO_DEG;
        if(RSS(xyz[0],xyz[1]) < Position::POSITION_TOLERANCE/5) {       // pole
           tpr[1] = 0;
           return;
        }
        tpr[1] = ::atan2(xyz[1],xyz[0]);
        tpr[1] *= RAD_TO_DEG;
        if(tpr[1] < 0) tpr[1] += 360;
     }
  
        // Fundamental routine to convert cartesian (ECEF) to geodetic coordinates,
        // (Geoid specified by semi-major axis and eccentricity squared).
        // @param xyz (input): X,Y,Z in meters
        // @param llh (output): geodetic lat(deg N), lon(deg E),
        //                             height above ellipsoid (meters)
        // @param A (input) Earth semi-major axis
        // @param eccSq (input) square of Earth eccentricity
        // Algorithm references:
     void Position::convertCartesianToGeodetic(const Triple& xyz,
                                               Triple& llh,
                                               const double A,
                                               const double eccSq)
        throw()
     {
        double p,slat,N,htold,latold;
        p = SQRT(xyz[0]*xyz[0]+xyz[1]*xyz[1]);
        if(p < Position::POSITION_TOLERANCE/5) {  // pole or origin
           llh[0] = (xyz[2] > 0 ? 90.0: -90.0);
           llh[1] = 0;                            // lon undefined, really
           llh[2] = ::fabs(xyz[2]) - A*SQRT(1.0-eccSq);
           return;
        }
        llh[0] = ::atan2(xyz[2], p*(1.0-eccSq));
        llh[2] = 0;
        for(int i=0; i<5; i++) {
           slat = ::sin(llh[0]);
           N = A / SQRT(1.0 - eccSq*slat*slat);
           htold = llh[2];
           llh[2] = p/::cos(llh[0]) - N;
           latold = llh[0];
           llh[0] = ::atan2(xyz[2], p*(1.0-eccSq*(N/(N+llh[2]))));
           if(::fabs(llh[0]-latold) < 1.0e-9 && fabs(llh[2]-htold) < 1.0e-9 * A) break;
        }
        llh[1] = ::atan2(xyz[1],xyz[0]);
        if(llh[1] < 0.0) llh[1] += TWO_PI;
        llh[0] *= RAD_TO_DEG;
        llh[1] *= RAD_TO_DEG;
     }
  
        // Fundamental routine to convert geodetic to cartesian (ECEF) coordinates,
        // (Geoid specified by semi-major axis and eccentricity squared).
        // @param llh (input): geodetic lat(deg N), lon(deg E),
        //            height above ellipsoid (meters)
        // @param xyz (output): X,Y,Z in meters
        // @param A (input) Earth semi-major axis
        // @param eccSq (input) square of Earth eccentricity
        // Algorithm references:
     void Position::convertGeodeticToCartesian(const Triple& llh,
                                               Triple& xyz,
                                               const double A,
                                               const double eccSq)
        throw()
     {
        double slat = ::sin(llh[0]*DEG_TO_RAD);
        double clat = ::cos(llh[0]*DEG_TO_RAD);
        double N = A/SQRT(1.0-eccSq*slat*slat);
        xyz[0] = (N+llh[2])*clat*::cos(llh[1]*DEG_TO_RAD);
        xyz[1] = (N+llh[2])*clat*::sin(llh[1]*DEG_TO_RAD);
        xyz[2] = (N*(1.0-eccSq)+llh[2])*slat;
     }
  
        // Fundamental routine to convert cartesian (ECEF) to geocentric coordinates.
        // @param xyz (input): X,Y,Z in meters
        // @param llr (output):
        //            geocentric lat(deg N),lon(deg E),radius (units of input)
     void Position::convertCartesianToGeocentric(const Triple& xyz,
                                                 Triple& llr)
        throw()
     {
        convertCartesianToSpherical(xyz, llr);
        llr[0] = 90 - llr[0];         // convert theta to latitude
     }
  
        // Fundamental routine to convert geocentric to cartesian (ECEF) coordinates.
        // @param llr (input): geocentric lat(deg N),lon(deg E),radius
        // @param xyz (output): X,Y,Z (units of radius)
     void Position::convertGeocentricToCartesian(const Triple& llr,
                                                 Triple& xyz)
        throw()
     {
        Triple llh(llr);
        llh[0] = 90 - llh[0];         // convert latitude to theta
        convertSphericalToCartesian(llh, xyz);
     }
  
        // Fundamental routine to convert geocentric to geodetic coordinates.
        // @param llr (input): geocentric Triple: lat(deg N),lon(deg E),radius (meters)
        // @param llh (output): geodetic latitude (deg N),
        //            longitude (deg E), and height above ellipsoid (meters)
        // @param A (input) Earth semi-major axis
        // @param eccSq (input) square of Earth eccentricity
     void Position::convertGeocentricToGeodetic(const Triple& llr,
                                                 Triple& llh,
                                                 const double A,
                                                 const double eccSq)
        throw()
     {
        double cl,p,sl,slat,N,htold,latold;
        llh[1] = llr[1];   // longitude is unchanged
        cl = ::sin((90-llr[0])*DEG_TO_RAD);
        sl = ::cos((90-llr[0])*DEG_TO_RAD);
        if(llr[2] <= Position::POSITION_TOLERANCE/5) {
           // radius is below tolerance, hence assign zero-length
           // arbitrarily set latitude = longitude = 0
           llh[0] = llh[1] = 0;
           llh[2] = -A;
           return;
        }
        else if(cl < 1.e-10) {
           // near pole ... note that 1mm/radius(Earth) = 1.5e-10
           if(llr[0] < 0) llh[0] = -90;
           else           llh[0] =  90;
           llh[1] = 0;
           llh[2] = llr[2] - A*SQRT(1-eccSq);
           return;
        }
        llh[0] = ::atan2(sl, cl*(1.0-eccSq));
        p = cl*llr[2];
        llh[2] = 0;
        for(int i=0; i<5; i++) {
           slat = ::sin(llh[0]);
           N = A / SQRT(1.0 - eccSq*slat*slat);
           htold = llh[2];
           llh[2] = p/::cos(llh[0]) - N;
           latold = llh[0];
           llh[0] = ::atan2(sl, cl*(1.0-eccSq*(N/(N+llh[2]))));
           if(fabs(llh[0]-latold) < 1.0e-9 && ::fabs(llh[2]-htold) < 1.0e-9 * A) break;
        }
        llh[0] *= RAD_TO_DEG;
     }
  
        // Fundamental routine to convert geodetic to geocentric coordinates.
        // @param geodeticllh (input): geodetic latitude (deg N),
        //            longitude (deg E), and height above ellipsoid (meters)
        // @param llr (output): geocentric lat (deg N),lon (deg E),radius (meters)
        // @param A (input) Earth semi-major axis
        // @param eccSq (input) square of Earth eccentricity
     void Position::convertGeodeticToGeocentric(const Triple& llh,
                                                Triple& llr,
                                                const double A,
                                                const double eccSq)
        throw()
     {
        double slat = ::sin(llh[0]*DEG_TO_RAD);
        double N = A/SQRT(1.0-eccSq*slat*slat);
        // longitude is unchanged
        llr[1] = llh[1];
        // radius
        llr[2] = SQRT((N+llh[2])*(N+llh[2]) + N*eccSq*(N*eccSq-2*(N+llh[2]))*slat*slat);
        if(llr[2] <= Position::POSITION_TOLERANCE/5) {
           // radius is below tolerance, hence assign zero-length
           // arbitrarily set latitude = longitude = 0
           llr[0] = llr[1] = llr[2] = 0;
           return;
        }
        if(1-::fabs(slat) < 1.e-10) {             // at the pole
           if(slat < 0) llr[0] = -90;
           else         llr[0] =  90;
           llr[1] = 0.0;
           return;
        }
        // theta
        llr[0] = ::acos((N*(1-eccSq)+llh[2])*slat/llr[2]);
        llr[0] *= RAD_TO_DEG;
        llr[0] = 90 - llr[0];
     }
  
     // ----------- Part 11: operator<< and other useful functions -------------
     //
       // Stream output for Position objects.
       // @param s stream to append formatted Position to.
       // @param t Position to append to stream \c s.
       // @return reference to \c s.
     ostream& operator<<(ostream& s, const Position& p)
     {
        if(p.system == Position::Cartesian)
           s << p.printf("%.4x m %.4y m %.4z m");
        else if(p.system == Position::Geodetic)
           s << p.printf("%.8A degN %.8L degE %.4h m");
        else if(p.system == Position::Geocentric)
           s << p.printf("%.8a degN %.8L degE %.4r m");
        else if(p.system == Position::Spherical)
           s << p.printf("%.8t deg %.8p deg %.4r m");
        else
           s << " Unknown system! : " << p[0] << " " << p[1] << " " << p[2];
  
        return s;
     }
  
        // Compute the range in meters between this Position and
        // the Position passed as input.
        // @param right Position to which to find the range
        // @return the range (in meters)
        // @throw GeometryException if ell values differ
     double range(const Position& A,
                  const Position& B)
        throw(GeometryException)
     {
        if(A.AEarth != B.AEarth || A.eccSquared != B.eccSquared)
        {
           GeometryException ge("Unequal geoids");
           GPSTK_THROW(ge);
        }
  
           Position L(A),R(B);
           L.transformTo(Position::Cartesian);
           R.transformTo(Position::Cartesian);
           double dif = RSS(L.X()-R.X(),L.Y()-R.Y(),L.Z()-R.Z());
           return dif;
        }
  
       // Compute the radius of the ellipsoidal Earth, given the geodetic latitude.
       // @param geolat geodetic latitude in degrees
       // @return the Earth radius (in meters)
     double Position::radiusEarth(const double geolat,
                                  const double A,
                                  const double eccSq)
        throw()
     {
        double slat=::sin(DEG_TO_RAD*geolat);
        double e=(1.0-eccSq);
        double f=(1.0+(e*e-1.0)*slat*slat)/(1.0-eccSq*slat*slat);
        return (A * SQRT(f));
     }
  
        // A member function that computes the elevation of the input
        // (Target) position as seen from this Position.
        // @param Target the Position which is observed to have the
        //        computed elevation, as seen from this Position.
        // @return the elevation in degrees
     double Position::elevation(const Position& Target) const
        throw(GeometryException)
     {
        Position R(*this),S(Target);
        R.transformTo(Cartesian);
        S.transformTo(Cartesian);
        // use Triple:: functions in cartesian coordinates (only)
        double elevation;
        try {
           elevation = R.elvAngle(S);
        }
        catch(GeometryException& ge)
        {
           GPSTK_RETHROW(ge);
        }
        return elevation;
     }
  
        // A member function that computes the elevation of the input
        // (Target) position as seen from this Position, using a Geodetic
        // (i.e. ellipsoidal) system.
        // @param Target the Position which is observed to have the
        //        computed elevation, as seen from this Position.
        // @return the elevation in degrees
     double Position::elevationGeodetic(const Position& Target) const
        throw(GeometryException)
     {
        Position R(*this),S(Target);
        double latGeodetic = R.getGeodeticLatitude()*DEG_TO_RAD;
        double longGeodetic = R.getLongitude()*DEG_TO_RAD;
        double localUp;
        double cosUp;
        R.transformTo(Cartesian);
        S.transformTo(Cartesian);
        Triple z;
        // Let's get the slant vector
        z = S.theArray - R.theArray;
  
        if (z.mag()<=1e-4) // if the positions are within .1 millimeter
        {
           GeometryException ge("Positions are within .1 millimeter");
           GPSTK_THROW(ge);
        }
  
        // Compute k vector in local North-East-Up (NEU) system
        Triple kVector(::cos(latGeodetic)*::cos(longGeodetic), ::cos(latGeodetic)*::sin(longGeodetic), ::sin(latGeodetic));
        // Take advantage of dot method to get Up coordinate in local NEU system
        localUp = z.dot(kVector);
        // Let's get cos(z), being z the angle with respect to local vertical (Up);
        cosUp = localUp/z.mag();
  
        return 90.0 - ((::acos(cosUp))*RAD_TO_DEG);
     }
  
        // A member function that computes the azimuth of the input
        // (Target) position as seen from this Position.
        // @param Target the Position which is observed to have the
        //        computed azimuth, as seen from this Position.
        // @return the azimuth in degrees
     double Position::azimuth(const Position& Target) const
        throw(GeometryException)
     {
        Position R(*this),S(Target);
        R.transformTo(Cartesian);
        S.transformTo(Cartesian);
        // use Triple:: functions in cartesian coordinates (only)
        double az;
        try
        {
           az = R.azAngle(S);
  
        }
        catch(GeometryException& ge)
        {
           GPSTK_RETHROW(ge);
        }
  
        return az;
     }
  
        // A member function that computes the azimuth of the input
        // (Target) position as seen from this Position, using a Geodetic
        // (i.e. ellipsoidal) system.
        // @param Target the Position which is observed to have the
        //        computed azimuth, as seen from this Position.
        // @return the azimuth in degrees
     double Position::azimuthGeodetic(const Position& Target) const
        throw(GeometryException)
     {
        Position R(*this),S(Target);
        double latGeodetic = R.getGeodeticLatitude()*DEG_TO_RAD;
        double longGeodetic = R.getLongitude()*DEG_TO_RAD;
        double localN, localE;
        R.transformTo(Cartesian);
        S.transformTo(Cartesian);
        Triple z;
        // Let's get the slant vector
        z = S.theArray - R.theArray;
  
        if (z.mag()<=1e-4) // if the positions are within .1 millimeter
        {
           GeometryException ge("Positions are within .1 millimeter");
           GPSTK_THROW(ge);
        }
  
        // Compute i vector in local North-East-Up (NEU) system
        Triple iVector(-::sin(latGeodetic)*::cos(longGeodetic), -::sin(latGeodetic)*::sin(longGeodetic), ::cos(latGeodetic));
        // Compute j vector in local North-East-Up (NEU) system
        Triple jVector(-::sin(longGeodetic), ::cos(longGeodetic), 0);
  
        // Now, let's use dot product to get localN and localE unitary vectors
        localN = (z.dot(iVector))/z.mag();
        localE = (z.dot(jVector))/z.mag();
  
        // Let's test if computing azimuth has any sense
        double test = fabs(localN) + fabs(localE);
  
        // Warning: If elevation is very close to 90 degrees, we will return azimuth = 0.0
        if (test < 1.0e-16) return 0.0;
  
        double alpha = ((::atan2(localE, localN)) * RAD_TO_DEG);
        if (alpha < 0.0)
        {
           return alpha + 360.0;
        }
        else
        {
           return alpha;
        }
     }
  
       // A member function that computes the point at which a signal, which
       // is received at *this Position and there is observed at the input
       // azimuth and elevation, crosses a model ionosphere that is taken to
       // be a uniform thin shell at the input height. This algorithm is done
       // in geocentric coordinates.
       // A member function that computes the point at which a signal, which
       // is received at *this Position and there is observed at the input
       // azimuth and elevation, crosses a model ionosphere that is taken to
       // be a uniform thin shell at the input height. This algorithm is done
       // in geocentric coordinates.
       // @param elev elevation angle of the signal at reception, in degrees
       // @param azim azimuth angle of the signal at reception, in degrees
       // @param ionoht height of the ionosphere, in meters
       // @return Position IPP the position of the ionospheric pierce point,
       //     in the same coordinate system as *this; *this is not modified.
     Position Position::getIonosphericPiercePoint(const double elev,
                                                  const double azim,
                                                  const double ionoht) const
        throw()
     {
        Position Rx(*this);
  
        // convert to Geocentric
        Rx.transformTo(Geocentric);
  
        // compute the geographic pierce point
        Position IPP(Rx);                   // copy system and geoid
        double el = elev * DEG_TO_RAD;
        // p is the angle subtended at Earth center by Rx and the IPP
        double p = PI/2.0 - el - ::asin(AEarth*::cos(el)/(AEarth+ionoht));
        double lat = Rx.theArray[0] * DEG_TO_RAD;
        double az = azim * DEG_TO_RAD;
        IPP.theArray[0] = std::asin(std::sin(lat)*std::cos(p) + std::cos(lat)*std::sin(p)*std::cos(az));
        IPP.theArray[1] = Rx.theArray[1]*DEG_TO_RAD
           + ::asin(::sin(p)*::sin(az)/::cos(IPP.theArray[0]));
  
        IPP.theArray[0] *= RAD_TO_DEG;
        IPP.theArray[1] *= RAD_TO_DEG;
        IPP.theArray[2] = AEarth + ionoht;
  
        // transform back
        IPP.transformTo(system);
  
        return IPP;
     }
  
  
          /**
          * A member function that computes the radius of curvature of the 
          * meridian (Rm) corresponding to this Position.
          * @return radius of curvature of the meridian (in meters)
          */
      double Position::getCurvMeridian() const
          throw()
      {
      
          double slat = ::sin(geodeticLatitude()*DEG_TO_RAD);
          double W = 1.0/SQRT(1.0-eccSquared*slat*slat);
          
          return AEarth*(1.0-eccSquared)*W*W*W;
          
      }
  
          /**
          * A member function that computes the radius of curvature in the 
          * prime vertical (Rn) corresponding to this Position.
          * @return radius of curvature in the prime vertical (in meters)
          */
      double Position::getCurvPrimeVertical() const
          throw()
      {
      
          double slat = ::sin(geodeticLatitude()*DEG_TO_RAD);
          
          return AEarth/SQRT(1.0-eccSquared*slat*slat);
          
      }
  
     // ----------- Part 12: private functions and member data -----------------
     //
        // Initialization function, used by the constructors.
        // @param a coordinate [ X(m), or latitude (degrees N) ]
        // @param b coordinate [ Y(m), or longitude (degrees E) ]
        // @param c coordinate [ Z, height above ellipsoid or radius, in m ]
        // @param s CoordinateSystem, defaults to Cartesian
        // @param geiod pointer to a GeoidModel, default NULL (WGS84)
        // @throw GeometryException on invalid input.
     void Position::initialize(const double a,
                    const double b,
                    const double c,
                    Position::CoordinateSystem s,
                    EllipsoidModel *ell,
                    ReferenceFrame frame)
        throw(GeometryException)
     {
        double bb(b);
        if(s == Geodetic || s==Geocentric)
        {
           if(a > 90 || a < -90)
           {
              GeometryException ge("Invalid latitude in constructor: "
                                      + StringUtils::asString(a));
              GPSTK_THROW(ge);
           }
           if(bb < 0)
              bb += 360*(1+(unsigned long)(bb/360));
           else if(bb >= 360)
              bb -= 360*(unsigned long)(bb/360);
        }
        if(s==Geocentric || s==Spherical)
        {
           if(c < 0)
           {
              GeometryException ge("Invalid radius in constructor: "
                                             + StringUtils::asString(c));
              GPSTK_THROW(ge);
           }
        }
        if(s==Spherical)
        {
           if(a < 0 || a > 180)
           {
              GeometryException ge("Invalid theta in constructor: "
                                      + StringUtils::asString(a));
              GPSTK_THROW(ge);
           }
           if(bb < 0)
              bb += 360*(1+(unsigned long)(bb/360));
           else if(bb >= 360)
              bb -= 360*(unsigned long)(bb/360);
        }
  
        theArray[0] = a;
        theArray[1] = bb;
        theArray[2] = c;
  
        if(ell) {
           AEarth = ell->a();
           eccSquared = ell->eccSquared();
        }
        else {
           WGS84Ellipsoid WGS84;
           AEarth = WGS84.a();
           eccSquared = WGS84.eccSquared();
        }
        system = s;
        tolerance = POSITION_TOLERANCE;
        refFrame = frame;
     }
  
  }  // namespace gpstk
  