FGLocation.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGLocation.h
 Author:       Jon S. Berndt, Mathias Froehlich
 Date started: 04/04/2004

 ------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) ------------------
 -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
 -------           (C) 2011  Ola Røer Thorsen (ola@silentwings.no) -----------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU Lesser 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 Lesser General Public License for more
 details.

 You should have received a copy of the GNU Lesser 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.

 Further information about the GNU Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.

HISTORY
-------------------------------------------------------------------------------
04/04/2004   MF   Created from code previously in the old positions class.
11/01/2011   ORT  Encapsulated ground callback code in FGLocation and removed
                  it from FGFDMExec.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGLOCATION_H
#define FGLOCATION_H

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGJSBBase.h"
#include "FGColumnVector3.h"
#include "FGMatrix33.h"
#include "input_output/FGGroundCallback.h"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_LOCATION "$Id: FGLocation.h,v 1.35 2015/09/20 20:53:13 bcoconni Exp $"

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

namespace JSBSim {

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGLocation : public FGJSBBase
{
public:
  FGLocation(void);

  FGLocation(double lon, double lat, double radius);

  FGLocation(const FGColumnVector3& lv);

  FGLocation(const FGLocation& l);

  void SetLongitude(double longitude);

  void SetLatitude(double latitude);

  void SetRadius(double radius);

  void SetPosition(double lon, double lat, double radius);

  void SetPositionGeodetic(double lon, double lat, double height);

  void SetEllipse(double semimajor, double semiminor);

  void SetEarthPositionAngle(double EPA) {epa = EPA; mCacheValid = false;}

  void IncrementEarthPositionAngle(double delta) {epa += delta; mCacheValid = false;}

  double GetLongitude() const { ComputeDerived(); return mLon; }

  double GetLongitudeDeg() const { ComputeDerived(); return radtodeg*mLon; }

  double GetSinLongitude() const { ComputeDerived(); return -mTec2l(2,1); }

  double GetCosLongitude() const { ComputeDerived(); return mTec2l(2,2); }

  double GetLatitude() const { ComputeDerived(); return mLat; }

  double GetGeodLatitudeRad(void) const { ComputeDerived(); return mGeodLat; }

  double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }

  double GetGeodLatitudeDeg(void) const { ComputeDerived(); return radtodeg*mGeodLat; }

  double GetGeodAltitude(void) const {ComputeDerived(); return GeodeticAltitude;}

  double GetSinLatitude() const { ComputeDerived(); return -mTec2l(3,3); }

  double GetCosLatitude() const { ComputeDerived(); return mTec2l(1,3); }

  double GetTanLatitude() const {
    ComputeDerived();
    double cLat = mTec2l(1,3);
    if (cLat == 0.0)
      return 0.0;
    else
      return -mTec2l(3,3)/cLat;
  }

  double GetEPA() const {return epa;}

  //double GetRadius() const { return mECLoc.Magnitude(); } // may not work with FlightGear
  double GetRadius() const { ComputeDerived(); return mRadius; }


  void SetAltitudeASL(double altitudeASL)
  { SetRadius(GetSeaLevelRadius() + altitudeASL); }

  void SetAltitudeAGL(double altitudeAGL)
  { SetRadius(GetTerrainRadius() + altitudeAGL); }

  double GetSeaLevelRadius(void) const
  { ComputeDerived(); return GroundCallback->GetSeaLevelRadius(*this); }

  double GetTerrainRadius(void) const
  { ComputeDerived(); return GroundCallback->GetTerrainGeoCentRadius(*this); }

  double GetAltitudeASL(void) const
  { ComputeDerived(); return GroundCallback->GetAltitude(*this); }

  double GetAltitudeAGL(void) const {
    FGLocation c;
    FGColumnVector3 n,v,w;
    return GetContactPoint(c,n,v,w);
  }

  double GetContactPoint(FGLocation& contact, FGColumnVector3& normal,
                         FGColumnVector3& v, FGColumnVector3& w) const
  { ComputeDerived(); return GroundCallback->GetAGLevel(*this, contact, normal, v, w); }

