src/FDM/JSBSim/models/FGPropagate.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Header:       FGPropagate.h
   4  Author:       Jon S. Berndt
   5  Date started: 1/5/99
   6
   7  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 HISTORY
  27 --------------------------------------------------------------------------------
  28 01/05/99   JSB   Created
  29
  30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  31 SENTRY
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  33
  34 #ifndef FGPROPAGATE_H
  35 #define FGPROPAGATE_H
  36
  37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 INCLUDES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  40
  41 #include "models/FGModel.h"
  42 #include "math/FGColumnVector3.h"
  43 #include "math/FGLocation.h"
  44 #include "math/FGQuaternion.h"
  45 #include "math/FGMatrix33.h"
  46 #include <deque>
  47
  48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  49 DEFINITIONS
  50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  51
  52 #define ID_PROPAGATE "$Id: FGPropagate.h,v 1.51 2010/10/07 03:45:40 jberndt Exp $"
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55 FORWARD DECLARATIONS
  56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 namespace JSBSim {
  59
  60 using std::deque;
  61 class FGInitialCondition;
  62
  63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  64 CLASS DOCUMENTATION
  65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  66
  67 /** Models the EOM and integration/propagation of state.
  68     The Equations of Motion (EOM) for JSBSim are integrated to propagate the
  69     state of the vehicle given the forces and moments that act on it. The
  70     integration accounts for a rotating Earth.
  71
  72     The general execution of this model follows this process:
  73
  74     -Calculate the angular accelerations
  75     -Calculate the translational accelerations
  76     -Calculate the angular rate
  77     -Calculate the translational velocity
  78
  79     -Integrate accelerations and rates
  80
  81     Integration of rotational and translation position and rate can be
  82     customized as needed or frozen by the selection of no integrator. The
  83     selection of which integrator to use is done through the setting of
  84     the associated property. There are four properties which can be set:
  85
  86     @code
  87     simulation/integrator/rate/rotational
  88     simulation/integrator/rate/translational
  89     simulation/integrator/position/rotational
  90     simulation/integrator/position/translational
  91     @endcode
  92
  93     Each of the integrators listed above can be set to one of the following values:
  94
  95     @code
  96     0: No integrator (Freeze)
  97     1: Rectangular Euler
  98     2: Trapezoidal
  99     3: Adams Bashforth 2
 100     4: Adams Bashforth 3
 101     5: Adams Bashforth 4
 102     @endcode
 103
 104     @author Jon S. Berndt, Mathias Froehlich
 105     @version $Id: FGPropagate.h,v 1.51 2010/10/07 03:45:40 jberndt Exp $
 106   */
 107
 108 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109 CLASS DECLARATION
 110 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 111
 112 class FGPropagate : public FGModel {
 113 public:
 114
 115   /** The current vehicle state vector structure contains the translational and
 116     angular position, and the translational and angular velocity. */
 117   struct VehicleState {
 118     /** Represents the current location of the vehicle in Earth centered Earth
 119         fixed (ECEF) frame.
 120         units ft */
 121     FGLocation vLocation;
 122
 123     /** The velocity vector of the vehicle with respect to the ECEF frame,
 124         expressed in the body system.
 125         units ft/sec */
 126     FGColumnVector3 vUVW;
 127
 128     /** The angular velocity vector for the vehicle relative to the ECEF frame,
 129         expressed in the body frame.
 130         units rad/sec */
 131     FGColumnVector3 vPQR;
 132
 133     /** The angular velocity vector for the vehicle body frame relative to the
 134         ECI frame, expressed in the body frame.
 135         units rad/sec */
 136     FGColumnVector3 vPQRi;
 137
 138     /** The angular velocity vector for the vehicle body frame relative to the
 139         ECI frame, expressed in the ECI frame.
 140         units rad/sec */
 141     FGColumnVector3 vPQRi_i;
 142
 143     /** The current orientation of the vehicle, that is, the orientation of the
 144         body frame relative to the local, NED frame. */
 145     FGQuaternion qAttitudeLocal;
 146
 147     /** The current orientation of the vehicle, that is, the orientation of the
 148         body frame relative to the inertial (ECI) frame. */
 149     FGQuaternion qAttitudeECI;
 150
 151     FGColumnVector3 vInertialVelocity;
 152
 153     FGColumnVector3 vInertialPosition;
 154
 155     deque <FGColumnVector3> dqPQRdot;
 156     deque <FGColumnVector3> dqPQRidot;
 157     deque <FGColumnVector3> dqUVWidot;
 158     deque <FGColumnVector3> dqInertialVelocity;
 159     deque <FGQuaternion>    dqQtrndot;
 160   };
 161
 162   /** Constructor.
 163       The constructor initializes several variables, and sets the initial set
 164       of integrators to use as follows:
 165       - integrator, rotational rate = Adams Bashforth 2
 166       - integrator, translational rate = Adams Bashforth 2
 167       - integrator, rotational position = Trapezoidal
 168       - integrator, translational position = Trapezoidal
 169       @param Executive a pointer to the parent executive object */
 170   FGPropagate(FGFDMExec* Executive);
 171
 172   /// Destructor
 173   ~FGPropagate();
 174
 175   /// These define the indices use to select the various integrators.
 176   enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
 177
 178   /// These define the indices use to select the gravitation models.
 179   enum eGravType {gtStandard, gtWGS84};
 180
 181   /** Initializes the FGPropagate class after instantiation and prior to first execution.
 182       The base class FGModel::InitModel is called first, initializing pointers to the
 183       other FGModel objects (and others).  */
 184   bool InitModel(void);
 185
 186   /** Runs the Propagate model; called by the Executive.
 187       @return false if no error */
 188   bool Run(void);
 189
 190   const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
 191
 192   /** Retrieves the velocity vector.
 193       The vector returned is represented by an FGColumnVector reference. The vector
 194       for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 195       is 1-based, so that the first element can be retrieved using the "()" operator.
 196       In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
 197       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 198       eNorth=1, eEast=2, eDown=3.
 199       units ft/sec
 200       @return The vehicle velocity vector with respect to the Earth centered frame,
 201               expressed in Local horizontal frame.
 202   */
 203   const FGColumnVector3& GetVel(void) const { return vVel; }
 204
 205   /** Retrieves the body frame vehicle velocity vector.
 206       The vector returned is represented by an FGColumnVector reference. The vector
 207       for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
 208       is 1-based, so that the first element can be retrieved using the "()" operator.
 209       In other words, vUVW(1) is Vx. Various convenience enumerators are defined
 210       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 211       eX=1, eY=2, eZ=3.
 212       units ft/sec
 213       @return The body frame vehicle velocity vector in ft/sec.
 214   */
 215   const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
 216
 217   /** Retrieves the body axis acceleration.
 218       Retrieves the computed body axis accelerations based on the
 219       applied forces and accounting for a rotating body frame.
 220       The vector returned is represented by an FGColumnVector reference. The vector
 221       for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
 222       is 1-based, so that the first element can be retrieved using the "()" operator.
 223       In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
 224       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 225       eX=1, eY=2, eZ=3.
 226       units ft/sec^2
 227       @return Body axis translational acceleration in ft/sec^2.
 228   */
 229   const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
 230
 231   /** Retrieves the body angular rates vector, relative to the ECEF frame.
 232       Retrieves the body angular rates (p, q, r), which are calculated by integration
 233       of the angular acceleration.
 234       The vector returned is represented by an FGColumnVector reference. The vector
 235       for the angular velocity in Body frame is organized (P, Q, R). The vector
 236       is 1-based, so that the first element can be retrieved using the "()" operator.
 237       In other words, vPQR(1) is P. Various convenience enumerators are defined
 238       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 239       eP=1, eQ=2, eR=3.
 240       units rad/sec
 241       @return The body frame angular rates in rad/sec.
 242   */
 243   const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
 244
 245   /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
 246       Retrieves the body angular rates (p, q, r), which are calculated by integration
 247       of the angular acceleration.
 248       The vector returned is represented by an FGColumnVector reference. The vector
 249       for the angular velocity in Body frame is organized (P, Q, R). The vector
 250       is 1-based, so that the first element can be retrieved using the "()" operator.
 251       In other words, vPQR(1) is P. Various convenience enumerators are defined
 252       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 253       eP=1, eQ=2, eR=3.
 254       units rad/sec
 255       @return The body frame inertial angular rates in rad/sec.
 256   */
 257   const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
 258
 259   /** Retrieves the body axis angular acceleration vector.
 260       Retrieves the body axis angular acceleration vector in rad/sec^2. The
 261       angular acceleration vector is determined from the applied forces and
 262       accounts for a rotating frame.
 263       The vector returned is represented by an FGColumnVector reference. The vector
 264       for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
 265       is 1-based, so that the first element can be retrieved using the "()" operator.
 266       In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
 267       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 268       eP=1, eQ=2, eR=3.
 269       units rad/sec^2
 270       @return The angular acceleration vector.
 271   */
 272   const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
 273
 274   /** Retrieves the Euler angles that define the vehicle orientation.
 275       Extracts the Euler angles from the quaternion that stores the orientation
 276       in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
 277       vector returned is represented by an FGColumnVector reference. The vector
 278       for the Euler angles is organized (Phi, Theta, Psi). The vector
 279       is 1-based, so that the first element can be retrieved using the "()" operator.
 280       In other words, the returned vector item with subscript (1) is Phi.
 281       Various convenience enumerators are defined in FGJSBBase. The relevant
 282       enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
 283       units radians
 284       @return The Euler angle vector, where the first item in the
 285               vector is the angle about the X axis, the second is the
 286               angle about the Y axis, and the third item is the angle
 287               about the Z axis (Phi, Theta, Psi).
 288   */
 289   const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
 290
 291   /** Retrieves a body frame velocity component.
 292       Retrieves a body frame velocity component. The velocity returned is
 293       extracted from the vUVW vector (an FGColumnVector). The vector for the
 294       velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
 295       In other words, GetUVW(1) returns Vx. Various convenience enumerators
 296       are defined in FGJSBBase. The relevant enumerators for the velocity
 297       returned by this call are, eX=1, eY=2, eZ=3.
 298       units ft/sec
 299       @param idx the index of the velocity component desired (1-based).
 300       @return The body frame velocity component.
 301   */
 302   double GetUVW   (int idx) const { return VState.vUVW(idx); }
 303
 304   /** Retrieves a body frame acceleration component.
 305       Retrieves a body frame acceleration component. The acceleration returned
 306       is extracted from the vUVWdot vector (an FGColumnVector). The vector for
 307       the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
 308       1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
 309       enumerators are defined in FGJSBBase. The relevant enumerators for the
 310       acceleration returned by this call are, eX=1, eY=2, eZ=3.
 311       units ft/sec^2
 312       @param idx the index of the acceleration component desired (1-based).
 313       @return The body frame acceleration component.
 314   */
 315   double GetUVWdot(int idx) const { return vUVWdot(idx); }
 316
 317   /** Retrieves a Local frame velocity component.
 318       Retrieves a Local frame velocity component. The velocity returned is
 319       extracted from the vVel vector (an FGColumnVector). The vector for the
 320       velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 321       is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
 322       enumerators are defined in FGJSBBase. The relevant enumerators for the
 323       velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
 324       units ft/sec
 325       @param idx the index of the velocity component desired (1-based).
 326       @return The body frame velocity component.
 327   */
 328   double GetVel(int idx) const { return vVel(idx); }
 329
 330   /** Retrieves the total inertial velocity in ft/sec.
 331   */
 332   double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 333
 334   /** Retrieves the inertial velocity vector in ft/sec.
 335   */
 336   const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
 337
 338   /** Retrieves the inertial position vector.
 339   */
 340   const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
 341
 342   /** Returns the current altitude above sea level.
 343       This function returns the altitude above sea level.
 344       units ft
 345       @return The current altitude above sea level in feet.
 346   */
 347   double GetAltitudeASL(void)   const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
 348
 349   /** Returns the current altitude above sea level.
 350       This function returns the altitude above sea level.
 351       units meters
 352       @return The current altitude above sea level in meters.
 353   */
 354   double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 355
 356   /** Retrieves a body frame angular velocity component relative to the ECEF frame.
 357       Retrieves a body frame angular velocity component. The angular velocity
 358       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 359       for the angular velocity in Body frame is organized (P, Q, R). The vector
 360       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 361       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 362       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 363       units rad/sec
 364       @param axis the index of the angular velocity component desired (1-based).
 365       @return The body frame angular velocity component.
 366   */
 367   double GetPQR(int axis) const {return VState.vPQR(axis);}
 368
 369   /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
 370       Retrieves a body frame angular velocity component. The angular velocity
 371       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 372       for the angular velocity in Body frame is organized (P, Q, R). The vector
 373       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 374       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 375       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 376       units rad/sec
 377       @param axis the index of the angular velocity component desired (1-based).
 378       @return The body frame angular velocity component.
 379   */
 380   double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 381
 382   /** Retrieves a body frame angular acceleration component.
 383       Retrieves a body frame angular acceleration component. The angular
 384       acceleration returned is extracted from the vPQRdot vector (an
 385       FGColumnVector). The vector for the angular acceleration in Body frame
 386       is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
 387       GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
 388       enumerators are defined in FGJSBBase. The relevant enumerators for the
 389       angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
 390       units rad/sec^2
 391       @param axis the index of the angular acceleration component desired (1-based).
 392       @return The body frame angular acceleration component.
 393   */
 394   double GetPQRdot(int axis) const {return vPQRdot(axis);}
 395
 396   /** Retrieves a vehicle Euler angle component.
 397       Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
 398       stores the vehicle orientation relative to the Local frame. The order of
 399       rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
 400       Phi. Various convenience enumerators are defined in FGJSBBase. The
 401       relevant enumerators for the Euler angle returned by this call are,
 402       ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
 403       units radians
 404       @return An Euler angle.
 405   */
 406   double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 407
 408   /** Retrieves the cosine of a vehicle Euler angle component.
 409       Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
 410       quaternion that stores the vehicle orientation relative to the Local frame.
 411       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 412       with subscript (1) is Phi. Various convenience enumerators are defined in
 413       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 414       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
 415       units none
 416       @return The cosine of an Euler angle.
 417   */
 418   double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 419
 420   /** Retrieves the sine of a vehicle Euler angle component.
 421       Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
 422       quaternion that stores the vehicle orientation relative to the Local frame.
 423       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 424       with subscript (1) is Phi. Various convenience enumerators are defined in
 425       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 426       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
 427       units none
 428       @return The sine of an Euler angle.
 429   */
 430   double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 431
 432   /** Returns the current altitude rate.
 433       Returns the current altitude rate (rate of climb).
 434       units ft/sec
 435       @return The current rate of change in altitude.
 436   */
 437   double Gethdot(void) const { return -vVel(eDown); }
 438
 439   /** Returns the "constant" LocalTerrainRadius.
 440       The LocalTerrainRadius parameter is set by the calling application or set to
 441       sea level + terrain elevation if JSBSim is running in standalone mode.
 442       units feet
 443       @return distance of the local terrain from the center of the earth.
 444       */
 445   double GetLocalTerrainRadius(void) const;
 446
 447   double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
 448   double GetTerrainElevation(void) const;
 449   double GetDistanceAGL(void)  const;
 450   double GetRadius(void) const {
 451       if (VState.vLocation.GetRadius() == 0) return 1.0;
 452       else return VState.vLocation.GetRadius();
 453   }
 454   double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
 455   double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
 456
 457   double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
 458   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 459
 460   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
 461
 462   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
 463   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
 464   const FGLocation& GetLocation(void) const { return VState.vLocation; }
 465
 466   /** Retrieves the local-to-body transformation matrix.
 467       The quaternion class, being the means by which the orientation of the
 468       vehicle is stored, manages the local-to-body transformation matrix.
 469       @return a reference to the local-to-body transformation matrix.  */
 470   const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
 471
 472   /** Retrieves the body-to-local transformation matrix.
 473       The quaternion class, being the means by which the orientation of the
 474       vehicle is stored, manages the body-to-local transformation matrix.
 475       @return a reference to the body-to-local matrix.  */
 476   const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
 477
 478   /** Retrieves the ECEF-to-body transformation matrix.
 479       @return a reference to the ECEF-to-body transformation matrix.  */
 480   const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 481
 482   /** Retrieves the body-to-ECEF transformation matrix.
 483       @return a reference to the body-to-ECEF matrix.  */
 484   const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 485
 486   /** Retrieves the ECI-to-body transformation matrix.
 487       @return a reference to the ECI-to-body transformation matrix.  */
 488   const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
 489
 490   /** Retrieves the body-to-ECI transformation matrix.
 491       @return a reference to the body-to-ECI matrix.  */
 492   const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
 493
 494   /** Retrieves the ECEF-to-ECI transformation matrix.
 495       @return a reference to the ECEF-to-ECI transformation matrix.  */
 496   const FGMatrix33& GetTec2i(void);
 497
 498   /** Retrieves the ECI-to-ECEF transformation matrix.
 499       @return a reference to the ECI-to-ECEF matrix.  */
 500   const FGMatrix33& GetTi2ec(void);
 501
 502   /** Retrieves the ECEF-to-local transformation matrix.
 503       Retrieves the ECEF-to-local transformation matrix. Note that the so-called
 504       local from is also know as the NED frame (for North, East, Down).
 505       @return a reference to the ECEF-to-local matrix.  */
 506   const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
 507
 508   /** Retrieves the local-to-ECEF transformation matrix.
 509       Retrieves the local-to-ECEF transformation matrix. Note that the so-called
 510       local from is also know as the NED frame (for North, East, Down).
 511       @return a reference to the local-to-ECEF matrix.  */
 512   const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
 513
 514   /** Retrieves the local-to-inertial transformation matrix.
 515       @return a reference to the local-to-inertial transformation matrix.  */
 516   const FGMatrix33& GetTl2i(void)  { return VState.vLocation.GetTl2i(); }
 517
 518   /** Retrieves the inertial-to-local transformation matrix.
 519       @return a reference to the inertial-to-local matrix.  */
 520   const FGMatrix33& GetTi2l(void)  { return VState.vLocation.GetTi2l(); }
 521
 522   VehicleState* GetVState(void) { return &VState; }
 523
 524   void SetVState(VehicleState* vstate) {
 525       VState.vLocation = vstate->vLocation;
 526       UpdateLocationMatrices();
 527       SetInertialOrientation(vstate->qAttitudeECI);
 528       VehicleRadius = GetRadius();
 529       VState.vUVW = vstate->vUVW;
 530       vVel = GetTb2l() * VState.vUVW;
 531       VState.vPQR = vstate->vPQR;
 532       VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
 533       VState.vInertialPosition = vstate->vInertialPosition;
 534
 535       InitializeDerivatives();
 536   }
 537
 538   void InitializeDerivatives(void);
 539
 540   void SetInertialOrientation(FGQuaternion Qi);
 541   void SetInertialVelocity(FGColumnVector3 Vi);
 542   void SetInertialRates(FGColumnVector3 vRates);
 543
 544   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 545
 546   void SetPQR(unsigned int i, double val) {
 547       if ((i>=1) && (i<=3) )
 548           VState.vPQR(i) = val;
 549   }
 550
 551   void SetUVW(unsigned int i, double val) {
 552       if ((i>=1) && (i<=3) )
 553           VState.vUVW(i) = val;
 554   }
 555
 556 // SET functions
 557
 558   void SetLongitude(double lon) {
 559       VState.vLocation.SetLongitude(lon);
 560       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 561       UpdateLocationMatrices();
 562   }
 563   void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
 564   void SetLatitude(double lat) {
 565       VState.vLocation.SetLatitude(lat);
 566       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 567       UpdateLocationMatrices();
 568   }
 569   void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
 570   void SetRadius(double r) {
 571       VState.vLocation.SetRadius(r);
 572       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 573       UpdateLocationMatrices();
 574   }
 575   void SetAltitudeASL(double altASL);
 576   void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
 577   void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
 578   void SetTerrainElevation(double tt);
 579   void SetDistanceAGL(double tt);
 580   void SetInitialState(const FGInitialCondition *);
 581   void SetPosition(const double Lon, const double Lat, const double Radius) {
 582       VState.vLocation.SetPosition(Lon, Lat, Radius);
 583       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 584       VehicleRadius = GetRadius();
 585       UpdateLocationMatrices();
 586   }
 587   void SetLocation(const FGLocation& l) {
 588       VState.vLocation = l;
 589       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 590       UpdateLocationMatrices();
 591   }
 592   void SetLocation(const FGColumnVector3& l) {
 593       VState.vLocation = l;
 594       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 595       UpdateLocationMatrices();
 596   }
 597
 598   void RecomputeLocalTerrainRadius(void);
 599
 600   void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
 601     vDeltaXYZEC = GetTb2ec()*deltaLoc;
 602     VState.vLocation -= vDeltaXYZEC;
 603   }
 604
 605   struct LagrangeMultiplier {
 606     FGColumnVector3 ForceJacobian;
 607     FGColumnVector3 MomentJacobian;
 608     double Min;
 609     double Max;
 610     double value;
 611   };
 612
 613   void DumpState(void);
 614
 615 private:
 616
 617 // state vector
 618
 619   struct VehicleState VState;
 620
 621   FGColumnVector3 vVel;
 622   FGColumnVector3 vPQRdot;
 623   FGColumnVector3 vPQRidot;
 624   FGColumnVector3 vUVWdot, vUVWidot;
 625   FGColumnVector3 vInertialVelocity;
 626   FGColumnVector3 vLocation;
 627   FGColumnVector3 vDeltaXYZEC;
 628   FGColumnVector3 vGravAccel;
 629   FGColumnVector3 vOmegaEarth;  // The Earth angular velocity vector
 630   FGQuaternion vQtrndot;
 631   FGMatrix33 Tec2b;
 632   FGMatrix33 Tb2ec;
 633   FGMatrix33 Tl2b;   // local to body frame matrix copy for immediate local use
 634   FGMatrix33 Tb2l;   // body to local frame matrix copy for immediate local use
 635   FGMatrix33 Tl2ec;  // local to ECEF matrix copy for immediate local use
 636   FGMatrix33 Tec2l;  // ECEF to local frame matrix copy for immediate local use
 637   FGMatrix33 Tec2i;  // ECEF to ECI frame matrix copy for immediate local use
 638   FGMatrix33 Ti2ec;  // ECI to ECEF frame matrix copy for immediate local use
 639   FGMatrix33 Ti2b;   // ECI to body frame rotation matrix
 640   FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
 641   FGMatrix33 Ti2l;
 642   FGMatrix33 Tl2i;
 643
 644   double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
 645   FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
 646   eIntegrateType integrator_rotational_rate;
 647   eIntegrateType integrator_translational_rate;
 648   eIntegrateType integrator_rotational_position;
 649   eIntegrateType integrator_translational_position;
 650   int gravType;
 651
 652   void CalculatePQRdot(void);
 653   void CalculateQuatdot(void);
 654   void CalculateInertialVelocity(void);
 655   void CalculateUVW(void);
 656   void CalculateUVWdot(void);
 657
 658   void Integrate( FGColumnVector3& Integrand,
 659                   FGColumnVector3& Val,
 660                   deque <FGColumnVector3>& ValDot,
 661                   double dt,
 662                   eIntegrateType integration_type);
 663
 664   void Integrate( FGQuaternion& Integrand,
 665                   FGQuaternion& Val,
 666                   deque <FGQuaternion>& ValDot,
 667                   double dt,
 668                   eIntegrateType integration_type);
 669
 670   void ResolveFrictionForces(double dt);
 671
 672   void UpdateLocationMatrices(void);
 673   void UpdateBodyMatrices(void);
 674
 675   void bind(void);
 676   void Debug(int from);
 677 };
 678 }
 679 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 680
 681 #include "initialization/FGInitialCondition.h"
 682
 683 #endif