const FGMatrix33& FGMassBalance::CalculatePMInertias(void)
{
size_t size = PointMasses.size();
if (size == 0) return pmJ;
pmJ = FGMatrix33();
for (unsigned int i=0; i<size; i++) {
pmJ += GetPointmassInertia( lbtoslug * PointMasses[i]->Weight, PointMasses[i]->Location );
pmJ += PointMasses[i]->GetPointMassInertia();
}
return pmJ;
}
bool FGMassBalance::Run(bool Holding)
{
double denom, k1, k2, k3, k4, k5, k6;
double Ixx, Iyy, Izz, Ixy, Ixz, Iyz;
if (FGModel::Run(Holding)) return true;
if (Holding) return false;
RunPreFunctions();
double ChildFDMWeight = 0.0;
for (int fdm=0; fdm<FDMExec->GetFDMCount(); fdm++) {
if (FDMExec->GetChildFDM(fdm)->mated) ChildFDMWeight += FDMExec->GetChildFDM(fdm)->exec->GetMassBalance()->GetWeight();
}
Weight = EmptyWeight + in.TanksWeight + GetTotalPointMassWeight()
+ in.GasMass*slugtolb + ChildFDMWeight;
Mass = lbtoslug*Weight;
// Calculate new CG
vXYZcg = (EmptyWeight*vbaseXYZcg
+ GetPointMassMoment()
+ in.TanksMoment
+ in.GasMoment) / Weight;
// Track frame-by-frame delta CG, and move the EOM-tracked location
// by this amount.
if (vLastXYZcg.Magnitude() == 0.0) vLastXYZcg = vXYZcg;
vDeltaXYZcg = vXYZcg - vLastXYZcg;
vDeltaXYZcgBody = StructuralToBody(vLastXYZcg) - StructuralToBody(vXYZcg);
vLastXYZcg = vXYZcg;
FDMExec->GetPropagate()->NudgeBodyLocation(vDeltaXYZcgBody);
// Calculate new total moments of inertia
// At first it is the base configuration inertia matrix ...
mJ = baseJ;
// ... with the additional term originating from the parallel axis theorem.
mJ += GetPointmassInertia( lbtoslug * EmptyWeight, vbaseXYZcg );
// Then add the contributions from the additional pointmasses.
mJ += CalculatePMInertias();
mJ += in.TankInertia;
mJ += in.GasInertia;
Ixx = mJ(1,1);
Iyy = mJ(2,2);
Izz = mJ(3,3);
Ixy = -mJ(1,2);
Ixz = -mJ(1,3);
Iyz = -mJ(2,3);
// Calculate inertia matrix inverse (ref. Stevens and Lewis, "Flight Control & Simulation")
k1 = (Iyy*Izz - Iyz*Iyz);
k2 = (Iyz*Ixz + Ixy*Izz);
k3 = (Ixy*Iyz + Iyy*Ixz);
denom = 1.0/(Ixx*k1 - Ixy*k2 - Ixz*k3 );
k1 = k1*denom;
k2 = k2*denom;
k3 = k3*denom;
k4 = (Izz*Ixx - Ixz*Ixz)*denom;
k5 = (Ixy*Ixz + Iyz*Ixx)*denom;
k6 = (Ixx*Iyy - Ixy*Ixy)*denom;
mJinv.InitMatrix( k1, k2, k3,
k2, k4, k5,
k3, k5, k6 );
RunPostFunctions();
Debug(0);
return false;
}
