SurfacePoint.cpp

#include "SurfacePoint.h"

#include <SpiceUsr.h>

#include "IException.h"
#include "IString.h"
#include "Latitude.h"
#include "Longitude.h"

using namespace boost::numeric::ublas;
using namespace std;

namespace Isis {

  SurfacePoint::SurfacePoint() {
    InitCovariance();
    InitPoint();
    InitRadii();
  }

  SurfacePoint::SurfacePoint(const SurfacePoint &other) {
    if(other.p_majorAxis) {
      p_majorAxis = new Distance(*other.p_majorAxis);
    }
    else {
      p_majorAxis = NULL;
    }

    if(other.p_minorAxis) {
      p_minorAxis = new Distance(*other.p_minorAxis);
    }
    else {
      p_minorAxis = NULL;
    }

    if(other.p_polarAxis) {
      p_polarAxis = new Distance(*other.p_polarAxis);
    }
    else {
      p_polarAxis = NULL;
    }

    if(other.p_x) {
      p_x = new Displacement(*other.p_x);
    }
    else {
      p_x = NULL;
    }

    if(other.p_y) {
      p_y = new Displacement(*other.p_y);
    }
    else {
      p_y = NULL;
    }

    if(other.p_z) {
      p_z = new Displacement(*other.p_z);
    }
    else {
      p_z = NULL;
    }

    if(other.p_rectCovar) {
      p_rectCovar = new symmetric_matrix<double, upper>(*other.p_rectCovar);
    }
    else {
      p_rectCovar = NULL;
    }

    if(other.p_sphereCovar) {
      p_sphereCovar = new symmetric_matrix<double, upper>(*other.p_sphereCovar);
    }
    else {
      p_sphereCovar = NULL;
    }
  }


