rosaria/Legacy/Aria/include/ArGPSCoords.h

/*
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