/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/guidance/GeoCoord.cc

Go to the documentation of this file.

/*
 * GeoCoord
 * Copyright (C) Eduardo Wiputra 2009 <ewiputra@users.sourceforge.net
 *
 * GeoCoord.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoCoord.cc  is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include<iostream>
#include<vector>
#include<iomanip>
#include<fstream>
#include<cmath>
#include<boost/numeric/ublas/vector.hpp>
#include "uvsim/guidance/GeoCoord.h"


namespace uvsim
{

GeoCoord::GeoCoord()
{
}
GeoCoord::~GeoCoord()
{
}
GeoCoord::GeoCoord(double latitude, double longitude, double altitude)
{
    //sets the latitude, longitude, altitude to m_Latitude, m_Longitude, and m_Altitude respectively
    setGeoCoord(latitude, longitude, altitude);
}

void GeoCoord::setGeoCoord (double latitude, double longitude, double altitude)
{
    m_Latitude = latitude;
    m_Longitude = longitude;
    m_Altitude = altitude;
}

std::ostream& operator<< (std::ostream &out, GeoCoord &geoCoord)
{
    std::cout << std::fixed << std::setprecision(3) << std::setfill('0') << std::setw(7) << geoCoord.m_Latitude
              << "      " << std::setw(7) << geoCoord.m_Longitude << "      " << std::setw(7)
              << geoCoord.m_Altitude << std::endl;
    return out;
}

void GeoCoord::print()
{
    std::cout << m_Latitude << std::setw(16)
              << m_Longitude << std::setw(15)
              << m_Altitude << std::endl;
}

vector<double> GeoCoord::getCartesian()
{
    //Code based on WGS84 and taken from Wikipedia
    vector<double> cartesian(3); //vector that contains cartesian coordinate
    double const earthSemiMajorAxis = 6378137;
    double const recipFlattening = 1 / 298.257223563;
    double eccentSquared = 2 * recipFlattening - pow(recipFlattening,2);
    double chi;
    chi = sqrt(1 - eccentSquared * pow((sin(m_Latitude * M_PI / 180)),2));
    cartesian[0] =      (earthSemiMajorAxis / chi + m_Altitude)
                   * cos(m_Latitude * M_PI / 180) * cos(m_Longitude * M_PI / 180);
    cartesian[1] =      (earthSemiMajorAxis / chi + m_Altitude)
                   * cos(m_Latitude * M_PI / 180) * sin(m_Longitude * M_PI / 180);
    cartesian[2] =      (earthSemiMajorAxis * (1 - eccentSquared) / chi +  m_Altitude)
                   * sin(m_Latitude * M_PI / 180);

    return (cartesian);
}

void GeoCoord::getGeoCoord(double cartesianX, double cartesianY, double cartesianZ)
{
    //Code based on WGS84 and taken from Wikipedia
    double earthSemiMajorAxis = 6378137;
    double recipFlattening = 1 / 298.257223563;
    double earthSemiMinorAxis = earthSemiMajorAxis * (1 - recipFlattening);

    double eccentSquared = 2 * recipFlattening - pow(recipFlattening,2);
    double eccent2Squared = recipFlattening * (2 - recipFlattening) / (pow((1 - recipFlattening), 2));

    double r2 = pow(cartesianX,2) + pow(cartesianY,2);
    double r = sqrt(r2);
    double E2 = pow(earthSemiMajorAxis,2) - pow(earthSemiMinorAxis,2);
    double F = 54 * pow(earthSemiMinorAxis,2) * pow(cartesianZ,2);
    double G = r2 + (1 - eccentSquared) * pow(cartesianZ,2) - eccentSquared * E2;
    double c = (eccentSquared * eccentSquared * F * r2) / (G * G * G);
    double s = pow((1 + c + sqrt(c * c + 2 * c)),1/3);
    double P = F / (3 * pow((s + 1 / s + 1),2) * G * G);
    double Q = sqrt(1 + 2 * eccentSquared * eccentSquared * P);
    double ro = -(eccentSquared * P * r) / (1 + Q) + sqrt((earthSemiMajorAxis * earthSemiMajorAxis / 2)
                * (1 + 1 / Q) - ((1 - eccentSquared) * P * pow(cartesianZ, 2)) / (Q * (1 + Q)) - P * r2/2);
    double tmp = pow((r - eccentSquared * ro), 2);
    double U = sqrt( tmp + pow(cartesianZ,2));
    double V = sqrt( tmp + (1 - eccentSquared) * pow(cartesianZ,2));
    double zo = (pow(earthSemiMinorAxis,2)*cartesianZ) / (earthSemiMajorAxis * V);

    m_Altitude = U * (1 - pow(earthSemiMinorAxis,2) / (earthSemiMajorAxis * V));
    m_Latitude = atan((cartesianZ + eccent2Squared * zo) / r) * 180 / M_PI;
    m_Longitude = atan2(cartesianY,cartesianX) * 180 / M_PI;
}
}

// vim:ts=2:sw=2

---