ReferenceEllipsoid.cc

Go to the documentation of this file.

#include <string>
#include <sstream>
#include <cmath>
#include <utl/ReferenceEllipsoid.h>
#include <utl/CoordinateSystem.h>
#include <utl/AugerUnits.h>
#include <utl/ErrorLogger.h>
#include <utl/AugerException.h>
#include <utl/Point.h>
#include <utl/StringCompare.h>

using namespace utl;
using namespace std;


double
ReferenceEllipsoid::GetFlattening()
  const
{
  // we need to take the negative sign of the solution...
  return 1 - sqrt(1-fEccentricity2);
}


double
ReferenceEllipsoid::GetPolarRadius()
  const
{
  return fEquatorialRadius * (1 - GetFlattening());
}


double
ReferenceEllipsoid::GetRn(const double phi)
  const
{
  using namespace std;

  const double& a  = fEquatorialRadius;
  const double& e2 = fEccentricity2;
  const double sinPhi = sin(phi);

  return a / sqrt(1 - e2*sinPhi*sinPhi);
}


ReferenceEllipsoid::Triple
ReferenceEllipsoid::PointToLatitudeLongitudeHeight(const Point& thePoint)
  const
{
  using namespace std;                // for math functions

  double x;
  double y;
  double z;
  Point pt(thePoint);
  boost::tie(x, y, z) = pt.GetCoordinates(fECEF);

  const double& e2 = fEccentricity2;

  double p = sqrt(x*x + y*y);

  double longitude = atan2(y, x);

  // first guess of latitude
  double latitude = atan(z/p);    // this is given wrongly in the paper cited
  double height;

  if(p == 0.) {                   // at the poles
    height = abs(z) - GetPolarRadius();
  } else {                        // not at the poles
    // solve iteratively for latitude and height
    const int iterations = 4;
    for (int i = 0; i < iterations; ++i) {
      double Rn = GetRn(latitude);
      height    = p / cos(latitude) - Rn;
      latitude  = atan(z/p/(1 - e2*(Rn / (Rn+height) ) ) );
    }
  }

  return boost::make_tuple(latitude, longitude, height);
}


Point
ReferenceEllipsoid::LatitudeLongitudeHeightToPoint(double const latitude,
                                                   double const longitude,
                                                   double const height)
  const
{
  using namespace std;           // for math functions

  const double& e2 = fEccentricity2;

  double Rn = GetRn(latitude);

  double x = (Rn + height) * cos(latitude) * cos(longitude);
  double y = (Rn + height) * cos(latitude) * sin(longitude);
  double z = (Rn*(1-e2) + height) * sin(latitude);

  return Point(x, y, z, fECEF);

}


Point
ReferenceEllipsoid::LatitudeLongitudeHeightToPoint(const Triple& theGeodeticCoordinates)
  const
{
  return LatitudeLongitudeHeightToPoint(theGeodeticCoordinates.get<0>(),
                                        theGeodeticCoordinates.get<1>(),
                                        theGeodeticCoordinates.get<2>());
}


boost::tuple<TransformationMatrix, TransformationMatrix>
ReferenceEllipsoid::TransformECEFToLocalSystem(const Point& theOrigin)
  const
{
  using namespace std;

  double latitude, longitude, height;
  boost::tie(latitude, longitude, height) =
    PointToLatitudeLongitudeHeight(theOrigin);
  double x, y, z;
  boost::tie(x, y, z) = Point(theOrigin).GetCoordinates(fECEF);

  const double cosLat = cos(latitude);
  const double sinLat = sin(latitude);
  const double cosLon = cos(longitude);
  const double sinLon = sin(longitude);

  const TransformationMatrix rot =
    TransformationMatrix::TransformToBasis(
      -sinLon, -sinLat*cosLon, cosLat*cosLon,
       cosLon, -sinLat*sinLon, cosLat*sinLon,
            0,          cosLat,          sinLat
    );
  const TransformationMatrix trans =
    TransformationMatrix::Translation(x, y, z);

  return boost::make_tuple(trans, rot);
}


/***************************************************************************
 * code for the ellipsoid registry
 ***************************************************************************/
ReferenceEllipsoid::Registry* ReferenceEllipsoid::gfRegistry = 0;


const ReferenceEllipsoid&
ReferenceEllipsoid::Get(const EllipsoidID theID)
{
  if (!gfRegistry) {
    InitWithECEF(CoordinateSystem::GetRootCoordinateSystem());
  }
  Registry::iterator i = gfRegistry->find(theID);
  if (i == gfRegistry->end()) {
    ostringstream msg;
    msg << "Ellipsoid with id " << theID << " not existent.";
    ERROR(msg);
    throw InvalidEllipsoidException(msg.str());
  } else {
    return i->second;
  }
}


void
ReferenceEllipsoid::InitWithECEF(const CoordinateSystemPtr& theCS)
{
  if (!gfRegistry) {
    gfRegistry = new Registry;
  } else {
    string msg("Calling InitWithECEF, "
               "but ReferenceEllipsoidRegistry already initialised.");
    ERROR(msg);
    throw InitSequenceException(msg);
  }
  //RegisterOneEllipsoid("WGS84", 6378137.*m, 0.00669438);
  // changed to the more precise values from Peter H Dana's page. LN.
  RegisterOneEllipsoid(eWGS84, 6378137.*m, 0.00669437999013, theCS);
}


ReferenceEllipsoid::EllipsoidID
ReferenceEllipsoid::GetEllipsoidIDFromString(const string& str)
{
  if (StringEquivalent(str, "WGS84"))
    return eWGS84;

  string msg("Unknown reference ellipsoid ");
  msg += str;
  ERROR(msg);
  throw InvalidEllipsoidException(msg);
}


void
ReferenceEllipsoid::RegisterOneEllipsoid(const EllipsoidID theID,
                                         double theEquatorialRadius,
                                         double theEccentricity,
                                         const CoordinateSystemPtr& theCS)
{
  ReferenceEllipsoid e(theEquatorialRadius, theEccentricity, theCS);
  bool ok = gfRegistry->insert(make_pair(theID, e)).second;
  if (!ok) {
    ostringstream msg;
    msg << "Double insertion of ellipsoid with id" << theID;
    WARNING(msg);
  }
}

// Configure (x)emacs for this file ...
// Local Variables:
// mode: c++
// End: