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.53 2010/11/28 13:02:43 bcoconni 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.53 2010/11/28 13:02:43 bcoconni 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> dqPQRidot;
 156     deque <FGColumnVector3> dqUVWidot;
 157     deque <FGColumnVector3> dqInertialVelocity;
 158     deque <FGQuaternion>    dqQtrndot;
 159   };
 160
 161   /** Constructor.
 162       The constructor initializes several variables, and sets the initial set
 163       of integrators to use as follows:
 164       - integrator, rotational rate = Adams Bashforth 2
 165       - integrator, translational rate = Adams Bashforth 2
 166       - integrator, rotational position = Trapezoidal
 167       - integrator, translational position = Trapezoidal
 168       @param Executive a pointer to the parent executive object */
 169   FGPropagate(FGFDMExec* Executive);
 170
 171   /// Destructor
 172   ~FGPropagate();
 173
 174   /// These define the indices use to select the various integrators.
 175   enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
 176
 177   /// These define the indices use to select the gravitation models.
 178   enum eGravType {gtStandard, gtWGS84};
 179
 180   /** Initializes the FGPropagate class after instantiation and prior to first execution.
 181       The base class FGModel::InitModel is called first, initializing pointers to the
 182       other FGModel objects (and others).  */
 183   bool InitModel(void);
 184
 185   /** Runs the Propagate model; called by the Executive.
 186       @return false if no error */
 187   bool Run(void);
 188
 189   const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
 190
 191   /** Retrieves the velocity vector.
 192       The vector returned is represented by an FGColumnVector reference. The vector
 193       for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 194       is 1-based, so that the first element can be retrieved using the "()" operator.
 195       In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
 196       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 197       eNorth=1, eEast=2, eDown=3.
 198       units ft/sec
 199       @return The vehicle velocity vector with respect to the Earth centered frame,
 200               expressed in Local horizontal frame.
 201   */
 202   const FGColumnVector3& GetVel(void) const { return vVel; }
 203
 204   /** Retrieves the body frame vehicle velocity vector.
 205       The vector returned is represented by an FGColumnVector reference. The vector
 206       for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
 207       is 1-based, so that the first element can be retrieved using the "()" operator.
 208       In other words, vUVW(1) is Vx. Various convenience enumerators are defined
 209       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 210       eX=1, eY=2, eZ=3.
 211       units ft/sec
 212       @return The body frame vehicle velocity vector in ft/sec.
 213   */
 214   const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
 215
 216   /** Retrieves the body axis acceleration.
 217       Retrieves the computed body axis accelerations based on the
 218       applied forces and accounting for a rotating body frame.
 219       The vector returned is represented by an FGColumnVector reference. The vector
 220       for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
 221       is 1-based, so that the first element can be retrieved using the "()" operator.
 222       In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
 223       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 224       eX=1, eY=2, eZ=3.
 225       units ft/sec^2
 226       @return Body axis translational acceleration in ft/sec^2.
 227   */
 228   const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
 229
 230   /** Retrieves the body angular rates vector, relative to the ECEF frame.
 231       Retrieves the body angular rates (p, q, r), which are calculated by integration
 232       of the angular acceleration.
 233       The vector returned is represented by an FGColumnVector reference. The vector
 234       for the angular velocity in Body frame is organized (P, Q, R). The vector
 235       is 1-based, so that the first element can be retrieved using the "()" operator.
 236       In other words, vPQR(1) is P. Various convenience enumerators are defined
 237       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 238       eP=1, eQ=2, eR=3.
 239       units rad/sec
 240       @return The body frame angular rates in rad/sec.
 241   */
 242   const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
 243
 244   /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
 245       Retrieves the body angular rates (p, q, r), which are calculated by integration
 246       of the angular acceleration.
 247       The vector returned is represented by an FGColumnVector reference. The vector
 248       for the angular velocity in Body frame is organized (P, Q, R). The vector
 249       is 1-based, so that the first element can be retrieved using the "()" operator.
 250       In other words, vPQR(1) is P. Various convenience enumerators are defined
 251       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 252       eP=1, eQ=2, eR=3.
 253       units rad/sec
 254       @return The body frame inertial angular rates in rad/sec.
 255   */
 256   const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
 257
 258   /** Retrieves the body axis angular acceleration vector.
 259       Retrieves the body axis angular acceleration vector in rad/sec^2. The
 260       angular acceleration vector is determined from the applied forces and
 261       accounts for a rotating frame.
 262       The vector returned is represented by an FGColumnVector reference. The vector
 263       for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
 264       is 1-based, so that the first element can be retrieved using the "()" operator.
 265       In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
 266       in FGJSBBase. The relevant enumerators for the vector returned by this call are,
 267       eP=1, eQ=2, eR=3.
 268       units rad/sec^2
 269       @return The angular acceleration vector.
 270   */
 271   const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
 272
 273   /** Retrieves the Euler angles that define the vehicle orientation.
 274       Extracts the Euler angles from the quaternion that stores the orientation
 275       in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
 276       vector returned is represented by an FGColumnVector reference. The vector
 277       for the Euler angles is organized (Phi, Theta, Psi). The vector
 278       is 1-based, so that the first element can be retrieved using the "()" operator.
 279       In other words, the returned vector item with subscript (1) is Phi.
 280       Various convenience enumerators are defined in FGJSBBase. The relevant
 281       enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
 282       units radians
 283       @return The Euler angle vector, where the first item in the
 284               vector is the angle about the X axis, the second is the
 285               angle about the Y axis, and the third item is the angle
 286               about the Z axis (Phi, Theta, Psi).
 287   */
 288   const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
 289
 290   /** Retrieves a body frame velocity component.
 291       Retrieves a body frame velocity component. The velocity returned is
 292       extracted from the vUVW vector (an FGColumnVector). The vector for the
 293       velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
 294       In other words, GetUVW(1) returns Vx. Various convenience enumerators
 295       are defined in FGJSBBase. The relevant enumerators for the velocity
 296       returned by this call are, eX=1, eY=2, eZ=3.
 297       units ft/sec
 298       @param idx the index of the velocity component desired (1-based).
 299       @return The body frame velocity component.
 300   */
 301   double GetUVW   (int idx) const { return VState.vUVW(idx); }
 302
 303   /** Retrieves a body frame acceleration component.
 304       Retrieves a body frame acceleration component. The acceleration returned
 305       is extracted from the vUVWdot vector (an FGColumnVector). The vector for
 306       the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
 307       1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
 308       enumerators are defined in FGJSBBase. The relevant enumerators for the
 309       acceleration returned by this call are, eX=1, eY=2, eZ=3.
 310       units ft/sec^2
 311       @param idx the index of the acceleration component desired (1-based).
 312       @return The body frame acceleration component.
 313   */
 314   double GetUVWdot(int idx) const { return vUVWdot(idx); }
 315
 316   /** Retrieves a Local frame velocity component.
 317       Retrieves a Local frame velocity component. The velocity returned is
 318       extracted from the vVel vector (an FGColumnVector). The vector for the
 319       velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
 320       is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
 321       enumerators are defined in FGJSBBase. The relevant enumerators for the
 322       velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
 323       units ft/sec
 324       @param idx the index of the velocity component desired (1-based).
 325       @return The body frame velocity component.
 326   */
 327   double GetVel(int idx) const { return vVel(idx); }
 328
 329   /** Retrieves the total inertial velocity in ft/sec.
 330   */
 331   double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 332
 333   /** Retrieves the inertial velocity vector in ft/sec.
 334   */
 335   const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
 336
 337   /** Retrieves the inertial position vector.
 338   */
 339   const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
 340
 341   /** Returns the current altitude above sea level.
 342       This function returns the altitude above sea level.
 343       units ft
 344       @return The current altitude above sea level in feet.
 345   */
 346   double GetAltitudeASL(void)   const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
 347
 348   /** Returns the current altitude above sea level.
 349       This function returns the altitude above sea level.
 350       units meters
 351       @return The current altitude above sea level in meters.
 352   */
 353   double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 354
 355   /** Retrieves a body frame angular velocity component relative to the ECEF frame.
 356       Retrieves a body frame angular velocity component. The angular velocity
 357       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 358       for the angular velocity in Body frame is organized (P, Q, R). The vector
 359       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 360       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 361       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 362       units rad/sec
 363       @param axis the index of the angular velocity component desired (1-based).
 364       @return The body frame angular velocity component.
 365   */
 366   double GetPQR(int axis) const {return VState.vPQR(axis);}
 367
 368   /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
 369       Retrieves a body frame angular velocity component. The angular velocity
 370       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 371       for the angular velocity in Body frame is organized (P, Q, R). The vector
 372       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 373       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 374       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 375       units rad/sec
 376       @param axis the index of the angular velocity component desired (1-based).
 377       @return The body frame angular velocity component.
 378   */
 379   double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 380
 381   /** Retrieves a body frame angular acceleration component.
 382       Retrieves a body frame angular acceleration component. The angular
 383       acceleration returned is extracted from the vPQRdot vector (an
 384       FGColumnVector). The vector for the angular acceleration in Body frame
 385       is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
 386       GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
 387       enumerators are defined in FGJSBBase. The relevant enumerators for the
 388       angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
 389       units rad/sec^2
 390       @param axis the index of the angular acceleration component desired (1-based).
 391       @return The body frame angular acceleration component.
 392   */
 393   double GetPQRdot(int axis) const {return vPQRdot(axis);}
 394
 395   /** Retrieves a vehicle Euler angle component.
 396       Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
 397       stores the vehicle orientation relative to the Local frame. The order of
 398       rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
 399       Phi. Various convenience enumerators are defined in FGJSBBase. The
 400       relevant enumerators for the Euler angle returned by this call are,
 401       ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
 402       units radians
 403       @return An Euler angle.
 404   */
 405   double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 406
 407   /** Retrieves the cosine of a vehicle Euler angle component.
 408       Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
 409       quaternion that stores the vehicle orientation relative to the Local frame.
 410       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 411       with subscript (1) is Phi. Various convenience enumerators are defined in
 412       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 413       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
 414       units none
 415       @return The cosine of an Euler angle.
 416   */
 417   double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 418
 419   /** Retrieves the sine of a vehicle Euler angle component.
 420       Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
 421       quaternion that stores the vehicle orientation relative to the Local frame.
 422       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 423       with subscript (1) is Phi. Various convenience enumerators are defined in
 424       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 425       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
 426       units none
 427       @return The sine of an Euler angle.
 428   */
 429   double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 430
 431   /** Returns the current altitude rate.
 432       Returns the current altitude rate (rate of climb).
 433       units ft/sec
 434       @return The current rate of change in altitude.
 435   */
 436   double Gethdot(void) const { return -vVel(eDown); }
 437
 438   /** Returns the "constant" LocalTerrainRadius.
 439       The LocalTerrainRadius parameter is set by the calling application or set to
 440       sea level + terrain elevation if JSBSim is running in standalone mode.
 441       units feet
 442       @return distance of the local terrain from the center of the earth.
 443       */
 444   double GetLocalTerrainRadius(void) const;
 445
 446   double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
 447   double GetTerrainElevation(void) const;
 448   double GetDistanceAGL(void)  const;
 449   double GetRadius(void) const {
 450       if (VState.vLocation.GetRadius() == 0) return 1.0;
 451       else return VState.vLocation.GetRadius();
 452   }
 453   double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
 454   double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
 455
 456   double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
 457   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 458
 459   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
 460
 461   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
 462   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
 463   const FGLocation& GetLocation(void) const { return VState.vLocation; }
 464
 465   /** Retrieves the local-to-body transformation matrix.
 466       The quaternion class, being the means by which the orientation of the
 467       vehicle is stored, manages the local-to-body transformation matrix.
 468       @return a reference to the local-to-body transformation matrix.  */
 469   const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
 470
 471   /** Retrieves the body-to-local transformation matrix.
 472       The quaternion class, being the means by which the orientation of the
 473       vehicle is stored, manages the body-to-local transformation matrix.
 474       @return a reference to the body-to-local matrix.  */
 475   const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
 476
 477   /** Retrieves the ECEF-to-body transformation matrix.
 478       @return a reference to the ECEF-to-body transformation matrix.  */
 479   const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 480
 481   /** Retrieves the body-to-ECEF transformation matrix.
 482       @return a reference to the body-to-ECEF matrix.  */
 483   const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 484
 485   /** Retrieves the ECI-to-body transformation matrix.
 486       @return a reference to the ECI-to-body transformation matrix.  */
 487   const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
 488
 489   /** Retrieves the body-to-ECI transformation matrix.
 490       @return a reference to the body-to-ECI matrix.  */
 491   const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
 492
 493   /** Retrieves the ECEF-to-ECI transformation matrix.
 494       @return a reference to the ECEF-to-ECI transformation matrix.  */
 495   const FGMatrix33& GetTec2i(void);
 496
 497   /** Retrieves the ECI-to-ECEF transformation matrix.
 498       @return a reference to the ECI-to-ECEF matrix.  */
 499   const FGMatrix33& GetTi2ec(void);
 500
 501   /** Retrieves the ECEF-to-local transformation matrix.
 502       Retrieves the ECEF-to-local transformation matrix. Note that the so-called
 503       local from is also know as the NED frame (for North, East, Down).
 504       @return a reference to the ECEF-to-local matrix.  */
 505   const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
 506
 507   /** Retrieves the local-to-ECEF transformation matrix.
 508       Retrieves the local-to-ECEF transformation matrix. Note that the so-called
 509       local from is also know as the NED frame (for North, East, Down).
 510       @return a reference to the local-to-ECEF matrix.  */
 511   const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
 512
 513   /** Retrieves the local-to-inertial transformation matrix.
 514       @return a reference to the local-to-inertial transformation matrix.  */
 515   const FGMatrix33& GetTl2i(void)  { return VState.vLocation.GetTl2i(); }
 516
 517   /** Retrieves the inertial-to-local transformation matrix.
 518       @return a reference to the inertial-to-local matrix.  */
 519   const FGMatrix33& GetTi2l(void)  { return VState.vLocation.GetTi2l(); }
 520
 521   VehicleState* GetVState(void) { return &VState; }
 522
 523   void SetVState(VehicleState* vstate) {
 524       VState.vLocation = vstate->vLocation;
 525       UpdateLocationMatrices();
 526       SetInertialOrientation(vstate->qAttitudeECI);
 527       VehicleRadius = GetRadius();
 528       VState.vUVW = vstate->vUVW;
 529       vVel = GetTb2l() * VState.vUVW;
 530       VState.vPQR = vstate->vPQR;
 531       VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
 532       VState.vInertialPosition = vstate->vInertialPosition;
 533
 534       InitializeDerivatives();
 535   }
 536
 537   void InitializeDerivatives(void);
 538
 539   void SetInertialOrientation(FGQuaternion Qi);
 540   void SetInertialVelocity(FGColumnVector3 Vi);
 541   void SetInertialRates(FGColumnVector3 vRates);
 542
 543   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 544
 545   void SetPQR(unsigned int i, double val) {
 546       if ((i>=1) && (i<=3) )
 547           VState.vPQR(i) = val;
 548   }
 549
 550   void SetUVW(unsigned int i, double val) {
 551       if ((i>=1) && (i<=3) )
 552           VState.vUVW(i) = val;
 553   }
 554
 555 // SET functions
 556
 557   void SetLongitude(double lon) {
 558       VState.vLocation.SetLongitude(lon);
 559       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 560       UpdateLocationMatrices();
 561   }
 562   void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
 563   void SetLatitude(double lat) {
 564       VState.vLocation.SetLatitude(lat);
 565       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 566       UpdateLocationMatrices();
 567   }
 568   void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
 569   void SetRadius(double r) {
 570       VState.vLocation.SetRadius(r);
 571       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 572       UpdateLocationMatrices();
 573   }
 574   void SetAltitudeASL(double altASL);
 575   void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
 576   void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
 577   void SetTerrainElevation(double tt);
 578   void SetDistanceAGL(double tt);
 579   void SetInitialState(const FGInitialCondition *);
 580   void SetPosition(const double Lon, const double Lat, const double Radius) {
 581       VState.vLocation.SetPosition(Lon, Lat, Radius);
 582       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 583       VehicleRadius = GetRadius();
 584       UpdateLocationMatrices();
 585   }
 586   void SetLocation(const FGLocation& l) {
 587       VState.vLocation = l;
 588       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 589       UpdateLocationMatrices();
 590   }
 591   void SetLocation(const FGColumnVector3& l) {
 592       VState.vLocation = l;
 593       VState.vInertialPosition = GetTec2i() * VState.vLocation;
 594       UpdateLocationMatrices();
 595   }
 596
 597   void RecomputeLocalTerrainRadius(void);
 598
 599   void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
 600     vDeltaXYZEC = GetTb2ec()*deltaLoc;
 601     VState.vLocation -= vDeltaXYZEC;
 602   }
 603
 604   struct LagrangeMultiplier {
 605     FGColumnVector3 ForceJacobian;
 606     FGColumnVector3 MomentJacobian;
 607     double Min;
 608     double Max;
 609     double value;
 610   };
 611
 612   void DumpState(void);
 613
 614 private:
 615
 616 // state vector
 617
 618   struct VehicleState VState;
 619
 620   FGColumnVector3 vVel;
 621   FGColumnVector3 vPQRdot;
 622   FGColumnVector3 vPQRidot;
 623   FGColumnVector3 vUVWdot, vUVWidot;
 624   FGColumnVector3 vInertialVelocity;
 625   FGColumnVector3 vLocation;
 626   FGColumnVector3 vDeltaXYZEC;
 627   FGColumnVector3 vGravAccel;
 628   FGColumnVector3 vOmegaEarth;  // The Earth angular velocity vector
 629   FGQuaternion vQtrndot;
 630   FGMatrix33 Tec2b;
 631   FGMatrix33 Tb2ec;
 632   FGMatrix33 Tl2b;   // local to body frame matrix copy for immediate local use
 633   FGMatrix33 Tb2l;   // body to local frame matrix copy for immediate local use
 634   FGMatrix33 Tl2ec;  // local to ECEF matrix copy for immediate local use
 635   FGMatrix33 Tec2l;  // ECEF to local frame matrix copy for immediate local use
 636   FGMatrix33 Tec2i;  // ECEF to ECI frame matrix copy for immediate local use
 637   FGMatrix33 Ti2ec;  // ECI to ECEF frame matrix copy for immediate local use
 638   FGMatrix33 Ti2b;   // ECI to body frame rotation matrix
 639   FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
 640   FGMatrix33 Ti2l;
 641   FGMatrix33 Tl2i;
 642
 643   double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
 644   FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
 645   eIntegrateType integrator_rotational_rate;
 646   eIntegrateType integrator_translational_rate;
 647   eIntegrateType integrator_rotational_position;
 648   eIntegrateType integrator_translational_position;
 649   int gravType;
 650
 651   void CalculatePQRdot(void);
 652   void CalculateQuatdot(void);
 653   void CalculateInertialVelocity(void);
 654   void CalculateUVW(void);
 655   void CalculateUVWdot(void);
 656
 657   void Integrate( FGColumnVector3& Integrand,
 658                   FGColumnVector3& Val,
 659                   deque <FGColumnVector3>& ValDot,
 660                   double dt,
 661                   eIntegrateType integration_type);
 662
 663   void Integrate( FGQuaternion& Integrand,
 664                   FGQuaternion& Val,
 665                   deque <FGQuaternion>& ValDot,
 666                   double dt,
 667                   eIntegrateType integration_type);
 668
 669   void EvaluateRateToResistTo(FGColumnVector3& vdot,
 670                               const FGColumnVector3& Val,
 671                               const FGColumnVector3& ValDot,
 672                               const FGColumnVector3& LocalTerrainVal,
 673                               deque <FGColumnVector3>& dqValDot,
 674                               const double dt,
 675                               const eIntegrateType integration_type);
 676
 677   void ResolveFrictionForces(double dt);
 678
 679   void UpdateLocationMatrices(void);
 680   void UpdateBodyMatrices(void);
 681
 682   void bind(void);
 683   void Debug(int from);
 684 };
 685 }
 686 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 687
 688 #include "initialization/FGInitialCondition.h"
 689
 690 #endif