/* Adept MobileRobots Robotics Interface for Applications (ARIA) Copyright (C) 2004, 2005 ActivMedia Robotics LLC Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc. Copyright (C) 2011, 2012, 2013 Adept Technology This program 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 2 of the License, or (at your option) any later version. This program 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, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA If you wish to redistribute ARIA under different terms, contact Adept MobileRobots for information about a commercial version of ARIA at robots@mobilerobots.com or Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960 */ #ifndef ARGPSCOORDS_H #define ARGPSCOORDS_H #include "ariaTypedefs.h" class Ar3DPoint; class ArLLACoords; class ArECEFCoords; class ArENUCoords; class ArWGS84; /** Base class for points in 3 dimensional cartesian space. @ingroup UtilityClasses */ class Ar3DPoint { public: Ar3DPoint(void) : myX(0), myY(0), myZ(0) {} Ar3DPoint(double x, double y, double z) : myX(x), myY(y), myZ(z) {} /// Destructor. ~Ar3DPoint() {} /// Add Ar3DPoint operator+(Ar3DPoint c) { Ar3DPoint sum(myX + c.myX, myY + c.myY, myZ + c.myZ); return sum; } /// Diff Ar3DPoint operator-(Ar3DPoint c) { Ar3DPoint dif(myX - c.myX, myY - c.myY, myZ - c.myZ); return dif; } /// Diff Ar3DPoint operator*(double c) { Ar3DPoint pro(myX*c, myY*c, myZ*c); return pro; } /// Dot product double dot(Ar3DPoint c) { double dotP(myX * c.myX + myY * c.myY + myZ * c.myZ); return dotP; } /// Cross product Ar3DPoint cross(Ar3DPoint c) { Ar3DPoint crossP(myY * c.myZ - myZ * c.myY, myZ * c.myX - myX * c.myZ, myX * c.myY - myY * c.myX); return crossP; } /// Print. /** @swignote Use 'printPoint' instead */ AREXPORT void print(const char* head=NULL); double getX() const {return myX;} double getY() const {return myY;} double getZ() const {return myZ;} void setX(double x) { myX = x; } void setY(double y) { myY = y; } void setZ(double z) { myZ = z; } protected: double myX; double myY; double myZ; }; #ifdef WIN32 // Need to export some variables on Windows because they are used in inline methods (which is good), but they can't be exported if const. #define ARGPSCOORDS_CONSTANT #else #define ARGPSCOORDS_CONSTANT const #endif /** * All the constants defined by the World Geodetic System 1984. * @ingroup UtilityClasses */ class ArWGS84 { public: ArWGS84(void) {} static double getE() {return mye;} static double getA() {return mya;} static double getB() {return myb;} static double getEP() {return myep;} static double get1byf() {return my1byf;} static double getOmega() {return myOmega;} static double getGM() {return myGM;} private: AREXPORT static ARGPSCOORDS_CONSTANT double mya; // meters AREXPORT static ARGPSCOORDS_CONSTANT double myb; // meters AREXPORT static ARGPSCOORDS_CONSTANT double myep; AREXPORT static ARGPSCOORDS_CONSTANT double myc; // m/sec AREXPORT static ARGPSCOORDS_CONSTANT double mye; AREXPORT static ARGPSCOORDS_CONSTANT double my1byf; AREXPORT static ARGPSCOORDS_CONSTANT double myOmega; // rad/sec AREXPORT static ARGPSCOORDS_CONSTANT double myGM; // m^3/sec^2 AREXPORT static ARGPSCOORDS_CONSTANT double myg; // m/sec^2. Ave g. AREXPORT static ARGPSCOORDS_CONSTANT double myM; // kg. Mass of earth. }; /** * Earth Centered Earth Fixed Coordinates. @ingroup UtilityClasses */ class ArECEFCoords : public Ar3DPoint { public: ArECEFCoords(double x, double y, double z) : Ar3DPoint(x, y, z) {} AREXPORT ArLLACoords ECEF2LLA(void); AREXPORT ArENUCoords ECEF2ENU(ArECEFCoords ref); }; /** * Latitude, Longitude and Altitude Coordinates. * @ingroup UtilityClasses */ class ArLLACoords : public Ar3DPoint { public: ArLLACoords(void) : Ar3DPoint(0, 0, 0) {} ArLLACoords(double x, double y, double z) : Ar3DPoint(x, y, z) {} AREXPORT ArECEFCoords LLA2ECEF(void); double getLatitude(void) const {return getX();} double getLongitude(void) const {return getY();} double getAltitude(void) const {return getZ();} void setLatitude(double l) { setX(l); } void setLongitude(double l) { setY(l); } void setAltitude(double a) { setZ(a); } }; /** * East North Up coordinates. @ingroup UtilityClasses */ class ArENUCoords : public Ar3DPoint { public: ArENUCoords(double x, double y, double z) : Ar3DPoint(x, y, z) {} AREXPORT ArECEFCoords ENU2ECEF(ArLLACoords ref); double getEast(void) const {return getX();} double getNorth(void) const {return getY();} double getUp(void) const {return getZ();} void setEast(double e) { setX(e); } void setNorth(double n) { setY(n); } void setUp(double u) { setZ(u); } }; /** * Coordinates based on a map with origin in LLA coords with conversion * methods from LLA to ENU and from ENU to LLA coordinates. * @ingroup UtilityClasses */ class ArMapGPSCoords : public ArENUCoords { public: ArMapGPSCoords(ArLLACoords org) : ArENUCoords(0.0, 0.0, 0.0), myOriginECEF(0), myOriginLLA(0), myOriginSet(false) { setOrigin(org); } ArMapGPSCoords() : ArENUCoords(0, 0, 0), myOriginECEF(0), myOriginLLA(0), myOriginSet(false) { } AREXPORT bool convertMap2LLACoords(const double ea, const double no, const double up, double& lat, double& lon, double& alt) const; AREXPORT bool convertLLA2MapCoords(const double lat, const double lon, const double alt, double& ea, double& no, double& up) const; bool convertLLA2MapCoords(const ArLLACoords& lla, double& ea, double& no, double& up) { return convertLLA2MapCoords(lla.getLatitude(), lla.getLongitude(), lla.getAltitude(), ea, no, up); } void setOrigin(ArLLACoords org) { if(myOriginLLA) delete myOriginLLA; if(myOriginECEF) delete myOriginECEF; myOriginSet = true; myOriginLLA = new ArLLACoords(org); myOriginECEF = new ArECEFCoords(myOriginLLA->LLA2ECEF()); } ArECEFCoords* myOriginECEF; ArLLACoords* myOriginLLA; bool myOriginSet; }; #endif // ARGPSCOORDS_H