DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@endcode
@author Jon S. Berndt, Mathias Froehlich
- @version $Id: FGPropagate.h,v 1.41 2010/07/09 04:11:45 jberndt Exp $
+ @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
units rad/sec */
FGColumnVector3 vPQRi;
+ /** The angular velocity vector for the vehicle body frame relative to the
+ ECI frame, expressed in the ECI frame.
+ units rad/sec */
+ FGColumnVector3 vPQRi_i;
+
/** The current orientation of the vehicle, that is, the orientation of the
body frame relative to the local, NED frame. */
FGQuaternion qAttitudeLocal;
FGColumnVector3 vInertialPosition;
- deque <FGColumnVector3> dqPQRdot;
- deque <FGColumnVector3> dqUVWdot;
+ deque <FGColumnVector3> dqPQRidot;
+ deque <FGColumnVector3> dqUVWidot;
deque <FGColumnVector3> dqInertialVelocity;
- deque <FGQuaternion> dqQtrndot;
+ deque <FGQuaternion> dqQtrndot;
};
/** Constructor.
@return false if no error */
bool Run(void);
- void CalculatePQRdot(void);
- void CalculateQuatdot(void);
- void CalculateInertialVelocity(void);
- void CalculateUVWdot(void);
const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
/** Retrieves the velocity vector.
void SetVState(VehicleState* vstate) {
VState.vLocation = vstate->vLocation;
+ UpdateLocationMatrices();
+ SetInertialOrientation(vstate->qAttitudeECI);
+ VehicleRadius = GetRadius();
VState.vUVW = vstate->vUVW;
+ vVel = GetTb2l() * VState.vUVW;
VState.vPQR = vstate->vPQR;
- VState.qAttitudeLocal = vstate->qAttitudeLocal;
- VState.qAttitudeECI = vstate->qAttitudeECI;
+ VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+ VState.vInertialPosition = vstate->vInertialPosition;
- 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));
+ InitializeDerivatives();
}
+ 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 SetLongitude(double lon) {
+ VState.vLocation.SetLongitude(lon);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
+ void SetLatitude(double lat) {
+ VState.vLocation.SetLatitude(lat);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
+ void SetRadius(double r) {
+ VState.vLocation.SetRadius(r);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
void SetAltitudeASL(double altASL);
void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
void SetTerrainElevation(double tt);
void SetDistanceAGL(double tt);
void SetInitialState(const FGInitialCondition *);
+ void SetPosition(const double Lon, const double Lat, const double Radius) {
+ VState.vLocation.SetPosition(Lon, Lat, Radius);
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ VehicleRadius = GetRadius();
+ UpdateLocationMatrices();
+ }
+ void SetLocation(const FGLocation& l) {
+ VState.vLocation = l;
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+ void SetLocation(const FGColumnVector3& l) {
+ VState.vLocation = l;
+ VState.vInertialPosition = GetTec2i() * VState.vLocation;
+ UpdateLocationMatrices();
+ }
+
void RecomputeLocalTerrainRadius(void);
void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
VState.vLocation -= vDeltaXYZEC;
}
+ struct LagrangeMultiplier {
+ FGColumnVector3 ForceJacobian;
+ FGColumnVector3 MomentJacobian;
+ double Min;
+ double Max;
+ double value;
+ };
+
+ void DumpState(void);
+
private:
// state vector
FGColumnVector3 vVel;
FGColumnVector3 vPQRdot;
- FGColumnVector3 vUVWdot;
+ FGColumnVector3 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;
eIntegrateType integrator_translational_position;
int gravType;
+ void CalculatePQRdot(void);
+ void CalculateQuatdot(void);
+ void CalculateInertialVelocity(void);
+ void CalculateUVW(void);
+ void CalculateUVWdot(void);
+
void Integrate( FGColumnVector3& Integrand,
FGColumnVector3& Val,
deque <FGColumnVector3>& ValDot,
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 bind(void);
void Debug(int from);
};