static PetscErrorCode _computeIJacobian(TS /* ts */,PetscReal time,Vec X,Vec Xdot,PetscReal shift,Mat *J,Mat *B,MatStructure *mstr,void *ctx) {
PetscTimeStepper *ths = (PetscTimeStepper*)ctx;
PetscVector<Number> mX(X), mXdot(Xdot);
PetscMatrix<Number> mJ(*J), mB(*B);
ths->computeTransientImplicitJacobian(time,mX,mXdot,shift,mB);
mJ.close();
*mstr = SAME_NONZERO_PATTERN;
return 0;
}
void FGMassBalance::GetMassPropertiesReport(void) const
{
cout << endl << fgblue << highint
<< " Mass Properties Report (English units: lbf, in, slug-ft^2)"
<< reset << endl;
cout << " " << underon << " Weight CG-X CG-Y"
<< " CG-Z Ixx Iyy Izz" << underoff << endl;
cout.precision(1);
cout << highint << setw(34) << left << " Base Vehicle " << normint
<< right << setw(10) << EmptyWeight << setw(8) << vbaseXYZcg(eX) << setw(8)
<< vbaseXYZcg(eY) << setw(8) << vbaseXYZcg(eZ) << setw(12) << baseJ(1,1)
<< setw(12) << baseJ(2,2) << setw(12) << baseJ(3,3) << endl;
for (unsigned int i=0;i<PointMasses.size();i++) {
PointMass* pm = PointMasses[i];
double pmweight = pm->GetPointMassWeight();
cout << highint << left << setw(4) << i << setw(30) << pm->GetName() << normint
<< right << setw(10) << pmweight << setw(8) << pm->GetLocation()(eX)
<< setw(8) << pm->GetLocation()(eY) << setw(8) << pm->GetLocation()(eZ)
<< setw(12) << pm->GetPointMassMoI(1,1) << setw(12) << pm->GetPointMassMoI(2,2)
<< setw(12) << pm->GetPointMassMoI(3,3) << endl;
}
cout << FDMExec->GetPropulsionTankReport();
cout << underon << setw(104) << " " << underoff << endl;
cout << highint << left << setw(30) << " Total: " << right << setw(14) << Weight
<< setw(8) << vXYZcg(eX)
<< setw(8) << vXYZcg(eY)
<< setw(8) << vXYZcg(eZ)
<< setw(12) << mJ(1,1)
<< setw(12) << mJ(2,2)
<< setw(12) << mJ(3,3)
<< normint << endl;
cout.setf(ios_base::fixed);
}
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;
}
