-// Calculate new moments of inertia here
-
- Ixx = baseIxx + Propulsion->GetTanksIxx(vXYZcg) + GetPMIxx();
- Iyy = baseIyy + Propulsion->GetTanksIyy(vXYZcg) + GetPMIyy();
- Izz = baseIzz + Propulsion->GetTanksIzz(vXYZcg) + GetPMIzz();
- Ixy = baseIxy + Propulsion->GetTanksIxy(vXYZcg) + GetPMIxy();
- Ixz = baseIxz + Propulsion->GetTanksIxz(vXYZcg) + GetPMIxz();
+// 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 += Propulsion->CalculateTankInertias();
+
+ 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 );