  SurfacePoint::SurfacePoint(const Latitude &lat, const Longitude &lon,
      const Distance &radius) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetSphericalPoint(lat, lon, radius);
  }



  SurfacePoint::SurfacePoint(const Latitude &lat, const Longitude &lon,
      const Distance &radius, const Angle &latSigma, const Angle &lonSigma,
      const Distance &radiusSigma) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetSpherical(lat, lon, radius, latSigma, lonSigma, radiusSigma);
  }


  SurfacePoint::SurfacePoint(const Latitude &lat, const Longitude &lon,
      const Distance &radius, const symmetric_matrix<double, upper> &covar) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetSpherical(lat, lon, radius, covar);
  }


  SurfacePoint::SurfacePoint(const Displacement &x, const Displacement &y,
      const Displacement &z) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetRectangular(x, y, z);
  }


  SurfacePoint::SurfacePoint(const Displacement &x, const Displacement &y,
      const Displacement &z, const Distance &xSigma, const Distance &ySigma,
      const Distance &zSigma) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetRectangular(x, y, z, xSigma, ySigma, zSigma);
  }


  SurfacePoint::SurfacePoint(const Displacement &x, const Displacement &y,
      const Displacement &z, const symmetric_matrix<double, upper> &covar) {
    InitCovariance();
    InitPoint();
    InitRadii();
    SetRectangular(x, y, z, covar);
  }


  SurfacePoint::~SurfacePoint() {
    FreeAllocatedMemory();
  }


  void SurfacePoint::InitCovariance() {
    p_rectCovar = NULL;
    p_sphereCovar = NULL;
  }


  void SurfacePoint::InitPoint() {
    p_x = NULL;
    p_y = NULL;
    p_z = NULL;
  }

  void SurfacePoint::InitRadii() {
    p_majorAxis = NULL;
    p_minorAxis = NULL;
    p_polarAxis = NULL;
  }


  void SurfacePoint::SetRectangularPoint(const Displacement &x,
      const Displacement &y, const Displacement &z) {

    if (!x.isValid() || !y.isValid() || !z.isValid()) {
      IString msg = "x, y, and z must be set to valid displacements.  One or "
        "more coordinates have been set to an invalid displacement.";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    if(p_x) {
      *p_x = x;
    }
    else {
      p_x = new Displacement(x);
    }

    if(p_y) {
      *p_y = y;
    }
    else {
      p_y = new Displacement(y);
    }

    if(p_z) {
      *p_z = z;
    }
    else {
      p_z = new Displacement(z);
    }
  }


  void SurfacePoint::SetRectangular(const Displacement &x,
      const Displacement &y, const Displacement &z, const Distance &xSigma,
      const Distance &ySigma, const Distance &zSigma) {
    SetRectangularPoint(x, y, z);

    if (xSigma.isValid() && ySigma.isValid() && zSigma.isValid())
      SetRectangularSigmas(xSigma, ySigma, zSigma);
  }


  void SurfacePoint::SetRectangular(Displacement x, Displacement y, Displacement z,
                                    const symmetric_matrix<double,upper>& covar) {
    SetRectangularPoint(x, y, z);
    SetRectangularMatrix(covar);
  }


  void SurfacePoint::SetRectangularSigmas(const Distance &xSigma,
                                          const Distance &ySigma,
                                          const Distance &zSigma) {
    // Is this error checking necessary or should we just use Distance?????
    if (!xSigma.isValid() || !ySigma.isValid() || !zSigma.isValid()) {
      IString msg = "x sigma, y sigma , and z sigma must be set to valid "
        "distances.  One or more sigmas have been set to an invalid distance.";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    symmetric_matrix<double,upper> covar(3);
    covar.clear();
    covar(0,0) = xSigma.meters() * xSigma.meters();
    covar(1,1) = ySigma.meters() * ySigma.meters();
    covar(2,2) = zSigma.meters() * zSigma.meters();
    SetRectangularMatrix(covar);
  }


  void SurfacePoint::SetRectangularMatrix(
       const symmetric_matrix<double, upper> &covar) {
    // Make sure the point is set first
    if (!Valid()) {
      IString msg = "A point must be set before a variance/covariance matrix "
        "can be set.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if(p_rectCovar) {
      *p_rectCovar = covar;
    }
    else {
      p_rectCovar = new symmetric_matrix<double, upper>(covar);
    }

    SpiceDouble rectMat[3][3];

    // Compute the local radius of the surface point
    double x2  = p_x->meters() * p_x->meters();
    double y2  = p_y->meters() * p_y->meters();
    double z   = p_z->meters();
    double radius = sqrt(x2 + y2 + z*z);

    // Should we use a matrix utility?
    rectMat[0][0] = covar(0,0);
    rectMat[0][1] = rectMat[1][0] = covar(0,1);
    rectMat[0][2] = rectMat[2][0] = covar(0,2);
    rectMat[1][1] = covar(1,1);
    rectMat[1][2] = rectMat[2][1] = covar(1,2);
    rectMat[2][2] = covar(2,2);

    // Compute the Jacobian
    SpiceDouble J[3][3];
    double zOverR = p_z->meters() / radius;
    double r2 = radius*radius;
    double denom = r2*radius*sqrt(1.0 - (zOverR*zOverR));
    J[0][0] = -p_x->meters() * p_z->meters() / denom;
    J[0][1] = -p_y->meters() * p_z->meters() / denom;
    J[0][2] = (r2 - p_z->meters() * p_z->meters()) / denom;
    J[1][0] = -p_y->meters() / (x2 + y2);
    J[1][1] = p_x->meters() / (x2 + y2);
    J[1][2] = 0.0;
    J[2][0] = p_x->meters() / radius;
    J[2][1] = p_y->meters() / radius;
    J[2][2] = p_z->meters() / radius;

    if(!p_sphereCovar)
      p_sphereCovar = new symmetric_matrix<double, upper>(3);

    SpiceDouble mat[3][3];
    mxm_c (J, rectMat, mat);
    mxmt_c (mat, J, mat);
    (*p_sphereCovar)(0,0) = mat[0][0];
    (*p_sphereCovar)(0,1) = mat[0][1];
    (*p_sphereCovar)(0,2) = mat[0][2];
    (*p_sphereCovar)(1,1) = mat[1][1];
    (*p_sphereCovar)(1,2) = mat[1][2];
    (*p_sphereCovar)(2,2) = mat[2][2];
  }


  void SurfacePoint::SetSphericalPoint(const Latitude  &lat,
                                       const Longitude &lon,
                                       const Distance  &radius) {
// Is error checking necessary or does Latitude, Longitude, and Distance handle it?????
    if (!lat.isValid()  ||  !lon.isValid()  ||  !radius.isValid()) {
      IString msg = "Latitude, longitude, or radius is an invalid value.";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    SpiceDouble dlat = (double) lat.radians();
    SpiceDouble dlon = (double) lon.radians();
    SpiceDouble dradius = radius.kilometers();

    SpiceDouble rect[3];
    latrec_c ( dradius, dlon, dlat, rect);

    SetRectangularPoint(Displacement(rect[0], Displacement::Kilometers),
                        Displacement(rect[1], Displacement::Kilometers),
                        Displacement(rect[2], Displacement::Kilometers));
  }


  void SurfacePoint::SetSpherical(const Latitude &lat, const Longitude &lon,
      const Distance &radius, const Angle &latSigma, const Angle &lonSigma,
      const Distance &radiusSigma) {
    SetSphericalPoint(lat, lon, radius);

    if (latSigma.isValid() && lonSigma.isValid() && radiusSigma.isValid())
      SetSphericalSigmas(latSigma, lonSigma, radiusSigma);
  }


  void SurfacePoint::SetSpherical(const Latitude &lat, const Longitude &lon,
      const Distance &radius, const symmetric_matrix<double, upper> &covar) {
    SetSphericalPoint(lat, lon, radius);
    SetSphericalMatrix(covar);
  }


  void SurfacePoint::SetSphericalCoordinates(const Latitude &lat,
                                                const Longitude &lon, const Distance &radius) {

      SetSphericalPoint(lat, lon, radius);
  }


  void SurfacePoint::SetSphericalSigmas(const Angle &latSigma,
                                        const Angle &lonSigma,
                                        const Distance &radiusSigma) {
    if (latSigma.isValid() && lonSigma.isValid() && radiusSigma.isValid()) {
      symmetric_matrix<double,upper> covar(3);
      covar.clear();

      double sphericalCoordinate;
      sphericalCoordinate = (double) latSigma.radians();
      covar(0,0) =  sphericalCoordinate*sphericalCoordinate;
      sphericalCoordinate = (double) lonSigma.radians();
      covar(1,1) = sphericalCoordinate*sphericalCoordinate;
      sphericalCoordinate = (double) radiusSigma.meters();
      covar(2,2) = sphericalCoordinate*sphericalCoordinate;

      SetSphericalMatrix(covar);
    }
    else {
      delete p_sphereCovar;
      p_sphereCovar = NULL;

      delete p_rectCovar;
      p_rectCovar = NULL;
    }
  }


  void SurfacePoint::SetSphericalSigmasDistance(const Distance &latSigma,
    const Distance &lonSigma, const Distance &radiusSigma) {

    if (!p_majorAxis || !p_minorAxis || !p_polarAxis || !p_majorAxis->isValid() ||
        !p_minorAxis->isValid() || !p_polarAxis->isValid()) {
      IString msg = "In order to use sigmas in meter units, the equitorial "
        "radius must be set with a call to SetRadii or an appropriate "
        "constructor";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if (!Valid()) {
      IString msg = "Cannot set spherical sigmas on an invalid surface point";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    double scaledLatSig = latSigma / *p_majorAxis;
    double scaledLonSig = lonSigma * cos((double)GetLatitude().radians())
                                   / *p_majorAxis;
    SetSphericalSigmas( Angle(scaledLatSig, Angle::Radians),
                        Angle(scaledLonSig, Angle::Radians), radiusSigma);
  }


  void SurfacePoint::SetSphericalMatrix(
     const symmetric_matrix<double, upper> & covar) {

    // Make sure the point is set first
    if (!Valid()) {
      IString msg = "A point must be set before a variance/covariance matrix "
        "can be set.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if(p_sphereCovar) {
      *p_sphereCovar = covar;
    }
    else {
      p_sphereCovar = new symmetric_matrix<double, upper>(covar);
    }

    SpiceDouble sphereMat[3][3];

    sphereMat[0][0] = covar(0,0);
    sphereMat[0][1] = sphereMat[1][0] = covar(0,1);
    sphereMat[0][2] = sphereMat[2][0] = covar(0,2);
    sphereMat[1][1] = covar(1,1);
    sphereMat[1][2] = sphereMat[2][1] = covar(1,2);
    sphereMat[2][2] = covar(2,2);

//     std::cout<<"Ocovar = "<<sphereMat[0][0]<<" "<<sphereMat[0][1]<<" "<<sphereMat[0][2]<<std::endl
//              <<"         "<<sphereMat[1][0]<<" "<<sphereMat[1][1]<<" "<<sphereMat[1][2]<<std::endl
//              <<"         "<<sphereMat[2][0]<<" "<<sphereMat[2][1]<<" "<<sphereMat[2][2]<<std::endl;

    // Get the lat/lon/radius of the point
    double lat = (double) GetLatitude().radians();
    double lon = (double) GetLongitude().radians();
    double radius = (double) GetLocalRadius().meters();

    // Compute the Jacobian
    SpiceDouble J[3][3];
    double cosPhi = cos(lat);
    double sinPhi = sin(lat);
    double cosLamda = cos(lon);
    double sinLamda = sin(lon);
    double rcosPhi = radius*cosPhi;
    double rsinPhi = radius*sinPhi;
    J[0][0] = -rsinPhi * cosLamda;
    J[0][1] = -rcosPhi * sinLamda;
    J[0][2] = cosPhi * cosLamda;
    J[1][0] = -rsinPhi * sinLamda;
    J[1][1] = rcosPhi * cosLamda;
    J[1][2] = cosPhi * sinLamda;
    J[2][0] = rcosPhi;
    J[2][1] = 0.0;
    J[2][2] = sinPhi;

    if(!p_rectCovar)
      p_rectCovar = new symmetric_matrix<double, upper>(3);

    SpiceDouble mat[3][3];
    mxm_c (J, sphereMat, mat);
    mxmt_c (mat, J, mat);
    //  TODO  Test to see if only the upper triangular portion of the matrix needs to be set
    (*p_rectCovar)(0,0) = mat[0][0];
    (*p_rectCovar)(0,1) = mat[0][1];
    (*p_rectCovar)(0,2) = mat[0][2];
    (*p_rectCovar)(1,1) = mat[1][1];
    (*p_rectCovar)(1,2) = mat[1][2];
    (*p_rectCovar)(2,2) = mat[2][2];

//     std::cout<<"Rcovar = "<<p_rectCovar(0,0)<<" "<<p_rectCovar(0,1)<<" "<<p_rectCovar(0,2)<<std::endl
//              <<"         "<<p_rectCovar(1,0)<<" "<<p_rectCovar(1,1)<<" "<<p_rectCovar(1,2)<<std::endl
//              <<"         "<<p_rectCovar(2,0)<<" "<<p_rectCovar(2,1)<<" "<<p_rectCovar(2,2)<<std::endl;
  }


  void SurfacePoint::ToNaifArray(double naifOutput[3]) const {
    if(Valid()) {
      naifOutput[0] = p_x->kilometers();
      naifOutput[1] = p_y->kilometers();
      naifOutput[2] = p_z->kilometers();
    }
    else {
      IString msg = "Cannot convert an invalid surface point to a naif array";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }

  void SurfacePoint::FromNaifArray(const double naifValues[3]) {
    if(p_x && p_y && p_z) {
      p_x->setKilometers(naifValues[0]);
      p_y->setKilometers(naifValues[1]);
      p_z->setKilometers(naifValues[2]);
    }
    else {
      p_x = new Displacement(naifValues[0], Displacement::Kilometers);
      p_y = new Displacement(naifValues[1], Displacement::Kilometers);
      p_z = new Displacement(naifValues[2], Displacement::Kilometers);
    }
  }


  void SurfacePoint::SetRadii(const Distance &majorRadius,
                              const Distance &minorRadius,
                              const Distance &polarRadius) {

    if (!majorRadius.isValid()  ||
        !minorRadius.isValid()  ||
        !polarRadius.isValid()) {
      IString msg = "Radii must be set to valid distances.  One or more radii "
        "have been set to an invalid distance.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if(p_majorAxis) {
      *p_majorAxis = majorRadius;
    }
    else {
      p_majorAxis = new Distance(majorRadius);
    }

    if(p_minorAxis) {
      *p_minorAxis = minorRadius;
    }
    else {
      p_minorAxis = new Distance(minorRadius);
    }

    if(p_polarAxis) {
      *p_polarAxis = polarRadius;
    }
    else {
      p_polarAxis = new Distance(polarRadius);
    }
  }


  void SurfacePoint::ResetLocalRadius(const Distance &radius) {

    if (!radius.isValid()) {
      IString msg = "Radius value must be a valid Distance.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if (!p_x || !p_y || !p_z || !p_x->isValid() || !p_y->isValid() ||
        !p_z->isValid()) {
        IString msg = "In order to reset the local radius, a Surface Point must "
          "already be set.";
        throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    SpiceDouble lat = (double) GetLatitude().radians();
    SpiceDouble lon = (double) GetLongitude().radians();
    SpiceDouble rect[3];
    latrec_c ((SpiceDouble) radius.kilometers(), lon, lat, rect);
    p_x->setKilometers(rect[0]);
    p_y->setKilometers(rect[1]);
    p_z->setKilometers(rect[2]);

    // TODO What should be done to the variance/covariance matrix when the
    // radius is reset??? With Bundle updates will this functionality be
    // obsolete??? Ask Ken
  }


  bool SurfacePoint::Valid() const {
    static const Displacement zero(0, Displacement::Meters);
    return p_x && p_y && p_z && p_x->isValid() && p_y->isValid() && p_z->isValid() &&
           (*p_x != zero || *p_y != zero || *p_z != zero);
  }


  Displacement SurfacePoint::GetX() const {
    if(!p_x) return Displacement();

    return *p_x;
  }


  Displacement SurfacePoint::GetY() const {
    if(!p_y) return Displacement();

    return *p_y;
  }


  Displacement SurfacePoint::GetZ() const {
    if(!p_z) return Displacement();

    return *p_z;
  }


  Distance SurfacePoint::GetXSigma() const {
    if(!p_rectCovar) return Distance();

    return Distance(sqrt((*p_rectCovar)(0, 0)), Distance::Meters);
  }


  Distance SurfacePoint::GetYSigma() const {
    if(!p_rectCovar) return Distance();

    return Distance(sqrt((*p_rectCovar)(1, 1)), Distance::Meters);
  }


  Distance SurfacePoint::GetZSigma() const {
    if(!p_rectCovar) return Distance();

    return Distance(sqrt((*p_rectCovar)(2, 2)), Distance::Meters);
  }


  symmetric_matrix<double, upper> SurfacePoint::GetRectangularMatrix()
      const {
    if(!p_rectCovar) {
      symmetric_matrix<double, upper> tmp(3);
      tmp.clear();
      return tmp;
    }

    return *p_rectCovar;
  }


  Angle SurfacePoint::GetLatSigma() const {
    if(!p_sphereCovar)
      return Angle();

    return Angle(sqrt((*p_sphereCovar)(0, 0)), Angle::Radians);
  }


  Angle SurfacePoint::GetLonSigma() const {
    if(!p_sphereCovar)
      return Angle();

    return Angle(sqrt((*p_sphereCovar)(1, 1)), Angle::Radians);
  }


    Latitude SurfacePoint::GetLatitude() const {
      if (!Valid())
        return Latitude();

      // TODO Scale for accuracy with coordinate of largest magnitude
      double x = p_x->meters();
      double y = p_y->meters();
      double z = p_z->meters();

      if (x != 0.  ||  y != 0.  || z != 0.)
        return Latitude(atan2(z, sqrt(x*x + y*y) ), Angle::Radians);
      else
        return Latitude();
    }


    Longitude SurfacePoint::GetLongitude() const {
      if (!Valid())
        return Longitude();

      double x = p_x->meters();
      double y = p_y->meters();

      if(x == 0.0 && y == 0.0) {
        return Longitude(0, Angle::Radians);
      }

      double lon = atan2(y, x);
      if (lon < 0) {
        lon += 2 * PI;
      }

      return Longitude(lon, Angle::Radians);
    }


    Distance SurfacePoint::GetLocalRadius() const {
      if (!Valid())
        return Distance();

      double x = p_x->meters();
      double y = p_y->meters();
      double z = p_z->meters();

      return Distance(sqrt(x*x + y*y + z*z), Distance::Meters);
    }


    Distance SurfacePoint::GetLatSigmaDistance() const {
      Distance latSigmaDistance;

      if(Valid()) {
        Angle latSigma = GetLatSigma();

        if (latSigma.isValid()) {
          if (!p_majorAxis || !p_majorAxis->isValid()) {
            IString msg = "In order to calculate sigmas in meter units, the "
              "equitorial radius must be set with a call to SetRadii.";
            throw IException(IException::Programmer, msg, _FILEINFO_);
          }

          // Convert from radians to meters
          latSigmaDistance = latSigma.radians() * *p_majorAxis;
        }
      }

      return latSigmaDistance;
    }


  Distance SurfacePoint::GetLonSigmaDistance() const {
    Distance lonSigmaDistance;

    if(Valid()) {
      Angle lonSigma = GetLonSigma();

      if (lonSigma.isValid()) {
        if (!p_majorAxis || !p_majorAxis->isValid()) {
          IString msg = "In order to calculate sigmas in meter units, the "
            "equitorial radius must be set with a call to SetRadii.";
          throw IException(IException::Programmer, msg, _FILEINFO_);
        }

        Latitude lat = GetLatitude();
        double scaler = cos(lat.radians());

        // Convert from radians to meters and return
        if (scaler != 0.)
          lonSigmaDistance = lonSigma.radians() * *p_majorAxis / scaler;
      }
    }

    return lonSigmaDistance;
  }


  Distance SurfacePoint::GetLocalRadiusSigma() const {
    if(!p_sphereCovar)
      return Distance();

    return Distance(sqrt((*p_sphereCovar)(2, 2)), Distance::Meters);
  }


  symmetric_matrix<double, upper> SurfacePoint::GetSphericalMatrix() const {
    if(!p_sphereCovar) {
      symmetric_matrix<double, upper> tmp(3);
      tmp.clear();
      return tmp;
    }

    return *p_sphereCovar;
  }


  double SurfacePoint::GetLatWeight() const {
    double dlatSigma = GetLatSigma().radians();

      if( dlatSigma <= 0.0 ) {
          IString msg = "SurfacePoint::GetLatWeight(): Sigma <= 0.0";
          throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      return 1.0/(dlatSigma*dlatSigma);
  }

  double SurfacePoint::GetLonWeight() const {
    double dlonSigma = GetLonSigma().radians();

        if( dlonSigma <= 0.0 ) {
            IString msg = "SurfacePoint::GetLonWeight(): Sigma <= 0.0";
            throw IException(IException::Programmer, msg, _FILEINFO_);
        }

        return 1.0/(dlonSigma*dlonSigma);
      }

  double SurfacePoint::GetLocalRadiusWeight() const {

    double dlocalRadiusSigma = GetLocalRadiusSigma().kilometers();

        if (dlocalRadiusSigma <= 0.0 ) {
            IString msg = "SurfacePoint::GetRadWeight(): Sigma <= 0.0";
            throw IException(IException::Programmer, msg, _FILEINFO_);
        }

        return 1.0/(dlocalRadiusSigma*dlocalRadiusSigma);
      }

  Distance SurfacePoint::GetDistanceToPoint(const SurfacePoint &other) const {
    if(p_majorAxis || p_minorAxis || p_polarAxis ||
       other.p_majorAxis || other.p_minorAxis || other.p_polarAxis) {
      IString msg = "SurfacePoint::GetDistanceToPoint not yet implemented for "
          "ellipsoids";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if(!Valid() || !other.Valid())
      return Distance();

    return GetDistanceToPoint(other,
        ((GetLocalRadius() + other.GetLocalRadius()) / 2.0));
  }


  Distance SurfacePoint::GetDistanceToPoint(const SurfacePoint &other,
      const Distance &sphereRadius) const {
    if(!Valid() || !other.Valid())
      return Distance();

    // Convert lat/lon values to radians
    const Angle &latitude = GetLatitude();
    const Angle &longitude = GetLongitude();
    const Angle &otherLatitude = other.GetLatitude();
    const Angle &otherLongitude = other.GetLongitude();

    // The harvestine method:
    //   http://en.wikipedia.org/wiki/Haversine_formula
    Angle deltaLat = latitude - otherLatitude;
    Angle deltaLon = longitude - otherLongitude;

    double haversinLat = sin(deltaLat.radians() / 2.0);
    haversinLat *= haversinLat;

    double haversinLon = sin(deltaLon.radians() / 2.0);
    haversinLon *= haversinLon;

    double a = haversinLat + cos(latitude.radians()) *
               cos(otherLatitude.radians()) *
               haversinLon;

    double c = 2 * atan(sqrt(a) / sqrt(1 - a));

    return sphereRadius * c;
  }


  bool SurfacePoint::operator==(const SurfacePoint &other) const {
    bool equal = true;

    if(equal && p_x && p_y && p_z && other.p_x && other.p_y && other.p_z) {
      equal = equal && *p_x == *other.p_x;
      equal = equal && *p_y == *other.p_y;
      equal = equal && *p_z == *other.p_z;
    }
    else if(equal) {
      equal = equal && p_x == NULL && other.p_x == NULL;
      equal = equal && p_y == NULL && other.p_y == NULL;
      equal = equal && p_z == NULL && other.p_z == NULL;
    }

    if(equal && p_majorAxis && p_minorAxis && p_polarAxis) {
      equal = equal && *p_majorAxis == *other.p_majorAxis;
      equal = equal && *p_minorAxis == *other.p_minorAxis;
      equal = equal && *p_polarAxis == *other.p_polarAxis;
    }
    else if(equal) {
      equal = equal && p_majorAxis == NULL && other.p_majorAxis == NULL;
      equal = equal && p_minorAxis == NULL && other.p_minorAxis == NULL;
      equal = equal && p_polarAxis == NULL && other.p_polarAxis == NULL;
    }

    if(equal && p_rectCovar) {
      equal = equal && (*p_rectCovar)(0, 0) == (*other.p_rectCovar)(0, 0);
      equal = equal && (*p_rectCovar)(0, 1) == (*other.p_rectCovar)(0, 1);
      equal = equal && (*p_rectCovar)(0, 2) == (*other.p_rectCovar)(0, 2);
      equal = equal && (*p_rectCovar)(1, 1) == (*other.p_rectCovar)(1, 1);
      equal = equal && (*p_rectCovar)(1, 2) == (*other.p_rectCovar)(1, 2);
      equal = equal && (*p_rectCovar)(2, 2) == (*other.p_rectCovar)(2, 2);
    }
    else if(equal) {
      equal = equal && p_rectCovar == NULL && other.p_rectCovar == NULL;
    }

    if(equal && p_sphereCovar) {
      equal = equal && (*p_sphereCovar)(0, 0) == (*other.p_sphereCovar)(0, 0);
      equal = equal && (*p_sphereCovar)(0, 1) == (*other.p_sphereCovar)(0, 1);
      equal = equal && (*p_sphereCovar)(0, 2) == (*other.p_sphereCovar)(0, 2);
      equal = equal && (*p_sphereCovar)(1, 1) == (*other.p_sphereCovar)(1, 1);
      equal = equal && (*p_sphereCovar)(1, 2) == (*other.p_sphereCovar)(1, 2);
      equal = equal && (*p_sphereCovar)(2, 2) == (*other.p_sphereCovar)(2, 2);
    }
    else if(equal) {
      equal = equal && p_sphereCovar == NULL && other.p_sphereCovar == NULL;
    }

    return equal;
  }

  SurfacePoint &SurfacePoint::operator=(const SurfacePoint &other) {
    // The lazy way of doing this (free all memory and copy) is too expensive
    // in the default case!
    if(p_x && other.p_x &&
       p_y && other.p_y &&
       p_z && other.p_z &&
       !p_majorAxis && !other.p_majorAxis &&
       !p_minorAxis && !other.p_minorAxis &&
       !p_polarAxis && !other.p_polarAxis &&
       !p_rectCovar && !other.p_rectCovar &&
       !p_sphereCovar && !other.p_sphereCovar) {
      *p_x = *other.p_x;
      *p_y = *other.p_y;
      *p_z = *other.p_z;
    }
    else {
      FreeAllocatedMemory();
      if(other.p_majorAxis) {
        p_majorAxis = new Distance(*other.p_majorAxis);
      }

      if(other.p_minorAxis) {
        p_minorAxis = new Distance(*other.p_minorAxis);
      }

      if(other.p_polarAxis) {
        p_polarAxis = new Distance(*other.p_polarAxis);
      }

      if(other.p_x) {
        p_x = new Displacement(*other.p_x);
      }

      if(other.p_y) {
        p_y = new Displacement(*other.p_y);
      }

      if(other.p_z) {
        p_z = new Displacement(*other.p_z);
      }

      if(other.p_rectCovar) {
        p_rectCovar = new symmetric_matrix<double, upper>(*other.p_rectCovar);
      }

      if(other.p_sphereCovar) {
        p_sphereCovar = new symmetric_matrix<double, upper>(*other.p_sphereCovar);
      }
    }

    return *this;
  }

  void SurfacePoint::FreeAllocatedMemory() {
    if(p_x) {
      delete p_x;
      p_x = NULL;
    }

    if(p_y) {
      delete p_y;
      p_y = NULL;
    }

    if(p_z) {
      delete p_z;
      p_z = NULL;
    }

    if(p_majorAxis) {
      delete p_majorAxis;
      p_majorAxis = NULL;
    }

    if(p_minorAxis) {
      delete p_minorAxis;
      p_minorAxis = NULL;
    }

    if(p_polarAxis) {
      delete p_polarAxis;
      p_polarAxis = NULL;
    }

    if(p_rectCovar) {
      delete p_rectCovar;
      p_rectCovar = NULL;
    }

    if(p_sphereCovar) {
      delete p_sphereCovar;
      p_sphereCovar = NULL;
    }
  }
}