DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.58 2011/04/03 19:24:58 jberndt Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@endcode
@author Jon S. Berndt, Mathias Froehlich
- @version $Id: FGPropagate.h,v 1.43 2010/07/25 15:35:11 jberndt Exp $
+ @version $Id: FGPropagate.h,v 1.58 2011/04/03 19:24:58 jberndt Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGColumnVector3 vInertialPosition;
- deque <FGColumnVector3> dqPQRdot;
- deque <FGColumnVector3> dqUVWdot;
+ deque <FGColumnVector3> dqPQRidot;
+ deque <FGColumnVector3> dqUVWidot;
deque <FGColumnVector3> dqInertialVelocity;
- deque <FGQuaternion> dqQtrndot;
+ deque <FGQuaternion> dqQtrndot;
};
/** Constructor.
*/
const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
+ /** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame.
+ */
+ const FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; }
+
/** Returns the current altitude above sea level.
This function returns the altitude above sea level.
units ft
units feet
@return distance of the local terrain from the center of the earth.
*/
- double GetLocalTerrainRadius(void) const;
+ double GetLocalTerrainRadius(void) const { return LocalTerrainRadius; }
double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
double GetTerrainElevation(void) const;
The quaternion class, being the means by which the orientation of the
vehicle is stored, manages the local-to-body transformation matrix.
@return a reference to the local-to-body transformation matrix. */
- const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
+ const FGMatrix33& GetTl2b(void) const { return Tl2b; }
/** Retrieves the body-to-local transformation matrix.
The quaternion class, being the means by which the orientation of the
vehicle is stored, manages the body-to-local transformation matrix.
@return a reference to the body-to-local matrix. */
- const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
+ const FGMatrix33& GetTb2l(void) const { return Tb2l; }
/** Retrieves the ECEF-to-body transformation matrix.
@return a reference to the ECEF-to-body transformation matrix. */
/** Retrieves the ECI-to-body transformation matrix.
@return a reference to the ECI-to-body transformation matrix. */
- const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
+ const FGMatrix33& GetTi2b(void) const { return Ti2b; }
/** Retrieves the body-to-ECI transformation matrix.
@return a reference to the body-to-ECI matrix. */
- const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
+ const FGMatrix33& GetTb2i(void) const { return Tb2i; }
/** Retrieves the ECEF-to-ECI transformation matrix.
@return a reference to the ECEF-to-ECI transformation matrix. */
- const FGMatrix33& GetTec2i(void);
+ const FGMatrix33& GetTec2i(void) const { return Tec2i; }
/** Retrieves the ECI-to-ECEF transformation matrix.
@return a reference to the ECI-to-ECEF matrix. */
- const FGMatrix33& GetTi2ec(void);
+ const FGMatrix33& GetTi2ec(void) const { return Ti2ec; }
/** Retrieves the ECEF-to-local transformation matrix.
Retrieves the ECEF-to-local transformation matrix. Note that the so-called
local from is also know as the NED frame (for North, East, Down).
@return a reference to the ECEF-to-local matrix. */
- const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
+ const FGMatrix33& GetTec2l(void) const { return Tec2l; }
/** Retrieves the local-to-ECEF transformation matrix.
Retrieves the local-to-ECEF transformation matrix. Note that the so-called
local from is also know as the NED frame (for North, East, Down).
@return a reference to the local-to-ECEF matrix. */
- const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
+ const FGMatrix33& GetTl2ec(void) const { return Tl2ec; }
/** Retrieves the local-to-inertial transformation matrix.
@return a reference to the local-to-inertial transformation matrix. */
- const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
+ const FGMatrix33& GetTl2i(void) const { return Tl2i; }
/** Retrieves the inertial-to-local transformation matrix.
@return a reference to the inertial-to-local matrix. */
- const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
+ const FGMatrix33& GetTi2l(void) const { return Ti2l; }
- VehicleState* GetVState(void) { return &VState; }
+ const VehicleState& GetVState(void) const { return VState; }
- void SetVState(VehicleState* vstate) {
- VState.vLocation = vstate->vLocation;
- VState.vUVW = vstate->vUVW;
- VState.vPQR = vstate->vPQR;
- VState.qAttitudeLocal = vstate->qAttitudeLocal;
- VState.qAttitudeECI = vstate->qAttitudeECI;
+ void SetVState(const VehicleState& vstate);
- VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
- VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
- }
+ void InitializeDerivatives(void);
void SetInertialOrientation(FGQuaternion Qi);
void SetInertialVelocity(FGColumnVector3 Vi);
+ void SetInertialRates(FGColumnVector3 vRates);
const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
// SET functions
- void SetLongitude(double lon) { VState.vLocation.SetLongitude(lon); }
- void SetLongitudeDeg(double lon) {SetLongitude(lon*degtorad);}
- void SetLatitude(double lat) { VState.vLocation.SetLatitude(lat); }
- void SetLatitudeDeg(double lat) {SetLatitude(lat*degtorad);}
- void SetRadius(double r) { VState.vLocation.SetRadius(r); }
- void SetLocation(const FGLocation& l) { VState.vLocation = l; }
- void SetAltitudeASL(double altASL);
- void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
+ void SetLongitude(double lon)
+ {
+ VState.vLocation.SetLongitude(lon);
+ UpdateVehicleState();
+ }
+ void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
+ void SetLatitude(double lat)
+ {
+ VState.vLocation.SetLatitude(lat);
+ UpdateVehicleState();
+ }
+ void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
+ void SetRadius(double r)
+ {
+ VState.vLocation.SetRadius(r);
+ VehicleRadius = r;
+ VState.vInertialPosition = Tec2i * VState.vLocation;
+ }
+ void SetAltitudeASL(double altASL) { SetRadius(altASL + SeaLevelRadius); }
+ void SetAltitudeASLmeters(double altASL) { SetRadius(altASL/fttom + SeaLevelRadius); }
void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
void SetTerrainElevation(double tt);
- void SetDistanceAGL(double tt);
+ void SetDistanceAGL(double tt) { SetRadius(tt + LocalTerrainRadius); }
void SetInitialState(const FGInitialCondition *);
+ void SetLocation(const FGLocation& l);
+ void SetLocation(const FGColumnVector3& lv)
+ {
+ FGLocation l = FGLocation(lv);
+ SetLocation(l);
+ }
+ void SetPosition(const double Lon, const double Lat, const double Radius)
+ {
+ FGLocation l = FGLocation(Lon, Lat, Radius);
+ SetLocation(l);
+ }
+
void RecomputeLocalTerrainRadius(void);
void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
- vDeltaXYZEC = GetTb2ec()*deltaLoc;
- VState.vLocation -= vDeltaXYZEC;
+ VState.vInertialPosition -= Tb2i*deltaLoc;
+ VState.vLocation -= Tb2ec*deltaLoc;
}
struct LagrangeMultiplier {
double value;
};
+ void DumpState(void);
+
private:
// state vector
struct VehicleState VState;
FGColumnVector3 vVel;
- FGColumnVector3 vPQRdot;
- FGColumnVector3 vUVWdot;
+ FGColumnVector3 vPQRdot, vPQRidot;
+ FGColumnVector3 vUVWdot, vUVWidot;
FGColumnVector3 vInertialVelocity;
FGColumnVector3 vLocation;
FGColumnVector3 vDeltaXYZEC;
FGMatrix33 Tb2i; // body to ECI frame rotation matrix
FGMatrix33 Ti2l;
FGMatrix33 Tl2i;
- FGLocation contactloc;
- FGColumnVector3 dv;
double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
- double radInv;
+ FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
eIntegrateType integrator_rotational_rate;
eIntegrateType integrator_translational_rate;
eIntegrateType integrator_rotational_position;
void CalculatePQRdot(void);
void CalculateQuatdot(void);
void CalculateInertialVelocity(void);
+ void CalculateUVW(void);
void CalculateUVWdot(void);
void Integrate( FGColumnVector3& Integrand,
double dt,
eIntegrateType integration_type);
+ void EvaluateRateToResistTo(FGColumnVector3& vdot,
+ const FGColumnVector3& Val,
+ const FGColumnVector3& ValDot,
+ const FGColumnVector3& LocalTerrainVal,
+ deque <FGColumnVector3>& dqValDot,
+ const double dt,
+ const eIntegrateType integration_type);
+
void ResolveFrictionForces(double dt);
+ void UpdateLocationMatrices(void);
+ void UpdateBodyMatrices(void);
+ void UpdateVehicleState(void);
+
void bind(void);
void Debug(int from);
};
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-#include "initialization/FGInitialCondition.h"
-
#endif
projects / flightgear.git / blobdiff