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.43 2010/07/25 15:35:11 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.43 2010/07/25 15:35:11 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> dqPQRdot;
 151     deque <FGColumnVector3> dqUVWdot;
 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   /** Returns the current altitude above sea level.
 337       This function returns the altitude above sea level.
 338       units ft
 339       @return The current altitude above sea level in feet.
 340   */
 341   double GetAltitudeASL(void)   const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
 342
 343   /** Returns the current altitude above sea level.
 344       This function returns the altitude above sea level.
 345       units meters
 346       @return The current altitude above sea level in meters.
 347   */
 348   double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
 349
 350   /** Retrieves a body frame angular velocity component relative to the ECEF frame.
 351       Retrieves a body frame angular velocity component. The angular velocity
 352       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 353       for the angular velocity in Body frame is organized (P, Q, R). The vector
 354       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 355       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 356       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 357       units rad/sec
 358       @param axis the index of the angular velocity component desired (1-based).
 359       @return The body frame angular velocity component.
 360   */
 361   double GetPQR(int axis) const {return VState.vPQR(axis);}
 362
 363   /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
 364       Retrieves a body frame angular velocity component. The angular velocity
 365       returned is extracted from the vPQR vector (an FGColumnVector). The vector
 366       for the angular velocity in Body frame is organized (P, Q, R). The vector
 367       is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
 368       convenience enumerators are defined in FGJSBBase. The relevant enumerators
 369       for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
 370       units rad/sec
 371       @param axis the index of the angular velocity component desired (1-based).
 372       @return The body frame angular velocity component.
 373   */
 374   double GetPQRi(int axis) const {return VState.vPQRi(axis);}
 375
 376   /** Retrieves a body frame angular acceleration component.
 377       Retrieves a body frame angular acceleration component. The angular
 378       acceleration returned is extracted from the vPQRdot vector (an
 379       FGColumnVector). The vector for the angular acceleration in Body frame
 380       is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
 381       GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
 382       enumerators are defined in FGJSBBase. The relevant enumerators for the
 383       angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
 384       units rad/sec^2
 385       @param axis the index of the angular acceleration component desired (1-based).
 386       @return The body frame angular acceleration component.
 387   */
 388   double GetPQRdot(int axis) const {return vPQRdot(axis);}
 389
 390   /** Retrieves a vehicle Euler angle component.
 391       Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
 392       stores the vehicle orientation relative to the Local frame. The order of
 393       rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
 394       Phi. Various convenience enumerators are defined in FGJSBBase. The
 395       relevant enumerators for the Euler angle returned by this call are,
 396       ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
 397       units radians
 398       @return An Euler angle.
 399   */
 400   double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
 401
 402   /** Retrieves the cosine of a vehicle Euler angle component.
 403       Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
 404       quaternion that stores the vehicle orientation relative to the Local frame.
 405       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 406       with subscript (1) is Phi. Various convenience enumerators are defined in
 407       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 408       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
 409       units none
 410       @return The cosine of an Euler angle.
 411   */
 412   double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
 413
 414   /** Retrieves the sine of a vehicle Euler angle component.
 415       Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
 416       quaternion that stores the vehicle orientation relative to the Local frame.
 417       The order of rotations used is Yaw-Pitch-Roll. The Euler angle
 418       with subscript (1) is Phi. Various convenience enumerators are defined in
 419       FGJSBBase. The relevant enumerators for the Euler angle referred to in this
 420       call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
 421       units none
 422       @return The sine of an Euler angle.
 423   */
 424   double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
 425
 426   /** Returns the current altitude rate.
 427       Returns the current altitude rate (rate of climb).
 428       units ft/sec
 429       @return The current rate of change in altitude.
 430   */
 431   double Gethdot(void) const { return -vVel(eDown); }
 432
 433   /** Returns the "constant" LocalTerrainRadius.
 434       The LocalTerrainRadius parameter is set by the calling application or set to
 435       sea level + terrain elevation if JSBSim is running in standalone mode.
 436       units feet
 437       @return distance of the local terrain from the center of the earth.
 438       */
 439   double GetLocalTerrainRadius(void) const;
 440
 441   double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
 442   double GetTerrainElevation(void) const;
 443   double GetDistanceAGL(void)  const;
 444   double GetRadius(void) const {
 445       if (VState.vLocation.GetRadius() == 0) return 1.0;
 446       else return VState.vLocation.GetRadius();
 447   }
 448   double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
 449   double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
 450
 451   double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
 452   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 453
 454   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
 455
 456   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
 457   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
 458   const FGLocation& GetLocation(void) const { return VState.vLocation; }
 459
 460   /** Retrieves the local-to-body transformation matrix.
 461       The quaternion class, being the means by which the orientation of the
 462       vehicle is stored, manages the local-to-body transformation matrix.
 463       @return a reference to the local-to-body transformation matrix.  */
 464   const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
 465
 466   /** Retrieves the body-to-local transformation matrix.
 467       The quaternion class, being the means by which the orientation of the
 468       vehicle is stored, manages the body-to-local transformation matrix.
 469       @return a reference to the body-to-local matrix.  */
 470   const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
 471
 472   /** Retrieves the ECEF-to-body transformation matrix.
 473       @return a reference to the ECEF-to-body transformation matrix.  */
 474   const FGMatrix33& GetTec2b(void) const { return Tec2b; }
 475
 476   /** Retrieves the body-to-ECEF transformation matrix.
 477       @return a reference to the body-to-ECEF matrix.  */
 478   const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
 479
 480   /** Retrieves the ECI-to-body transformation matrix.
 481       @return a reference to the ECI-to-body transformation matrix.  */
 482   const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
 483
 484   /** Retrieves the body-to-ECI transformation matrix.
 485       @return a reference to the body-to-ECI matrix.  */
 486   const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
 487
 488   /** Retrieves the ECEF-to-ECI transformation matrix.
 489       @return a reference to the ECEF-to-ECI transformation matrix.  */
 490   const FGMatrix33& GetTec2i(void);
 491
 492   /** Retrieves the ECI-to-ECEF transformation matrix.
 493       @return a reference to the ECI-to-ECEF matrix.  */
 494   const FGMatrix33& GetTi2ec(void);
 495
 496   /** Retrieves the ECEF-to-local transformation matrix.
 497       Retrieves the ECEF-to-local transformation matrix. Note that the so-called
 498       local from is also know as the NED frame (for North, East, Down).
 499       @return a reference to the ECEF-to-local matrix.  */
 500   const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
 501
 502   /** Retrieves the local-to-ECEF transformation matrix.
 503       Retrieves the local-to-ECEF 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 local-to-ECEF matrix.  */
 506   const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
 507
 508   /** Retrieves the local-to-inertial transformation matrix.
 509       @return a reference to the local-to-inertial transformation matrix.  */
 510   const FGMatrix33& GetTl2i(void)  { return VState.vLocation.GetTl2i(); }
 511
 512   /** Retrieves the inertial-to-local transformation matrix.
 513       @return a reference to the inertial-to-local matrix.  */
 514   const FGMatrix33& GetTi2l(void)  { return VState.vLocation.GetTi2l(); }
 515
 516   VehicleState* GetVState(void) { return &VState; }
 517
 518   void SetVState(VehicleState* vstate) {
 519       VState.vLocation = vstate->vLocation;
 520       VState.vUVW = vstate->vUVW;
 521       VState.vPQR = vstate->vPQR;
 522       VState.qAttitudeLocal = vstate->qAttitudeLocal;
 523       VState.qAttitudeECI = vstate->qAttitudeECI;
 524
 525       VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 526       VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 527       VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 528       VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 529   }
 530
 531   void SetInertialOrientation(FGQuaternion Qi);
 532   void SetInertialVelocity(FGColumnVector3 Vi);
 533
 534   const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
 535
 536   void SetPQR(unsigned int i, double val) {
 537       if ((i>=1) && (i<=3) )
 538           VState.vPQR(i) = val;
 539   }
 540
 541   void SetUVW(unsigned int i, double val) {
 542       if ((i>=1) && (i<=3) )
 543           VState.vUVW(i) = val;
 544   }
 545
 546 // SET functions
 547
 548   void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
 549   void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
 550   void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
 551   void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
 552   void SetRadius(double r) { VState.vLocation.SetRadius(r); }
 553   void SetLocation(const FGLocation& l) { VState.vLocation = l; }
 554   void SetAltitudeASL(double altASL);
 555   void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
 556   void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
 557   void SetTerrainElevation(double tt);
 558   void SetDistanceAGL(double tt);
 559   void SetInitialState(const FGInitialCondition *);
 560   void RecomputeLocalTerrainRadius(void);
 561
 562   void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
 563     vDeltaXYZEC = GetTb2ec()*deltaLoc;
 564     VState.vLocation -= vDeltaXYZEC;
 565   }
 566
 567   struct LagrangeMultiplier {
 568     FGColumnVector3 ForceJacobian;
 569     FGColumnVector3 MomentJacobian;
 570     double Min;
 571     double Max;
 572     double value;
 573   };
 574
 575 private:
 576
 577 // state vector
 578
 579   struct VehicleState VState;
 580
 581   FGColumnVector3 vVel;
 582   FGColumnVector3 vPQRdot;
 583   FGColumnVector3 vUVWdot;
 584   FGColumnVector3 vInertialVelocity;
 585   FGColumnVector3 vLocation;
 586   FGColumnVector3 vDeltaXYZEC;
 587   FGColumnVector3 vGravAccel;
 588   FGColumnVector3 vOmegaEarth;  // The Earth angular velocity vector
 589   FGQuaternion vQtrndot;
 590   FGMatrix33 Tec2b;
 591   FGMatrix33 Tb2ec;
 592   FGMatrix33 Tl2b;   // local to body frame matrix copy for immediate local use
 593   FGMatrix33 Tb2l;   // body to local frame matrix copy for immediate local use
 594   FGMatrix33 Tl2ec;  // local to ECEF matrix copy for immediate local use
 595   FGMatrix33 Tec2l;  // ECEF to local frame matrix copy for immediate local use
 596   FGMatrix33 Tec2i;  // ECEF to ECI frame matrix copy for immediate local use
 597   FGMatrix33 Ti2ec;  // ECI to ECEF frame matrix copy for immediate local use
 598   FGMatrix33 Ti2b;   // ECI to body frame rotation matrix
 599   FGMatrix33 Tb2i;   // body to ECI frame rotation matrix
 600   FGMatrix33 Ti2l;
 601   FGMatrix33 Tl2i;
 602   FGLocation contactloc;
 603   FGColumnVector3 dv;
 604
 605   double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
 606   double radInv;
 607   eIntegrateType integrator_rotational_rate;
 608   eIntegrateType integrator_translational_rate;
 609   eIntegrateType integrator_rotational_position;
 610   eIntegrateType integrator_translational_position;
 611   int gravType;
 612
 613   void CalculatePQRdot(void);
 614   void CalculateQuatdot(void);
 615   void CalculateInertialVelocity(void);
 616   void CalculateUVWdot(void);
 617
 618   void Integrate( FGColumnVector3& Integrand,
 619                   FGColumnVector3& Val,
 620                   deque <FGColumnVector3>& ValDot,
 621                   double dt,
 622                   eIntegrateType integration_type);
 623
 624   void Integrate( FGQuaternion& Integrand,
 625                   FGQuaternion& Val,
 626                   deque <FGQuaternion>& ValDot,
 627                   double dt,
 628                   eIntegrateType integration_type);
 629
 630   void ResolveFrictionForces(double dt);
 631
 632   void bind(void);
 633   void Debug(int from);
 634 };
 635 }
 636 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 637
 638 #include "initialization/FGInitialCondition.h"
 639
 640 #endif