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