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 <cassert>
 
#include "FGJSBBase.h"
#include "FGColumnVector3.h"
#include "FGMatrix33.h"
 
/* Setting the -ffast-math compilation flag is highly discouraged */
#ifdef __FAST_MATH__
#error Usage of -ffast-math is strongly discouraged
#endif
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
namespace JSBSim {
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
class JSBSIM_API 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);
 
  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 {
    assert(mEllipseSet);
    ComputeDerived(); return mGeodLat;
  }
 
  double GetLatitudeDeg() const { ComputeDerived(); return radtodeg*mLat; }
 
  double GetGeodLatitudeDeg(void) const {
    assert(mEllipseSet);
    ComputeDerived(); return radtodeg*mGeodLat;
  }
 
  double GetGeodAltitude(void) const {
    assert(mEllipseSet);
    ComputeDerived(); return GeodeticAltitude;
  }
 
  double GetSeaLevelRadius(void) const;
 
  double GetRadius() const { ComputeDerived(); return mRadius; }
 
  const FGMatrix33& GetTl2ec(void) const { ComputeDerived(); return mTl2ec; }
 
  const FGMatrix33& GetTec2l(void) const { ComputeDerived(); return mTec2l; }
 
  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;
  }
 
  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 {
    FGLocation result(mECLoc + l.mECLoc);
    if (mEllipseSet) result.SetEllipse(a, ec*a);
    return result;
  }
 
  FGLocation operator-(const FGLocation& l) const {
    FGLocation result(mECLoc - l.mECLoc);
    if (mEllipseSet) result.SetEllipse(a, ec*a);
    return result;
  }
 
  FGLocation operator*(double scalar) const {
    FGLocation result(scalar*mECLoc);
    if (mEllipseSet) result.SetEllipse(a, ec*a);
    return result;
  }
 
  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;
 
  /* 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;
  // Flag that checks that geodetic methods are called after SetEllipse() has
  // been called.
  bool mEllipseSet = false;
};
 
inline FGLocation operator*(double scalar, const FGLocation& l)
{
  return l.operator*(scalar);
}
 
} // namespace JSBSim
 
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#endif