hrl_ellipsoidal_control: ellipsoid

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 
00006 using namespace std;
00007 
00008 #define SQ(x) ((x) * (x))
00009 #define PI 3.14159265
00010 
00011 struct Ellipsoid 
00012 {
00013     bool is_prolate;
00014     double A, B, E, height, a;
00015     Ellipsoid(bool _is_prolate=true) : 
00016         is_prolate(_is_prolate), A(1), B(1), height(1)
00017     {
00018         E = sqrt(fabs(SQ(A) - SQ(B))) / A;
00019     }
00020     
00021     Ellipsoid(double a, double b, bool _is_prolate=true) : 
00022         is_prolate(_is_prolate), A(a), B(b), height(1) 
00023     {
00024         E = sqrt(fabs(SQ(A) - SQ(B))) / A;
00025     }
00026 
00027     void cartToEllipsoidal(double x, double y, double z, double& lat, double& lon, double& height);
00028     void ellipsoidalToCart(double lat, double lon, double height, double& x, double& y, double& z);
00029     void mollweideProjection(double lat, double lon, double& x, double& y);
00030 };
00031 
00032 void Ellipsoid::cartToEllipsoidal(double x, double y, double z, double& lat, double& lon, double& height) {
00033     printf("ERROR -- Fix this from python code...!!!\n");
00034     /*
00035     lon = atan2(y, x);
00036     if(lon < 0) 
00037         lon += 2 * PI;
00038     double a = A * E;
00039     double p = sqrt(SQ(x) + SQ(y));
00040     lat = asin(sqrt((sqrt(SQ(SQ(z) - SQ(a) + SQ(p)) + SQ(2 * a * p)) / SQ(a) -
00041                      SQ(z / a) - SQ(p / a) + 1) / 2));
00042     if(z < 0)
00043         lat = PI - fabs(lat);
00044     double cosh_height = z / (a * cos(lat));
00045     height = log(cosh_height + sqrt(SQ(cosh_height) - 1));
00046     */
00047 }
00048 
00049 void Ellipsoid::ellipsoidalToCart(double lat, double lon, double height, double& x, double& y, double& z) {
00050     double a = A * E;
00051     if(is_prolate) {
00052         x = a * sinh(height) * sin(lat) * cos(lon);
00053         y = a * sinh(height) * sin(lat) * sin(lon);
00054         z = a * cosh(height) * cos(lat);
00055     } else {
00056         x = a * cosh(height) * cos(lat) * cos(lon);
00057         y = a * cosh(height) * cos(lat) * sin(lon);
00058         z = a * sinh(height) * sin(lat);
00059     }
00060 }
00061 
00062 void Ellipsoid::mollweideProjection(double lat, double lon, double& x, double& y) {
00063     double a = A;
00064     double b = A * (1 - SQ(E)) / (PI * E) * (log((1 + E) / (1 - E)) + 2 * E / (1 - SQ(E)));
00065     double Sl = sin(lat);
00066     double k = PI * ( (log((1 + E * Sl) / (1 - E * Sl)) + 2 * E * Sl / (1 - SQ(E) * SQ(Sl))) /
00067                       (log((1 + E)      / (1 - E))      + 2 * E      / (1 - SQ(E))));
00068     double t = lat;
00069     double diff_val = 10000.0;
00070     while(fabs(diff_val) > 0.00001) {
00071         diff_val = - ( 2 * t + sin(2 * t) - k) / (2 + 2 * cos(2 * t));
00072         t += diff_val;
00073     }
00074     x = a * lon * cos(t);
00075     y = b * sin(t);
00076 }
00077 #endif // ELLIPSOID_SPACE_H