Go to the documentation of this file.00001 #ifndef ELLIPSOID_SPACE_H
00002 #define ELLIPSOID_SPACE_H
00003 #include <cmath>
00004 #include <ros/ros.h>
00005 #include <hrl_phri_2011/EllipsoidParams.h>
00006
00007 using namespace std;
00008
00009 #define SQ(x) ((x) * (x))
00010 #define PI 3.14159265
00011
00012 struct Ellipsoid
00013 {
00014 double A, B, E, height;
00015 Ellipsoid() : A(1), B(1), height(1)
00016 {
00017 E = sqrt(fabs(SQ(A) - SQ(B))) / A;
00018 }
00019
00020 Ellipsoid(double a, double b) : A(a), B(b), height(1)
00021 {
00022 E = sqrt(fabs(SQ(A) - SQ(B))) / A;
00023 }
00024
00025 Ellipsoid(const hrl_phri_2011::EllipsoidParams& ep)
00026 {
00027 setParams(ep);
00028 }
00029
00030 void setParams(const hrl_phri_2011::EllipsoidParams& ep)
00031 {
00032 A = 1;
00033 E = ep.E;
00034 B = sqrt(1 - SQ(E));
00035 height = ep.height;
00036 }
00037
00038 void cartToEllipsoidal(double x, double y, double z, double& lat, double& lon, double& height);
00039 void ellipsoidalToCart(double lat, double lon, double height, double& x, double& y, double& z);
00040 void mollweideProjection(double lat, double lon, double& x, double& y);
00041 };
00042
00043 void Ellipsoid::cartToEllipsoidal(double x, double y, double z, double& lat, double& lon, double& height) {
00044 lon = atan2(y, x);
00045 if(lon < 0)
00046 lon += 2 * PI;
00047 double a = A * E;
00048 double p = sqrt(SQ(x) + SQ(y));
00049 lat = asin(sqrt((sqrt(SQ(SQ(z) - SQ(a) + SQ(p)) + SQ(2 * a * p)) / SQ(a) -
00050 SQ(z / a) - SQ(p / a) + 1) / 2));
00051 if(z < 0)
00052 lat = PI - fabs(lat);
00053 double cosh_height = z / (a * cos(lat));
00054 height = log(cosh_height + sqrt(SQ(cosh_height) - 1));
00055 }
00056
00057 void Ellipsoid::ellipsoidalToCart(double lat, double lon, double height, double& x, double& y, double& z) {
00058 double a = A * E;
00059 x = a * sinh(height) * sin(lat) * cos(lon);
00060 y = a * sinh(height) * sin(lat) * sin(lon);
00061 z = a * cosh(height) * cos(lat);
00062 }
00063
00064 void Ellipsoid::mollweideProjection(double lat, double lon, double& x, double& y) {
00065 double a = A;
00066 double b = A * (1 - SQ(E)) / (PI * E) * (log((1 + E) / (1 - E)) + 2 * E / (1 - SQ(E)));
00067 double Sl = sin(lat);
00068 double k = PI * ( (log((1 + E * Sl) / (1 - E * Sl)) + 2 * E * Sl / (1 - SQ(E) * SQ(Sl))) /
00069 (log((1 + E) / (1 - E)) + 2 * E / (1 - SQ(E))));
00070 double t = lat;
00071 double diff_val = 10000.0;
00072 while(fabs(diff_val) > 0.00001) {
00073 diff_val = - ( 2 * t + sin(2 * t) - k) / (2 + 2 * cos(2 * t));
00074 t += diff_val;
00075 }
00076 x = a * lon * cos(t);
00077 y = b * sin(t);
00078 }
00079 #endif // ELLIPSOID_SPACE_H