src/FDM/JSBSim/math/FGLocation.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGLocation.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 04/04/2004
   6  Purpose:      Store an arbitrary location on the globe
   7
   8  ------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) ------------------
   9  -------           (C) 2004  Mathias Froehlich (Mathias.Froehlich@web.de) ----
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 ------------------------------------------------------------------------------
  30 This class encapsulates an arbitrary position in the globe with its accessors.
  31 It has vector properties, so you can add multiply ....
  32
  33 HISTORY
  34 ------------------------------------------------------------------------------
  35 04/04/2004   MF    Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #ifdef FGFS
  42 #  include <simgear/compiler.h>
  43 #  ifdef SG_HAVE_STD_INCLUDES
  44 #    include <cmath>
  45 #  else
  46 #    include <math.h>
  47 #  endif
  48 #else
  49 #  if defined(sgi) && !defined(__GNUC__)
  50 #    include <math.h>
  51 #  else
  52 #    include <cmath>
  53 #  endif
  54 #endif
  55
  56 #include "FGLocation.h"
  57 #include <input_output/FGPropertyManager.h>
  58
  59 namespace JSBSim {
  60
  61 static const char *IdSrc = "$Id$";
  62 static const char *IdHdr = ID_LOCATION;
  63
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  65 CLASS IMPLEMENTATION
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  67
  68 FGLocation::FGLocation(double lon, double lat, double radius)
  69 {
  70   mCacheValid = false;
  71
  72   double sinLat = sin(lat);
  73   double cosLat = cos(lat);
  74   double sinLon = sin(lon);
  75   double cosLon = cos(lon);
  76   mECLoc = FGColumnVector3( radius*cosLat*cosLon,
  77                             radius*cosLat*sinLon,
  78                             radius*sinLat );
  79 }
  80
  81 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82
  83 void FGLocation::SetLongitude(double longitude)
  84 {
  85   double rtmp = mECLoc.Magnitude(eX, eY);
  86   // Check if we have zero radius.
  87   // If so set it to 1, so that we can set a position
  88   if (0.0 == mECLoc.Magnitude())
  89     rtmp = 1.0;
  90
  91   // Fast return if we are on the north or south pole ...
  92   if (rtmp == 0.0)
  93     return;
  94
  95   mCacheValid = false;
  96
  97   mECLoc(eX) = rtmp*cos(longitude);
  98   mECLoc(eY) = rtmp*sin(longitude);
  99 }
 100
 101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 102
 103 void FGLocation::SetLatitude(double latitude)
 104 {
 105   mCacheValid = false;
 106
 107   double r = mECLoc.Magnitude();
 108   if (r == 0.0) {
 109     mECLoc(eX) = 1.0;
 110     r = 1.0;
 111   }
 112
 113   double rtmp = mECLoc.Magnitude(eX, eY);
 114   if (rtmp != 0.0) {
 115     double fac = r/rtmp*cos(latitude);
 116     mECLoc(eX) *= fac;
 117     mECLoc(eY) *= fac;
 118   } else {
 119     mECLoc(eX) = r*cos(latitude);
 120     mECLoc(eY) = 0.0;
 121   }
 122   mECLoc(eZ) = r*sin(latitude);
 123 }
 124
 125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 126
 127 void FGLocation::SetRadius(double radius)
 128 {
 129   mCacheValid = false;
 130
 131   double rold = mECLoc.Magnitude();
 132   if (rold == 0.0)
 133     mECLoc(eX) = radius;
 134   else
 135     mECLoc *= radius/rold;
 136 }
 137
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139
 140 void FGLocation::ComputeDerivedUnconditional(void) const
 141 {
 142   // The radius is just the Euclidean norm of the vector.
 143   mRadius = mECLoc.Magnitude();
 144
 145   // The distance of the location to the y-axis, which is the axis
 146   // through the poles.
 147   double rxy = sqrt(mECLoc(eX)*mECLoc(eX) + mECLoc(eY)*mECLoc(eY));
 148
 149   // Compute the sin/cos values of the longitude
 150   double sinLon, cosLon;
 151   if (rxy == 0.0) {
 152     sinLon = 0.0;
 153     cosLon = 1.0;
 154   } else {
 155     sinLon = mECLoc(eY)/rxy;
 156     cosLon = mECLoc(eX)/rxy;
 157   }
 158
 159   // Compute the sin/cos values of the latitude
 160   double sinLat, cosLat;
 161   if (mRadius == 0.0)  {
 162     sinLat = 0.0;
 163     cosLat = 1.0;
 164   } else {
 165     sinLat = mECLoc(eZ)/mRadius;
 166     cosLat = rxy/mRadius;
 167   }
 168
 169   // Compute the longitude and latitude itself
 170   if ( mECLoc( eX ) == 0.0 && mECLoc( eY ) == 0.0 )
 171     mLon = 0.0;
 172   else
 173     mLon = atan2( mECLoc( eY ), mECLoc( eX ) );
 174
 175   if ( rxy == 0.0 && mECLoc( eZ ) == 0.0 )
 176     mLat = 0.0;
 177   else
 178     mLat = atan2( mECLoc(eZ), rxy );
 179
 180   // Compute the transform matrices from and to the earth centered frame.
 181   // see Durham Chapter 4, problem 1, page 52
 182   mTec2l = FGMatrix33( -cosLon*sinLat, -sinLon*sinLat,  cosLat,
 183                            -sinLon   ,     cosLon    ,    0.0 ,
 184                        -cosLon*cosLat, -sinLon*cosLat, -sinLat  );
 185
 186   mTl2ec = mTec2l.Transposed();
 187
 188   // Mark the cached values as valid
 189   mCacheValid = true;
 190 }
 191
 192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 193
 194 void FGLocation::bind(FGPropertyManager* PropertyManager, const string& prefix) const
 195 {
 196   PropertyManager->Tie(prefix + "lat-gc-rad", (FGLocation*)this,
 197                        &FGLocation::GetLatitude);
 198   PropertyManager->Tie(prefix + "lat-gc-deg", (FGLocation*)this,
 199                        &FGLocation::GetLatitudeDeg);
 200   PropertyManager->Tie(prefix + "long-gc-rad", (FGLocation*)this,
 201                        &FGLocation::GetLongitude);
 202   PropertyManager->Tie(prefix + "long-gc-deg", (FGLocation*)this,
 203                        &FGLocation::GetLongitudeDeg);
 204   PropertyManager->Tie(prefix + "radius-ft", (FGLocation*)this,
 205                        &FGLocation::GetRadius);
 206 }
 207
 208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 209
 210 void FGLocation::unbind(FGPropertyManager* PropertyManager, const string& prefix) const
 211 {
 212   PropertyManager->Untie(prefix + "lat-gc-rad");
 213   PropertyManager->Untie(prefix + "lat-gc-deg");
 214   PropertyManager->Untie(prefix + "long-gc-rad");
 215   PropertyManager->Untie(prefix + "long-gc-deg");
 216   PropertyManager->Untie(prefix + "radius-ft");
 217 }
 218
 219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 220
 221 } // namespace JSBSim