  static void SetGroundCallback(FGGroundCallback* gc) { GroundCallback = gc; }

  static FGGroundCallback* GetGroundCallback(void) { return GroundCallback; }

  const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }

  const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }

  const FGMatrix33& GetTi2ec(void) const { ComputeDerived(); return mTi2ec; }

  const FGMatrix33& GetTec2i(void) const { ComputeDerived(); return mTec2i; }

  const FGMatrix33& GetTi2l(void) const {ComputeDerived(); return mTi2l;}

  const FGMatrix33& GetTl2i(void) const {ComputeDerived(); return mTl2i;}

  double GetDistanceTo(double target_longitude, double target_latitude) const;
  
  double GetHeadingTo(double target_longitude, double target_latitude) const;

  FGLocation LocalToLocation(const FGColumnVector3& lvec) const {
    ComputeDerived(); return mTl2ec*lvec + mECLoc;
  }

  FGColumnVector3 LocationToLocal(const FGColumnVector3& ecvec) const {
    ComputeDerived(); return mTec2l*(ecvec - mECLoc);
  }

  // For time-stepping, locations have vector properties...

  double operator()(unsigned int idx) const { return mECLoc.Entry(idx); }

  double& operator()(unsigned int idx) { mCacheValid = false; return mECLoc.Entry(idx); }

  double Entry(unsigned int idx) const { return mECLoc.Entry(idx); }

  double& Entry(unsigned int idx) {
    mCacheValid = false; return mECLoc.Entry(idx);
  }

  const FGLocation& operator=(const FGColumnVector3& v)
  {
    mECLoc(eX) = v(eX);
    mECLoc(eY) = v(eY);
    mECLoc(eZ) = v(eZ);
    mCacheValid = false;
    //ComputeDerived();
    return *this;
  }

  const FGLocation& operator=(const FGLocation& l);

  bool operator==(const FGLocation& l) const {
    return mECLoc == l.mECLoc;
  }

  bool operator!=(const FGLocation& l) const { return ! operator==(l); }

  const FGLocation& operator+=(const FGLocation &l) {
    mCacheValid = false;
    mECLoc += l.mECLoc;
    return *this;
  }

  const FGLocation& operator-=(const FGLocation &l) {
    mCacheValid = false;
    mECLoc -= l.mECLoc;
    return *this;
  }

  const FGLocation& operator*=(double scalar) {
    mCacheValid = false;
    mECLoc *= scalar;
    return *this;
  }

  const FGLocation& operator/=(double scalar) {
    return operator*=(1.0/scalar);
  }

  FGLocation operator+(const FGLocation& l) const {
    return FGLocation(mECLoc + l.mECLoc);
  }

  FGLocation operator-(const FGLocation& l) const {
    return FGLocation(mECLoc - l.mECLoc);
  }

  FGLocation operator*(double scalar) const {
    return FGLocation(scalar*mECLoc);
  }

  operator const FGColumnVector3&() const {
    return mECLoc;
  }

private:
  void ComputeDerivedUnconditional(void) const;

  void ComputeDerived(void) const {
    if (!mCacheValid)
      ComputeDerivedUnconditional();
  }

  FGColumnVector3 mECLoc;

  mutable double mLon;
  mutable double mLat;
  mutable double mRadius;
  mutable double mGeodLat;
  mutable double GeodeticAltitude;

  mutable FGMatrix33 mTl2ec;
  mutable FGMatrix33 mTec2l;
  mutable FGMatrix33 mTi2ec;
  mutable FGMatrix33 mTec2i;
  mutable FGMatrix33 mTi2l;
  mutable FGMatrix33 mTl2i;

  double epa;

  /* Terms for geodetic latitude calculation. Values are from WGS84 model */
  double a;    // Earth semimajor axis in feet
  double e2;   // Earth eccentricity squared
  double c;
  double ec;
  double ec2;

  mutable bool mCacheValid;

  static FGGroundCallback_ptr GroundCallback;
};

inline FGLocation operator*(double scalar, const FGLocation& l)
{
  return l.operator*(scalar);
}

} // namespace JSBSim

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif