1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
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
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
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.
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.
27 --------------------------------------------------------------------------------
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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"
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 #define ID_PROPAGATE "$Id: FGPropagate.h,v 1.55 2011/01/16 16:10:59 bcoconni Exp $"
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
61 class FGInitialCondition;
63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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.
72 The general execution of this model follows this process:
74 -Calculate the angular accelerations
75 -Calculate the translational accelerations
76 -Calculate the angular rate
77 -Calculate the translational velocity
79 -Integrate accelerations and rates
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:
87 simulation/integrator/rate/rotational
88 simulation/integrator/rate/translational
89 simulation/integrator/position/rotational
90 simulation/integrator/position/translational
93 Each of the integrators listed above can be set to one of the following values:
96 0: No integrator (Freeze)
104 @author Jon S. Berndt, Mathias Froehlich
105 @version $Id: FGPropagate.h,v 1.55 2011/01/16 16:10:59 bcoconni Exp $
108 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
112 class FGPropagate : public FGModel {
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
121 FGLocation vLocation;
123 /** The velocity vector of the vehicle with respect to the ECEF frame,
124 expressed in the body system.
126 FGColumnVector3 vUVW;
128 /** The angular velocity vector for the vehicle relative to the ECEF frame,
129 expressed in the body frame.
131 FGColumnVector3 vPQR;
133 /** The angular velocity vector for the vehicle body frame relative to the
134 ECI frame, expressed in the body frame.
136 FGColumnVector3 vPQRi;
138 /** The angular velocity vector for the vehicle body frame relative to the
139 ECI frame, expressed in the ECI frame.
141 FGColumnVector3 vPQRi_i;
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;
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;
151 FGColumnVector3 vInertialVelocity;
153 FGColumnVector3 vInertialPosition;
155 deque <FGColumnVector3> dqPQRidot;
156 deque <FGColumnVector3> dqUVWidot;
157 deque <FGColumnVector3> dqInertialVelocity;
158 deque <FGQuaternion> dqQtrndot;
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);
174 /// These define the indices use to select the various integrators.
175 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
177 /// These define the indices use to select the gravitation models.
178 enum eGravType {gtStandard, gtWGS84};
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);
185 /** Runs the Propagate model; called by the Executive.
186 @return false if no error */
189 const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
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.
199 @return The vehicle velocity vector with respect to the Earth centered frame,
200 expressed in Local horizontal frame.
202 const FGColumnVector3& GetVel(void) const { return vVel; }
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,
212 @return The body frame vehicle velocity vector in ft/sec.
214 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
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,
226 @return Body axis translational acceleration in ft/sec^2.
228 const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
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,
240 @return The body frame angular rates in rad/sec.
242 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
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,
254 @return The body frame inertial angular rates in rad/sec.
256 const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
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,
269 @return The angular acceleration vector.
271 const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
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.
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).
288 const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
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.
298 @param idx the index of the velocity component desired (1-based).
299 @return The body frame velocity component.
301 double GetUVW (int idx) const { return VState.vUVW(idx); }
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.
311 @param idx the index of the acceleration component desired (1-based).
312 @return The body frame acceleration component.
314 double GetUVWdot(int idx) const { return vUVWdot(idx); }
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.
324 @param idx the index of the velocity component desired (1-based).
325 @return The body frame velocity component.
327 double GetVel(int idx) const { return vVel(idx); }
329 /** Retrieves the total inertial velocity in ft/sec.
331 double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
333 /** Retrieves the inertial velocity vector in ft/sec.
335 const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
337 /** Retrieves the inertial position vector.
339 const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
341 /** Returns the current altitude above sea level.
342 This function returns the altitude above sea level.
344 @return The current altitude above sea level in feet.
346 double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
348 /** Returns the current altitude above sea level.
349 This function returns the altitude above sea level.
351 @return The current altitude above sea level in meters.
353 double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
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.
363 @param axis the index of the angular velocity component desired (1-based).
364 @return The body frame angular velocity component.
366 double GetPQR(int axis) const {return VState.vPQR(axis);}
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.
376 @param axis the index of the angular velocity component desired (1-based).
377 @return The body frame angular velocity component.
379 double GetPQRi(int axis) const {return VState.vPQRi(axis);}
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.
390 @param axis the index of the angular acceleration component desired (1-based).
391 @return The body frame angular acceleration component.
393 double GetPQRdot(int axis) const {return vPQRdot(axis);}
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).
403 @return An Euler angle.
405 double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
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)).
415 @return The cosine of an Euler angle.
417 double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
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)).
427 @return The sine of an Euler angle.
429 double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
431 /** Returns the current altitude rate.
432 Returns the current altitude rate (rate of climb).
434 @return The current rate of change in altitude.
436 double Gethdot(void) const { return -vVel(eDown); }
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.
442 @return distance of the local terrain from the center of the earth.
444 double GetLocalTerrainRadius(void) const { return LocalTerrainRadius; }
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();
453 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
454 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
456 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
457 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
459 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
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; }
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 Tl2b; }
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 Tb2l; }
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; }
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; }
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 Ti2b; }
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 Tb2i; }
493 /** Retrieves the ECEF-to-ECI transformation matrix.
494 @return a reference to the ECEF-to-ECI transformation matrix. */
495 const FGMatrix33& GetTec2i(void) const { return Tec2i; }
497 /** Retrieves the ECI-to-ECEF transformation matrix.
498 @return a reference to the ECI-to-ECEF matrix. */
499 const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
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 Tec2l; }
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 Tl2ec; }
513 /** Retrieves the local-to-inertial transformation matrix.
514 @return a reference to the local-to-inertial transformation matrix. */
515 const FGMatrix33& GetTl2i(void) const { return Tl2i; }
517 /** Retrieves the inertial-to-local transformation matrix.
518 @return a reference to the inertial-to-local matrix. */
519 const FGMatrix33& GetTi2l(void) const { return Ti2l; }
521 const VehicleState& GetVState(void) const { return VState; }
523 void SetVState(const VehicleState& vstate);
525 void InitializeDerivatives(void);
527 void SetInertialOrientation(FGQuaternion Qi);
528 void SetInertialVelocity(FGColumnVector3 Vi);
529 void SetInertialRates(FGColumnVector3 vRates);
531 const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
533 void SetPQR(unsigned int i, double val) {
534 if ((i>=1) && (i<=3) )
535 VState.vPQR(i) = val;
538 void SetUVW(unsigned int i, double val) {
539 if ((i>=1) && (i<=3) )
540 VState.vUVW(i) = val;
545 void SetLongitude(double lon)
547 VState.vLocation.SetLongitude(lon);
548 UpdateVehicleState();
550 void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
551 void SetLatitude(double lat)
553 VState.vLocation.SetLatitude(lat);
554 UpdateVehicleState();
556 void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
557 void SetRadius(double r)
559 VState.vLocation.SetRadius(r);
561 VState.vInertialPosition = Tec2i * VState.vLocation;
563 void SetAltitudeASL(double altASL) { SetRadius(altASL + SeaLevelRadius); }
564 void SetAltitudeASLmeters(double altASL) { SetRadius(altASL/fttom + SeaLevelRadius); }
565 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
566 void SetTerrainElevation(double tt);
567 void SetDistanceAGL(double tt) { SetRadius(tt + LocalTerrainRadius); }
568 void SetInitialState(const FGInitialCondition *);
569 void SetLocation(const FGLocation& l);
570 void SetLocation(const FGColumnVector3& lv)
572 FGLocation l = FGLocation(lv);
575 void SetPosition(const double Lon, const double Lat, const double Radius)
577 FGLocation l = FGLocation(Lon, Lat, Radius);
581 void RecomputeLocalTerrainRadius(void);
583 void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
584 vDeltaXYZEC = Tb2ec*deltaLoc;
585 VState.vLocation -= vDeltaXYZEC;
588 struct LagrangeMultiplier {
589 FGColumnVector3 ForceJacobian;
590 FGColumnVector3 MomentJacobian;
596 void DumpState(void);
602 struct VehicleState VState;
604 FGColumnVector3 vVel;
605 FGColumnVector3 vPQRdot;
606 FGColumnVector3 vPQRidot;
607 FGColumnVector3 vUVWdot, vUVWidot;
608 FGColumnVector3 vInertialVelocity;
609 FGColumnVector3 vLocation;
610 FGColumnVector3 vDeltaXYZEC;
611 FGColumnVector3 vGravAccel;
612 FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
613 FGQuaternion vQtrndot;
616 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
617 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
618 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
619 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
620 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
621 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
622 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
623 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
627 double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
628 FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
629 eIntegrateType integrator_rotational_rate;
630 eIntegrateType integrator_translational_rate;
631 eIntegrateType integrator_rotational_position;
632 eIntegrateType integrator_translational_position;
635 void CalculatePQRdot(void);
636 void CalculateQuatdot(void);
637 void CalculateInertialVelocity(void);
638 void CalculateUVW(void);
639 void CalculateUVWdot(void);
641 void Integrate( FGColumnVector3& Integrand,
642 FGColumnVector3& Val,
643 deque <FGColumnVector3>& ValDot,
645 eIntegrateType integration_type);
647 void Integrate( FGQuaternion& Integrand,
649 deque <FGQuaternion>& ValDot,
651 eIntegrateType integration_type);
653 void EvaluateRateToResistTo(FGColumnVector3& vdot,
654 const FGColumnVector3& Val,
655 const FGColumnVector3& ValDot,
656 const FGColumnVector3& LocalTerrainVal,
657 deque <FGColumnVector3>& dqValDot,
659 const eIntegrateType integration_type);
661 void ResolveFrictionForces(double dt);
663 void UpdateLocationMatrices(void);
664 void UpdateBodyMatrices(void);
665 void UpdateVehicleState(void);
668 void Debug(int from);
671 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%