public EditorSimManager(InstantConditionSim _instantSim)
 {
     _instantCondition   = _instantSim;
     StabDerivCalculator = new StabilityDerivCalculator(_instantCondition);
     SweepSim            = new SweepSim(_instantCondition);
     _aeroCenter         = new EditorAeroCenter();
     vehicleData         = new StabilityDerivOutput();
 }
        public StabilityDerivGUI(EditorSimManager simManager, GUIDropDown<int> flapSettingDropDown, GUIDropDown<CelestialBody> bodySettingDropdown)
        {
            this.simManager = simManager;
            _flapSettingDropdown = flapSettingDropDown;
            _bodySettingDropdown = bodySettingDropdown;

            stabDerivOutput = new StabilityDerivOutput();
        }
 public EditorSimManager()
 {
     _instantCondition = new InstantConditionSim();
     _stabDerivCalculator = new StabilityDerivCalculator(_instantCondition);
     _stabDerivLinearSim = new StabilityDerivLinearSim(_instantCondition);
     _sweepSim = new SweepSim(_instantCondition);
     _aeroCenter = new EditorAeroCenter();
     vehicleData = new StabilityDerivOutput();
 }
        public void Display()
        {
            //stabDerivHelp = GUILayout.Toggle(stabDerivHelp, "?", ButtonStyle, GUILayout.Width(200));

            GUILayout.Label("Flight Condition:");
            GUILayout.BeginHorizontal();
            GUILayout.Label("Planet:");
            _bodySettingDropdown.GUIDropDownDisplay();

            GUILayout.Label("Altitude (km):");
            altitude = GUILayout.TextField(altitude, GUILayout.ExpandWidth(true));

            GUILayout.Label("Mach Number: ");
            machNumber = GUILayout.TextField(machNumber, GUILayout.ExpandWidth(true));

            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Flap Setting: ");
            _flapSettingDropdown.GUIDropDownDisplay();
            GUILayout.Label("Spoilers:");
            spoilersDeployed = GUILayout.Toggle(spoilersDeployed, spoilersDeployed ? "Deployed" : "Retracted", GUILayout.Width(100));
            GUILayout.EndHorizontal();

            if (GUILayout.Button("Calculate Stability Derivatives", GUILayout.Width(250.0F), GUILayout.Height(25.0F)))
            {
                CelestialBody body = _bodySettingDropdown.ActiveSelection;
                FARAeroUtil.UpdateCurrentActiveBody(body);
                //atm_temp_str = Regex.Replace(atm_temp_str, @"[^-?[0-9]*(\.[0-9]*)?]", "");
                //rho_str = Regex.Replace(rho_str, @"[^-?[0-9]*(\.[0-9]*)?]", "");
                machNumber = Regex.Replace(machNumber, @"[^-?[0-9]*(\.[0-9]*)?]", "");

                altitude = Regex.Replace(altitude, @"[^-?[0-9]*(\.[0-9]*)?]", "");
                double altitudeDouble = Convert.ToDouble(altitude);
                altitudeDouble *= 1000;

                double temp = body.GetTemperature(altitudeDouble);
                double pressure = body.GetPressure(altitudeDouble);
                if (pressure > 0)
                {
                    //double temp = Convert.ToSingle(atm_temp_str);
                    double machDouble = Convert.ToSingle(machNumber);
                    machDouble = FARMathUtil.Clamp(machDouble, 0.001, float.PositiveInfinity);

                    double density = body.GetDensity(pressure, temp);

                    double sspeed = body.GetSpeedOfSound(pressure, density);
                    double vel = sspeed * machDouble;

                    //UpdateControlSettings();

                    double q = vel * vel * density * 0.5f;

                    stabDerivOutput = simManager.StabDerivCalculator.CalculateStabilityDerivs(vel, q, machDouble, 0, 0, 0, _flapSettingDropdown.ActiveSelection, spoilersDeployed, body, altitudeDouble);
                    simManager.vehicleData = stabDerivOutput;
                    SetAngleVectors(stabDerivOutput.stableAoA);
                }
                else
                    PopupDialog.SpawnPopupDialog("Error!", "Altitude was above atmosphere", "OK", false, HighLogic.Skin);
            }
            GUILayout.BeginHorizontal();
            GUILayout.Label("Aircraft Properties", GUILayout.Width(180));
            GUILayout.Label("Moments of Inertia", GUILayout.Width(160));
            GUILayout.Label("Products of Inertia", GUILayout.Width(160));
            GUILayout.Label("Level Flight", GUILayout.Width(140));
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.BeginVertical(GUILayout.Width(180));
            GUILayout.Label("Ref Area: " + stabDerivOutput.area.ToString("G3") + " m²");
            GUILayout.Label("Scaled Chord: " + stabDerivOutput.MAC.ToString("G3") + " m");
            GUILayout.Label("Scaled Span: " + stabDerivOutput.b.ToString("G3") + " m");
            GUILayout.EndVertical();

            GUILayout.BeginVertical(GUILayout.Width(160));
            GUILayout.Label(new GUIContent("Ixx: " + stabDerivOutput.stabDerivs[0].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about X-axis"));
            GUILayout.Label(new GUIContent("Iyy: " + stabDerivOutput.stabDerivs[1].ToString("G6") + " kg * m²", "Inertia about Y-axis due to rotation about Y-axis"));
            GUILayout.Label(new GUIContent("Izz: " + stabDerivOutput.stabDerivs[2].ToString("G6") + " kg * m²", "Inertia about Z-axis due to rotation about Z-axis"));
            GUILayout.EndVertical();

            GUILayout.BeginVertical(GUILayout.Width(160));
            GUILayout.Label(new GUIContent("Ixy: " + stabDerivOutput.stabDerivs[24].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about Y-axis; is equal to inertia about Y-axis due to rotation about X-axis"));
            GUILayout.Label(new GUIContent("Iyz: " + stabDerivOutput.stabDerivs[25].ToString("G6") + " kg * m²", "Inertia about Y-axis due to rotation about Z-axis; is equal to inertia about Z-axis due to rotation about Y-axis"));
            GUILayout.Label(new GUIContent("Ixz: " + stabDerivOutput.stabDerivs[26].ToString("G6") + " kg * m²", "Inertia about X-axis due to rotation about Z-axis; is equal to inertia about Z-axis due to rotation about X-axis"));
            GUILayout.EndVertical();

            GUILayout.BeginVertical(GUILayout.Width(140));
            GUILayout.Label(new GUIContent("u0: " + stabDerivOutput.nominalVelocity.ToString("G6") + " m/s", "Air speed based on this mach number and temperature."));
            GUILayout.BeginHorizontal();
            GUILayout.Label(new GUIContent("Cl: " + stabDerivOutput.stableCl.ToString("G3"), "Required lift coefficient at this mass, speed and air density."));
            GUILayout.Label(new GUIContent("Cd: " + stabDerivOutput.stableCd.ToString("G3"), "Resulting drag coefficient at this mass, speed and air density."));
            GUILayout.EndHorizontal();
            GUILayout.Label(new GUIContent("AoA: " + stabDerivOutput.stableAoAState + stabDerivOutput.stableAoA.ToString("G6") + " deg", "Angle of attack required to achieve the necessary lift force."));
            GUILayout.EndVertical();

            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Longitudinal Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();

            GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
            BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;

            GUILayout.BeginVertical(BackgroundStyle);
            GUILayout.BeginHorizontal();
            GUILayout.Label("Down Vel Derivatives", GUILayout.Width(160));
            GUILayout.Label("Fwd Vel Derivatives", GUILayout.Width(160));
            GUILayout.Label("Pitch Rate Derivatives", GUILayout.Width(160));
            GUILayout.Label("Pitch Ctrl Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Zw: ", stabDerivOutput.stabDerivs[3], " s⁻¹", "Change in Z-direction acceleration with respect to Z-direction velocity; should be negative", 160, -1);
            StabilityLabel("Zu: ", stabDerivOutput.stabDerivs[6], " s⁻¹", "Change in Z-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
            StabilityLabel("Zq: ", stabDerivOutput.stabDerivs[9], " m/s", "Change in Z-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
            StabilityLabel("Zδe: ", stabDerivOutput.stabDerivs[12], " m/s²", "Change in Z-direction acceleration with respect to pitch control input; should be negative", 160, 0);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Xw: ", stabDerivOutput.stabDerivs[4], " s⁻¹", "Change in X-direction acceleration with respect to Z-direction velocity; sign unimportant", 160, 0);
            StabilityLabel("Xu: ", stabDerivOutput.stabDerivs[7], " s⁻¹", "Change in X-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
            StabilityLabel("Xq: ", stabDerivOutput.stabDerivs[10], " m/s", "Change in X-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
            StabilityLabel("Xδe: ", stabDerivOutput.stabDerivs[13], " m/s²", "Change in X-direction acceleration with respect to pitch control input; sign unimportant", 160, 0);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Mw: ", stabDerivOutput.stabDerivs[5], " (m * s)⁻¹", "Change in pitch-up angular acceleration with respect to Z-direction velocity; should be negative", 160, -1);
            StabilityLabel("Mu: ", stabDerivOutput.stabDerivs[8], " (m * s)⁻¹", "Change in pitch-up angular acceleration acceleration with respect to X-direction velocity; sign unimportant", 160, 0);
            StabilityLabel("Mq: ", stabDerivOutput.stabDerivs[11], " s⁻¹", "Change in pitch-up angular acceleration acceleration with respect to pitch-up rate; should be negative", 160, -1);
            StabilityLabel("Mδe: ", stabDerivOutput.stabDerivs[14], " s⁻²", "Change in pitch-up angular acceleration acceleration with respect to pitch control input; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.EndVertical();

            GUILayout.BeginHorizontal();
            GUILayout.Label("Lateral Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Sideslip Derivatives", GUILayout.Width(160));
            GUILayout.Label("Roll Rate Derivatives", GUILayout.Width(160));
            GUILayout.Label("Yaw Rate Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();
            GUILayout.BeginVertical(BackgroundStyle);
            GUILayout.BeginHorizontal();
            StabilityLabel("Yβ: ", stabDerivOutput.stabDerivs[15], " m/s²", "Change in Y-direction acceleration with respect to sideslip angle β; should be negative", 160, -1);
            StabilityLabel("Yp: ", stabDerivOutput.stabDerivs[18], " m/s", "Change in Y-direction acceleration with respect to roll-right rate; sign unimportant", 160, 0);
            StabilityLabel("Yr: ", stabDerivOutput.stabDerivs[21], " m/s", "Change in Y-direction acceleration with respect to yaw-right rate; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Lβ: ", stabDerivOutput.stabDerivs[16], " s⁻²", "Change in roll-right angular acceleration with respect to sideslip angle β; should be negative", 160, -1);
            StabilityLabel("Lp: ", stabDerivOutput.stabDerivs[19], " s⁻¹", "Change in roll-right angular acceleration with respect to roll-right rate; should be negative", 160, -1);
            StabilityLabel("Lr: ", stabDerivOutput.stabDerivs[22], " s⁻¹", "Change in roll-right angular acceleration with respect to yaw-right rate; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Nβ: ", stabDerivOutput.stabDerivs[17], " s⁻²", "Change in yaw-right angular acceleration with respect to sideslip angle β; should be positive", 160, 1);
            StabilityLabel("Np: ", stabDerivOutput.stabDerivs[20], " s⁻¹", "Change in yaw-right angular acceleration with respect to roll-right rate; sign unimportant", 160, 0);
            StabilityLabel("Nr: ", stabDerivOutput.stabDerivs[23], " s⁻¹", "Change in yaw-right angular acceleration with respect to yaw-right rate; should be negative", 160, -1);
            GUILayout.EndHorizontal();
            GUILayout.EndVertical();

            DrawTooltip();
        }
Exemplo n.º 5
0
        public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
        {
            StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();

            stabDerivOutput.nominalVelocity = u0;
            stabDerivOutput.altitude        = alt;
            stabDerivOutput.body            = body;

            Vector3d CoM  = Vector3d.zero;
            double   mass = 0;

            double MAC  = 0;
            double b    = 0;
            double area = 0;

            double Ix  = 0;
            double Iy  = 0;
            double Iz  = 0;
            double Ixy = 0;
            double Iyz = 0;
            double Ixz = 0;

            InstantConditionSimInput  input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
            InstantConditionSimOutput nominalOutput;
            InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();

            _instantCondition.GetClCdCmSteady(input, out nominalOutput, true);

            List <Part> partsList = EditorLogic.SortedShipList;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM  += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
                FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
                if (w != null)
                {
                    if (w.isShielded)
                    {
                        continue;
                    }

                    area += w.S;
                    MAC  += w.GetMAC() * w.S;
                    b    += w.Getb_2() * w.S;
                    if (w is FARControllableSurface)
                    {
                        (w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
                    }
                }
            }
            if (area == 0)
            {
                area = _instantCondition._maxCrossSectionFromBody;
                MAC  = _instantCondition._bodyLength;
                b    = 1;
            }
            MAC  /= area;
            b    /= area;
            CoM  /= mass;
            mass *= 1000;

            stabDerivOutput.b    = b;
            stabDerivOutput.MAC  = MAC;
            stabDerivOutput.area = area;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (p == null || FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                //This section handles the parallel axis theorem
                Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double  x2, y2, z2, x, y, z;
                x2 = relPos.z * relPos.z;
                y2 = relPos.x * relPos.x;
                z2 = relPos.y * relPos.y;
                x  = relPos.z;
                y  = relPos.x;
                z  = relPos.y;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass

                Ix += (y2 + z2) * partMass;
                Iy += (x2 + z2) * partMass;
                Iz += (x2 + y2) * partMass;

                Ixy += -x * y * partMass;
                Iyz += -z * y * partMass;
                Ixz += -x * z * partMass;

                //And this handles the part's own moment of inertia
                Vector3    principalInertia = p.Rigidbody.inertiaTensor;
                Quaternion prncInertRot     = p.Rigidbody.inertiaTensorRotation;

                //The rows of the direction cosine matrix for a quaternion
                Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                                           2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
                                           2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));

                Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
                                           -prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                                           2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));

                Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
                                           2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
                                           -prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);


                //And converting the principal moments of inertia into the coordinate system used by the system
                Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
                Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
                Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;

                Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
                Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
                Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
            }
            Ix *= 1000;
            Iy *= 1000;
            Iz *= 1000;

            stabDerivOutput.stabDerivs[0] = Ix;
            stabDerivOutput.stabDerivs[1] = Iy;
            stabDerivOutput.stabDerivs[2] = Iz;

            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;


            double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity

            effectiveG -= u0 * u0 / (alt + body.Radius);                                        //This is the effective reduction of gravity due to high velocity
            double neededCl = mass * effectiveG / (q * area);


            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
            //Longitudinal Mess
            _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);

            alpha         = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
            input.alpha   = alpha;
            nominalOutput = _instantCondition.iterationOutput;
            //alpha_str = (alpha * Mathf.PI / 180).ToString();

            input.alpha = (alpha + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            stabDerivOutput.stableCl       = neededCl;
            stabDerivOutput.stableCd       = nominalOutput.Cd;
            stabDerivOutput.stableAoA      = alpha;
            stabDerivOutput.stableAoAState = "";
            if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
            {
                stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
            }

            Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);                   //vert vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= nominalOutput.Cl;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            stabDerivOutput.stabDerivs[3] = pertOutput.Cl;  //Zw
            stabDerivOutput.stabDerivs[4] = pertOutput.Cd;  //Xw
            stabDerivOutput.stabDerivs[5] = pertOutput.Cm;  //Mw

            input.alpha      = alpha;
            input.machNumber = machNumber + 0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;                   //fwd vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (u0 * Iy);

            stabDerivOutput.stabDerivs[6] = pertOutput.Cl;  //Zu
            stabDerivOutput.stabDerivs[7] = pertOutput.Cd;  //Xu
            stabDerivOutput.stabDerivs[8] = pertOutput.Cm;  //Mu

            input.machNumber = machNumber;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.alphaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;                   //pitch rate derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;

            pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);

            stabDerivOutput.stabDerivs[9]  = pertOutput.Cl; //Zq
            stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
            stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq

            input.alphaDot   = 0;
            input.pitchValue = 0.1;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;                   //elevator derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;

            pertOutput.Cl *= q * area / mass;
            pertOutput.Cd *= q * area / mass;
            pertOutput.Cm *= q * area * MAC / Iy;

            stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
            stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
            stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me

            //Lateral Mess

            input.pitchValue = 0;
            input.beta       = (beta + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);               //sideslip angle derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            stabDerivOutput.stabDerivs[15] = pertOutput.Cy;     //Yb
            stabDerivOutput.stabDerivs[17] = pertOutput.Cn;     //Nb
            stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb

            input.beta = beta;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.phiDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / 0.05;               //roll rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[18] = pertOutput.Cy;     //Yp
            stabDerivOutput.stabDerivs[20] = pertOutput.Cn;     //Np
            stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp


            input.phiDot = 0;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.betaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;                   //yaw rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[21] = pertOutput.Cy;     //Yr
            stabDerivOutput.stabDerivs[23] = pertOutput.Cn;     //Nr
            stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr

            return(stabDerivOutput);
        }
 public StabilityDerivExportOutput(StabilityDerivOutput outputvalues, StabilityDerivExportVariables exportvalues)
 {
     this.outputvals = outputvalues;
     this.exportvals = exportvalues;
 }
        public static GraphData RunTransientSimLateral(
            StabilityDerivOutput vehicleData,
            double endTime,
            double initDt,
            double[] InitCond
            )
        {
            var A = new SimMatrix(4, 4);

            A.PrintToConsole();

            int i      = 0;
            int j      = 0;
            int num    = 0;
            var Derivs = new double[27];

            vehicleData.stabDerivs.CopyTo(Derivs, 0);

            Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
            Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
            Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;

            double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
            Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;

            Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
            Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;

            Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
            Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;

            foreach (double f in Derivs)
            {
                if (num < 15)
                {
                    num++; //Avoid Ix, Iy, Iz and long derivs
                    continue;
                }

                num++;
                FARLogger.Info("" + i + "," + j);
                if (i <= 2)
                {
                    A.Add(f, i, j);
                }

                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }

            A.Add(InstantConditionSim.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) *
                  Math.Cos(vehicleData.stableAoA * Math.PI / 180) /
                  vehicleData.nominalVelocity,
                  3,
                  0);
            A.Add(1, 1, 3);


            A.PrintToConsole(); //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) ,  g Cos(θ0) / u0 ]
             *       |  [   Lb    ,    Lp   ,      Lr       ,          0          ]
             *       |  [   Nb    ,    Np   ,      Nr       ,          0          ]
             *      \ / [    0    ,    1    ,      0        ,          0          ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */
            var transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            var lines = new GraphData {
                xValues = transSolve.time
            };

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.LdColor, "β", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.CmColor, "p", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.CdColor, "r", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.ClColor, "φ", true);

            return(lines);
        }
        public StabilityDerivExportOutput CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double alpha, double beta, double phi)
        {
            double pressure    = body.GetPressure(alt);
            double temperature = body.GetTemperature(alt);
            double density     = body.GetDensity(pressure, temperature);
            double sspeed      = body.GetSpeedOfSound(pressure, density);
            double u0          = sspeed * machNumber;
            double q           = u0 * u0 * density * 0.5f;

            StabilityDerivOutput          stabDerivOutput = new StabilityDerivOutput();
            StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables();

            stabDerivOutput.nominalVelocity = u0;
            stabDerivOutput.altitude        = alt;
            stabDerivOutput.body            = body;

            Vector3d CoM  = Vector3d.zero;
            double   mass = 0;

            double MAC  = 0;
            double b    = 0;
            double area = 0;

            double Ix  = 0;
            double Iy  = 0;
            double Iz  = 0;
            double Ixy = 0;
            double Iyz = 0;
            double Ixz = 0;

            InstantConditionSimInput  input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
            InstantConditionSimOutput nominalOutput;
            InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();

            _instantCondition.GetClCdCmSteady(input, out nominalOutput, true);

            List <Part> partsList = EditorLogic.SortedShipList;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM  += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
                FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
                if (w != null)
                {
                    if (w.isShielded)
                    {
                        continue;
                    }

                    area += w.S;
                    MAC  += w.GetMAC() * w.S;
                    b    += w.Getb_2() * w.S;
                    if (w is FARControllableSurface)
                    {
                        (w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
                    }
                }
            }
            if (area.NearlyEqual(0))
            {
                area = _instantCondition._maxCrossSectionFromBody;
                MAC  = _instantCondition._bodyLength;
                b    = 1;
            }
            MAC  /= area;
            b    /= area;
            CoM  /= mass;
            mass *= 1000;

            stabDerivOutput.b    = b;
            stabDerivOutput.MAC  = MAC;
            stabDerivOutput.area = area;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (p == null || FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                //This section handles the parallel axis theorem
                Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double  x2, y2, z2, x, y, z;
                x2 = relPos.z * relPos.z;
                y2 = relPos.x * relPos.x;
                z2 = relPos.y * relPos.y;
                x  = relPos.z;
                y  = relPos.x;
                z  = relPos.y;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass

                Ix += (y2 + z2) * partMass;
                Iy += (x2 + z2) * partMass;
                Iz += (x2 + y2) * partMass;

                Ixy += -x * y * partMass;
                Iyz += -z * y * partMass;
                Ixz += -x * z * partMass;

                //And this handles the part's own moment of inertia
                Vector3    principalInertia = p.Rigidbody.inertiaTensor;
                Quaternion prncInertRot     = p.Rigidbody.inertiaTensorRotation;

                //The rows of the direction cosine matrix for a quaternion
                Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                                           2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
                                           2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));

                Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
                                           -prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                                           2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));

                Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
                                           2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
                                           -prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);


                //And converting the principal moments of inertia into the coordinate system used by the system
                Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
                Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
                Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;

                Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
                Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
                Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
            }
            Ix *= 1000;
            Iy *= 1000;
            Iz *= 1000;

            stabDerivOutput.stabDerivs[0] = Ix;
            stabDerivOutput.stabDerivs[1] = Iy;
            stabDerivOutput.stabDerivs[2] = Iz;

            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;


            double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity

            effectiveG -= u0 * u0 / (alt + body.Radius);                                        //This is the effective reduction of gravity due to high velocity
            double neededCl = mass * effectiveG / (q * area);


            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
            //Longitudinal Mess
            _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);

            alpha         = FARMathUtil.SelectedSearchMethod(machNumber, _instantCondition.FunctionIterateForAlpha);
            input.alpha   = alpha;
            nominalOutput = _instantCondition.iterationOutput;
            //alpha_str = (alpha * Mathf.PI / 180).ToString();

            input.alpha = (alpha + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            stabDerivOutput.stableCl       = neededCl;
            stabDerivOutput.stableCd       = nominalOutput.Cd;
            stabDerivOutput.stableAoA      = alpha;
            stabDerivOutput.stableAoAState = "";
            if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
            {
                stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
            }

            FARLogger.Info("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);                   //vert vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= nominalOutput.Cl;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            stabDerivOutput.stabDerivs[3] = pertOutput.Cl;  //Zw
            stabDerivOutput.stabDerivs[4] = pertOutput.Cd;  //Xw
            stabDerivOutput.stabDerivs[5] = pertOutput.Cm;  //Mw

            // Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
            //  is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
            //  here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057

            input.alpha      = alpha;
            input.machNumber = machNumber + 0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;                   //fwd vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (u0 * Iy);

            stabDerivOutput.stabDerivs[6] = pertOutput.Cl;  //Zu
            stabDerivOutput.stabDerivs[7] = pertOutput.Cd;  //Xu
            stabDerivOutput.stabDerivs[8] = pertOutput.Cm;  //Mu

            input.machNumber = machNumber;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.alphaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;                   //pitch rate derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;

            pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
            pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); //  for Zq and Xq match those for Zu and Xu.
            pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);

            stabDerivOutput.stabDerivs[9]  = pertOutput.Cl; //Zq
            stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
            stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq

            input.alphaDot   = 0;
            input.pitchValue = 0.1;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;                   //elevator derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;

            pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
            pertOutput.Cd *= -q * area / mass; //  for Ze and Xe match those for Zu and Xu.
            pertOutput.Cm *= q * area * MAC / Iy;

            stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
            stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
            stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me

            //Lateral Mess

            input.pitchValue = 0;
            input.beta       = (beta + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);               //sideslip angle derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            stabDerivOutput.stabDerivs[15] = pertOutput.Cy;     //Yb
            stabDerivOutput.stabDerivs[17] = pertOutput.Cn;     //Nb
            stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb

            input.beta = beta;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.phiDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / 0.05;               //roll rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[18] = pertOutput.Cy;     //Yp
            stabDerivOutput.stabDerivs[20] = pertOutput.Cn;     //Np
            stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp


            input.phiDot = 0;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.betaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;                   //yaw rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[21] = pertOutput.Cy;     //Yr
            stabDerivOutput.stabDerivs[23] = pertOutput.Cn;     //Nr
            stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr

            // Assign values to export variables
            stabDerivExport.craftmass      = mass;
            stabDerivExport.envpressure    = pressure;
            stabDerivExport.envtemperature = temperature;
            stabDerivExport.envdensity     = density;
            stabDerivExport.envsoundspeed  = sspeed;
            stabDerivExport.envg           = _instantCondition.CalculateAccelerationDueToGravity(body, alt);
            stabDerivExport.sitmach        = machNumber;
            stabDerivExport.sitdynpres     = q;
            stabDerivExport.siteffg        = effectiveG;

            return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport));
        }
        public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);

            A.PrintToConsole();

            int i = 0;
            int j = 0;
            int num = 0;
            double[] Derivs = new double[27];

            vehicleData.stabDerivs.CopyTo(Derivs, 0);

            Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
            Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
            Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;

            double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
            Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;

            Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
            Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;

            Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
            Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;

            for (int k = 0; k < Derivs.Length; k++)
            {
                double f = Derivs[k];
                if (num < 15)
                {
                    num++;              //Avoid Ix, Iy, Iz and long derivs
                    continue;
                }
                else
                    num++;
                Debug.Log(i + "," + j);
                if (i <= 2)
                    A.Add(f, i, j);

                if (j < 2)
                    j++;
                else
                {
                    j = 0;
                    i++;
                }

            }
            A.Add(_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) * Math.Cos(vehicleData.stableAoA * Math.PI / 180) / vehicleData.nominalVelocity, 3, 0);
            A.Add(1, 1, 3);


            A.PrintToConsole();                //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) ,  g Cos(θ0) / u0 ]
             *       |  [   Lb    ,    Lp   ,      Lr       ,          0          ]
             *       |  [   Nb    ,    Np   ,      Nr       ,          0          ]
             *      \ / [    0    ,    1    ,      0        ,          0          ]
             *       V                              //And one that looks like this:
             *                              
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             * 
             * 
             */
            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);
            transSolve.Solve();

            GraphData lines = new GraphData();
            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "β", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "p", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "r", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "φ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
            graph.SetGridScaleUsingValues(1, 5);
            graph.horizontalLabel = "time";
            graph.verticalLabel = "value";
            graph.Update();*/

            return lines;
        }
        public static GraphData RunTransientSimLongitudinal(
            StabilityDerivOutput vehicleData,
            double endTime,
            double initDt,
            double[] InitCond
            )
        {
            var A = new SimMatrix(4, 4);
            var B = new SimMatrix(1, 4);

            A.PrintToConsole();

            int i   = 0;
            int j   = 0;
            int num = 0;

            foreach (double f in vehicleData.stabDerivs)
            {
                if (num < 3 || num >= 15)
                {
                    num++; //Avoid Ix, Iy, Iz
                    continue;
                }

                num++;
                FARLogger.Info(i + "," + j);
                if (i <= 2)
                {
                    if (num == 10)
                    {
                        A.Add(f + vehicleData.nominalVelocity, i, j);
                    }
                    else
                    {
                        A.Add(f, i, j);
                    }
                }
                else
                {
                    B.Add(f, 0, j);
                }
                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }

            A.Add(-InstantConditionSim.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
            A.Add(1, 2, 3);


            A.PrintToConsole(); //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Z w , Z u , Z q  + u,  0 ]
             *       |  [ X w , X u , X q     , -g ]
             *       |  [ M w , M u , M q     ,  0 ]
             *      \ / [  0  ,  0  ,  1      ,  0 ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */

            var transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            var lines = new GraphData {
                xValues = transSolve.time
            };

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, FARConfig.GUIColors.LdColor, "w", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, FARConfig.GUIColors.CmColor, "u", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.CdColor, "q", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, FARConfig.GUIColors.ClColor, "θ", true);

            return(lines);
        }
        public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);

            int i = 0;
            int j = 0;

            double[] Derivs = new double[27];

            vehicleData.stabDerivs.CopyTo(Derivs, 0);

            double u0           = vehicleData.nominalVelocity;
            double b2u          = vehicleData.b / (2 * u0);
            double effg         = _instantCondition.CalculateEffectiveGravity(vehicleData.body, vehicleData.altitude, u0) * Math.Cos(vehicleData.stableCondition.stableAoA * Math.PI / 180);
            double factor_xz_x  = Derivs[26] / Derivs[0];
            double factor_xz_z  = Derivs[26] / Derivs[2];
            double factor_invxz = 1 / (1 - factor_xz_x * factor_xz_z);

            FARLogger.Info("u0= " + u0);
            FARLogger.Info("b/(2u)= " + b2u + " IGNORED!");
            FARLogger.Info("effg= " + effg + ", after multiplication with cos(AoA).");
            FARLogger.Info("Ixz/Ix= " + factor_xz_x + ", used to add yaw to roll-deriv.");
            FARLogger.Info("Ixz/Iz= " + factor_xz_z + ", used to add roll to yaw-deriv.");
            FARLogger.Info("(1 - Ixz^2/(IxIz))^-1= " + factor_invxz);

            // Rodhern: For possible backward compability the rotation (moment) derivatives can be
            //  scaled by "b/(2u)" (for pitch rate "mac/(2u)").
            //for (int h = 18; h <= 23; h++)
            //    Derivs[h] = Derivs[h] * b2u;

            Derivs[15] = Derivs[15] / u0;
            Derivs[18] = Derivs[18] / u0;
            Derivs[21] = Derivs[21] / u0 - 1;

            double Lb = Derivs[16] * factor_invxz;
            double Nb = Derivs[17] * factor_invxz;

            double Lp = Derivs[19] * factor_invxz;
            double Np = Derivs[20] * factor_invxz;

            double Lr = Derivs[22] * factor_invxz;
            double Nr = Derivs[23] * factor_invxz;

            Derivs[16] = Lb + factor_xz_x * Nb;
            Derivs[17] = Nb + factor_xz_z * Lb;

            Derivs[19] = Lp + factor_xz_x * Np;
            Derivs[20] = Np + factor_xz_z * Lp;

            Derivs[22] = Lr + factor_xz_x * Nr;
            Derivs[23] = Nr + factor_xz_z * Lr;

            for (int k = 15; k < Derivs.Length; k++)
            {
                double f = Derivs[k];

                if (i <= 2)
                {
                    FARLogger.Info("A[" + i + "," + j + "]= f_" + k + " = " + f + ", after manipulation.");
                    A.Add(f, i, j);
                }

                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }
            A.Add(effg / u0, 3, 0);
            A.Add(1, 1, 3);

            A.PrintToConsole();                //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) ,  g Cos(θ0) / u0 ]
             *       |  [   Lb    ,    Lp   ,      Lr       ,          0          ]
             *       |  [   Nb    ,    Np   ,      Nr       ,          0          ]
             *      \ / [    0    ,    1    ,      0        ,          0          ]
             *       V
             */

            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            GraphData lines = new GraphData();

            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "β", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "p", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "r", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "φ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
             * graph.SetGridScaleUsingValues(1, 5);
             * graph.horizontalLabel = "time";
             * graph.verticalLabel = "value";
             * graph.Update();*/

            return(lines);
        }
        public GraphData RunTransientSimLongitudinal(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);
            SimMatrix B = new SimMatrix(1, 4);

            A.PrintToConsole();

            int i = 0;
            int j = 0;
            int num = 0;
            double[] Derivs = new double[27];

            for (int k = 0; k < vehicleData.stabDerivs.Length; k++)
            {
                double f = vehicleData.stabDerivs[k];
                if (num < 3 || num >= 15)
                {
                    num++;              //Avoid Ix, Iy, Iz
                    continue;
                }
                else
                    num++;
                MonoBehaviour.print(i + "," + j);
                if (i <= 2)
                    if (num == 10)
                        A.Add(f + vehicleData.nominalVelocity, i, j);
                    else
                        A.Add(f, i, j);
                else
                    B.Add(f, 0, j);
                if (j < 2)
                    j++;
                else
                {
                    j = 0;
                    i++;
                }

            }
            A.Add(-_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
            A.Add(1, 2, 3);


            A.PrintToConsole();                //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Z w , Z u , Z q  + u,  0 ]
             *       |  [ X w , X u , X q     , -g ]
             *       |  [ M w , M u , M q     ,  0 ]
             *      \ / [  0  ,  0  ,  1      ,  0 ]
             *       V                              //And one that looks like this:
             *                              
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             * 
             * 
             */

            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);
            transSolve.Solve();

            GraphData lines = new GraphData();
            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "w", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "u", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "q", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "θ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
            graph.SetGridScaleUsingValues(1, 5);
            graph.horizontalLabel = "time";
            graph.verticalLabel = "value";
            graph.Update();*/

            return lines;
        }
        public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
        {
            StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
            stabDerivOutput.nominalVelocity = u0;
            stabDerivOutput.altitude = alt;
            stabDerivOutput.body = body;

            Vector3d CoM = Vector3d.zero;
            double mass = 0;

            double MAC = 0;
            double b = 0;
            double area = 0;

            double Ix = 0;
            double Iy = 0;
            double Iz = 0;
            double Ixy = 0;
            double Iyz = 0;
            double Ixz = 0;

            InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
            InstantConditionSimOutput nominalOutput;
            InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();

            _instantCondition.GetClCdCmSteady(input, out nominalOutput, true);

            List<Part> partsList = EditorLogic.SortedShipList;
            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (FARAeroUtil.IsNonphysical(p))
                    continue;
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
                FARWingAerodynamicModel w = p.GetComponent<FARWingAerodynamicModel>();
                if (w != null)
                {
                    if (w.isShielded)
                        continue;

                    area += w.S;
                    MAC += w.GetMAC() * w.S;
                    b += w.Getb_2() * w.S;
                    if (w is FARControllableSurface)
                    {
                        (w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
                    }
                }
            }
            if (area == 0)
            {
                area = _instantCondition._maxCrossSectionFromBody;
                MAC = _instantCondition._bodyLength;
                b = 1;
            }
            MAC /= area;
            b /= area;
            CoM /= mass;
            mass *= 1000;

            stabDerivOutput.b = b;
            stabDerivOutput.MAC = MAC;
            stabDerivOutput.area = area;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (p == null || FARAeroUtil.IsNonphysical(p))
                    continue;
                //This section handles the parallel axis theorem
                Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double x2, y2, z2, x, y, z;
                x2 = relPos.z * relPos.z;
                y2 = relPos.x * relPos.x;
                z2 = relPos.y * relPos.y;
                x = relPos.z;
                y = relPos.x;
                z = relPos.y;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass

                Ix += (y2 + z2) * partMass;
                Iy += (x2 + z2) * partMass;
                Iz += (x2 + y2) * partMass;

                Ixy += -x * y * partMass;
                Iyz += -z * y * partMass;
                Ixz += -x * z * partMass;

                //And this handles the part's own moment of inertia
                Vector3 principalInertia = p.Rigidbody.inertiaTensor;
                Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;

                //The rows of the direction cosine matrix for a quaternion
                Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                    2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
                    2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));

                Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
                    -prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                    2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));

                Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
                    2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
                    -prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);


                //And converting the principal moments of inertia into the coordinate system used by the system
                Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
                Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
                Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;

                Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
                Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
                Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
            }
            Ix *= 1000;
            Iy *= 1000;
            Iz *= 1000;

            stabDerivOutput.stabDerivs[0] = Ix;
            stabDerivOutput.stabDerivs[1] = Iy;
            stabDerivOutput.stabDerivs[2] = Iz;

            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;


            double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt);     //This is the effect of gravity
            effectiveG -= u0 * u0 / (alt + body.Radius);                          //This is the effective reduction of gravity due to high velocity
            double neededCl = mass * effectiveG / (q * area);


            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
            //Longitudinal Mess
            _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);

            alpha = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
            input.alpha = alpha;
            nominalOutput = _instantCondition.iterationOutput;
            //alpha_str = (alpha * Mathf.PI / 180).ToString();

            input.alpha = (alpha + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            stabDerivOutput.stableCl = neededCl;
            stabDerivOutput.stableCd = nominalOutput.Cd;
            stabDerivOutput.stableAoA = alpha;
            stabDerivOutput.stableAoAState = "";
            if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
                stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");

            Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);                   //vert vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= nominalOutput.Cl;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            stabDerivOutput.stabDerivs[3] = pertOutput.Cl;  //Zw
            stabDerivOutput.stabDerivs[4] = pertOutput.Cd;  //Xw
            stabDerivOutput.stabDerivs[5] = pertOutput.Cm;  //Mw

            input.alpha = alpha;
            input.machNumber = machNumber + 0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;                   //fwd vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (u0 * Iy);

            stabDerivOutput.stabDerivs[6] = pertOutput.Cl;  //Zu
            stabDerivOutput.stabDerivs[7] = pertOutput.Cd;  //Xu
            stabDerivOutput.stabDerivs[8] = pertOutput.Cm;  //Mu

            input.machNumber = machNumber;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.alphaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
           
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;                   //pitch rate derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;

            pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);

            stabDerivOutput.stabDerivs[9] = pertOutput.Cl;  //Zq
            stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
            stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq

            input.alphaDot = 0;
            input.pitchValue = 0.1;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;                   //elevator derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;

            pertOutput.Cl *= q * area / mass;
            pertOutput.Cd *= q * area / mass;
            pertOutput.Cm *= q * area * MAC / Iy;

            stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
            stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
            stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me

            //Lateral Mess

            input.pitchValue = 0;
            input.beta = (beta + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);                   //sideslip angle derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy *= q * area / mass;
            pertOutput.Cn *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            stabDerivOutput.stabDerivs[15] = pertOutput.Cy;     //Yb
            stabDerivOutput.stabDerivs[17] = pertOutput.Cn;     //Nb
            stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb

            input.beta = beta;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.phiDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
           
            pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05;                   //roll rate derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;

            pertOutput.Cy *= q * area * b / (2 * mass * u0);
            pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[18] = pertOutput.Cy;     //Yp
            stabDerivOutput.stabDerivs[20] = pertOutput.Cn;     //Np
            stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp


            input.phiDot = 0;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.betaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;                   //yaw rate derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;

            pertOutput.Cy *= q * area * b / (2 * mass * u0);
            pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[21] = pertOutput.Cy;     //Yr
            stabDerivOutput.stabDerivs[23] = pertOutput.Cn;     //Nr
            stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr

            return stabDerivOutput;
        }
Exemplo n.º 14
0
        public GraphData RunTransientSimLongitudinal(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);
            SimMatrix B = new SimMatrix(1, 4);

            A.PrintToConsole();

            int i   = 0;
            int j   = 0;
            int num = 0;

            double[] Derivs = new double[27];

            for (int k = 0; k < vehicleData.stabDerivs.Length; k++)
            {
                double f = vehicleData.stabDerivs[k];
                if (num < 3 || num >= 15)
                {
                    num++;              //Avoid Ix, Iy, Iz
                    continue;
                }
                else
                {
                    num++;
                }
                FARLogger.Info(i + "," + j);
                if (i <= 2)
                {
                    if (num == 10)
                    {
                        A.Add(f + vehicleData.nominalVelocity, i, j);
                    }
                    else
                    {
                        A.Add(f, i, j);
                    }
                }
                else
                {
                    B.Add(f, 0, j);
                }
                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }
            A.Add(-_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude), 3, 1);
            A.Add(1, 2, 3);


            A.PrintToConsole();                //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Z w , Z u , Z q  + u,  0 ]
             *       |  [ X w , X u , X q     , -g ]
             *       |  [ M w , M u , M q     ,  0 ]
             *      \ / [  0  ,  0  ,  1      ,  0 ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */

            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            GraphData lines = new GraphData();

            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "w", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "u", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "q", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "θ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
             * graph.SetGridScaleUsingValues(1, 5);
             * graph.horizontalLabel = "time";
             * graph.verticalLabel = "value";
             * graph.Update();*/

            return(lines);
        }
        private void DataInput(InitialConditions inits, StabilityDerivOutput vehicleData, bool longitudinal)
        {
            GUILayout.BeginHorizontal();
            for (int i = 0; i < inits.inits.Length; i++)
            {
                GUILayout.Label("Init " + inits.names[i] +": ");
                inits.inits[i] = GUILayout.TextField(inits.inits[i], GUILayout.ExpandWidth(true));
            }
            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();
            GUILayout.Label("End Time: ");
            inits.maxTime = GUILayout.TextField(inits.maxTime, GUILayout.ExpandWidth(true));
            GUILayout.Label("dt: ");
            inits.dt = GUILayout.TextField(inits.dt, GUILayout.ExpandWidth(true));
            if (GUILayout.Button("Run Simulation", GUILayout.Width(150.0F), GUILayout.Height(25.0F)))
            {
                for (int i = 0; i < inits.inits.Length; i++)
                {
                    inits.inits[i] = Regex.Replace(inits.inits[i], @"[^-?[0-9]*(\.[0-9]*)?]", "");
                }
                inits.maxTime = Regex.Replace(inits.maxTime, @"[^-?[0-9]*(\.[0-9]*)?]", "");
                inits.dt = Regex.Replace(inits.dt, @"[^-?[0-9]*(\.[0-9]*)?]", "");

                double[] initCond = new double[inits.inits.Length];
                for (int i = 0; i < initCond.Length; i++)
                {
                    initCond[i] = Convert.ToDouble(inits.inits[i]) * inits.scaling[i];
                }


                GraphData data;
                if(longitudinal)
                    data = simManager.StabDerivLinearSim.RunTransientSimLongitudinal(vehicleData, Convert.ToDouble(inits.maxTime), Convert.ToDouble(inits.dt), initCond);
                else
                    data = simManager.StabDerivLinearSim.RunTransientSimLateral(vehicleData, Convert.ToDouble(inits.maxTime), Convert.ToDouble(inits.dt), initCond);

                UpdateGraph(data, "time", "params", 0, Convert.ToDouble(inits.maxTime), 50);
            }
            GUILayout.EndHorizontal();
        }
        private void LateralGUI(StabilityDerivOutput vehicleData)
        {
            GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
            BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;

            GUILayout.BeginHorizontal();
            GUILayout.Label("Lateral Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();

            GUILayout.BeginVertical();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Sideslip Derivatives", GUILayout.Width(160));
            GUILayout.Label("Roll Rate Derivatives", GUILayout.Width(160));
            GUILayout.Label("Yaw Rate Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();

            GUILayout.BeginVertical(BackgroundStyle);
            GUILayout.BeginHorizontal();
            StabilityLabel("Yβ: ", vehicleData.stabDerivs[15], " m/s²", "Change in Y-direction acceleration with respect to sideslip angle β; should be negative", 160, -1);
            StabilityLabel("Yp: ", vehicleData.stabDerivs[18], " m/s", "Change in Y-direction acceleration with respect to roll-right rate; sign unimportant", 160, 0);
            StabilityLabel("Yr: ", vehicleData.stabDerivs[21], " m/s", "Change in Y-direction acceleration with respect to yaw-right rate; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Lβ: ", vehicleData.stabDerivs[16], " s⁻²", "Change in roll-right angular acceleration with respect to sideslip angle β; should be negative", 160, -1);
            StabilityLabel("Lp: ", vehicleData.stabDerivs[19], " s⁻¹", "Change in roll-right angular acceleration with respect to roll-right rate; should be negative", 160, -1);
            StabilityLabel("Lr: ", vehicleData.stabDerivs[22], " s⁻¹", "Change in roll-right angular acceleration with respect to yaw-right rate; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Nβ: ", vehicleData.stabDerivs[17], " s⁻²", "Change in yaw-right angular acceleration with respect to sideslip angle β; should be positive", 160, 1);
            StabilityLabel("Np: ", vehicleData.stabDerivs[20], " s⁻¹", "Change in yaw-right angular acceleration with respect to roll-right rate; sign unimportant", 160, 0);
            StabilityLabel("Nr: ", vehicleData.stabDerivs[23], " s⁻¹", "Change in yaw-right angular acceleration with respect to yaw-right rate; should be negative", 160, -1);
            GUILayout.EndHorizontal();
            GUILayout.EndVertical();
            GUILayout.EndVertical();


        }
        private void LongitudinalGUI(StabilityDerivOutput vehicleData)
        {

            GUIStyle BackgroundStyle = new GUIStyle(GUI.skin.box);
            BackgroundStyle.hover = BackgroundStyle.active = BackgroundStyle.normal;

            GUILayout.BeginHorizontal();
            GUILayout.Label("Longitudinal Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();

            GUILayout.BeginVertical();
            GUILayout.BeginHorizontal();
            GUILayout.Label("Down Vel Derivatives", GUILayout.Width(160));
            GUILayout.Label("Fwd Vel Derivatives", GUILayout.Width(160));
            GUILayout.Label("Pitch Rate Derivatives", GUILayout.Width(160));
            GUILayout.Label("Pitch Ctrl Derivatives", GUILayout.Width(160));
            GUILayout.EndHorizontal();

            GUILayout.BeginVertical(BackgroundStyle);
            GUILayout.BeginHorizontal();
            StabilityLabel("Zw: ", vehicleData.stabDerivs[3], " s⁻¹", "Change in Z-direction acceleration with respect to Z-direction velocity; should be negative", 160, -1);
            StabilityLabel("Zu: ", vehicleData.stabDerivs[6], " s⁻¹", "Change in Z-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
            StabilityLabel("Zq: ", vehicleData.stabDerivs[9], " m/s", "Change in Z-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
            StabilityLabel("Zδe: ", vehicleData.stabDerivs[12], " m/s²", "Change in Z-direction acceleration with respect to pitch control input; should be negative", 160, 0);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Xw: ", vehicleData.stabDerivs[4], " s⁻¹", "Change in X-direction acceleration with respect to Z-direction velocity; sign unimportant", 160, 0);
            StabilityLabel("Xu: ", vehicleData.stabDerivs[7], " s⁻¹", "Change in X-direction acceleration with respect to X-direction velocity; should be negative", 160, -1);
            StabilityLabel("Xq: ", vehicleData.stabDerivs[10], " m/s", "Change in X-direction acceleration with respect to pitch-up rate; sign unimportant", 160, 0);
            StabilityLabel("Xδe: ", vehicleData.stabDerivs[13], " m/s²", "Change in X-direction acceleration with respect to pitch control input; sign unimportant", 160, 0);
            GUILayout.EndHorizontal();
            GUILayout.BeginHorizontal();
            StabilityLabel("Mw: ", vehicleData.stabDerivs[5], " (m * s)⁻¹", "Change in pitch-up angular acceleration with respect to Z-direction velocity; should be negative", 160, -1);
            StabilityLabel("Mu: ", vehicleData.stabDerivs[8], " (m * s)⁻¹", "Change in pitch-up angular acceleration acceleration with respect to X-direction velocity; sign unimportant", 160, 0);
            StabilityLabel("Mq: ", vehicleData.stabDerivs[11], " s⁻¹", "Change in pitch-up angular acceleration acceleration with respect to pitch-up rate; should be negative", 160, -1);
            StabilityLabel("Mδe: ", vehicleData.stabDerivs[14], " s⁻²", "Change in pitch-up angular acceleration acceleration with respect to pitch control input; should be positive", 160, 1);
            GUILayout.EndHorizontal();
            GUILayout.EndVertical();
            GUILayout.EndVertical();
        }
Exemplo n.º 18
0
        public StabilityDerivExportOutput CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double beta, double phi)
        {
            double pressure    = body.GetPressure(alt);
            double temperature = body.GetTemperature(alt);
            double density     = body.GetDensity(pressure, temperature);
            double sspeed      = body.GetSpeedOfSound(pressure, density);
            double u0          = sspeed * machNumber;
            double q           = u0 * u0 * density * 0.5f;

            Vector3d CoM;
            double   mass, area, MAC, b;

            _instantCondition.GetCoMAndSize(out CoM, out mass, out area, out MAC, out b);

            double effectiveG = _instantCondition.CalculateEffectiveGravity(body, alt, u0);
            double neededCl   = effectiveG * mass / (q * area);

            InstantConditionSimVars iterationSimVars =
                new InstantConditionSimVars(_instantCondition, body, alt, machNumber, neededCl, beta, phi, flapSetting, spoilers);
            InstantConditionSimInput           nominalInput;
            InstantConditionSimOutput          nominalOutput;
            InstantConditionSimIterationResult stableCondition =
                iterationSimVars.IterateForAlphaAndPitch(out nominalInput, out nominalOutput);

            InstantConditionSimInput  input = nominalInput.Clone();
            InstantConditionSimOutput pertOutput;

            double[] derivatives = new double[27];

            // update size (in practice MAC and b) to match stableCondition
            _instantCondition.GetCoMAndSize(out CoM, out mass, out area, out MAC, out b);

            double Ix, Iy, Iz;
            double Ixy, Iyz, Ixz;

            _instantCondition.GetInertia(CoM, out Ix, out Iy, out Iz, out Ixy, out Iyz, out Ixz);


            input.alpha = stableCondition.stableAoA + 2;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            // Rodhern: A change is made to the Xw formula. Theoretically doing " -= nominalOutput.Cl" is the most 'correct',
            //  it does however in some way mix the Cd value measured at 'stableAoA + 2' with a Cl value measured at 'StableAoA'.
            //  Because Cl and Cd are very AoA-dependent, the asymmetrical measurement (AoA+=[0;2]) is quite affected.
            double pertOutCl = pertOutput.Cl;

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);                   //vert vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= pertOutCl; // Rodhern: Convergence is worse, but possibly the numerical value is more useful this way.

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            derivatives[3] = pertOutput.Cl;  //Zw
            derivatives[4] = pertOutput.Cd;  //Xw
            derivatives[5] = pertOutput.Cm;  //Mw


            input.alpha             = stableCondition.stableAoA;
            input.fltenv.MachNumber = machNumber + 0.05;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;                   //fwd vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;
            pertOutput.Cm += 2 * nominalOutput.Cm;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            derivatives[6] = pertOutput.Cl;  //Zu
            derivatives[7] = pertOutput.Cd;  //Xu
            derivatives[8] = pertOutput.Cm;  //Mu


            input.fltenv.MachNumber = machNumber;
            input.alphaDot          = -3;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (3 * FARMathUtil.deg2rad);                   //pitch rate derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (3 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (3 * FARMathUtil.deg2rad);

            pertOutput.Cl *= -q * area / mass;
            pertOutput.Cd *= -q * area / mass;
            pertOutput.Cm *= q * area * MAC / Iy;

            derivatives[9]  = pertOutput.Cl; //Zq
            derivatives[10] = pertOutput.Cd; //Xq
            derivatives[11] = pertOutput.Cm; //Mq


            input.alphaDot = 0;
            double pitchDelta = (stableCondition.stablePitchValue > 0) ? -0.1 : 0.1;

            input.pitchValue = stableCondition.stablePitchValue + pitchDelta;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / pitchDelta;                   //elevator derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / pitchDelta;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / pitchDelta;

            pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
            pertOutput.Cd *= -q * area / mass; //  for Ze and Xe match those for Zu and Xu.
            pertOutput.Cm *= q * area * MAC / Iy;

            derivatives[12] = pertOutput.Cl; //Ze
            derivatives[13] = pertOutput.Cd; //Xe
            derivatives[14] = pertOutput.Cm; //Me


            input.pitchValue = stableCondition.stablePitchValue;
            input.beta       = (beta + 2);
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);               //sideslip angle derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            derivatives[15] = pertOutput.Cy;     //Yb
            derivatives[17] = pertOutput.Cn;     //Nb
            derivatives[16] = pertOutput.C_roll; //Lb


            input.beta   = beta;
            input.phiDot = -3;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (3 * FARMathUtil.deg2rad);               //roll rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (3 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (3 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            derivatives[18] = pertOutput.Cy;     //Yp
            derivatives[20] = pertOutput.Cn;     //Np
            derivatives[19] = pertOutput.C_roll; //Lp


            input.phiDot  = 0;
            input.betaDot = -3;
            iterationSimVars.ResetAndGetClCdCmSteady(input, out pertOutput);

            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (3 * FARMathUtil.deg2rad);               //yaw rate derivs
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (3 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (3 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            derivatives[21] = pertOutput.Cy;     //Yr
            derivatives[23] = pertOutput.Cn;     //Nr
            derivatives[22] = pertOutput.C_roll; //Lr


            input = new InstantConditionSimInput(body); // Reset to (an artificial) default condition
            _instantCondition.ResetClCdCmSteady(CoM, input);

            // Assign values to output variables
            StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput(stableCondition, derivatives);

            stabDerivOutput.nominalVelocity = u0;
            stabDerivOutput.altitude        = alt;
            stabDerivOutput.body            = body;
            stabDerivOutput.b              = b;
            stabDerivOutput.MAC            = MAC;
            stabDerivOutput.area           = area;
            stabDerivOutput.stabDerivs[0]  = Ix;
            stabDerivOutput.stabDerivs[1]  = Iy;
            stabDerivOutput.stabDerivs[2]  = Iz;
            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;

            // Assign values to export variables
            StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables();

            stabDerivExport.craftmass      = mass;
            stabDerivExport.envpressure    = pressure;
            stabDerivExport.envtemperature = temperature;
            stabDerivExport.envdensity     = density;
            stabDerivExport.envsoundspeed  = sspeed;
            stabDerivExport.envg           = _instantCondition.CalculateAccelerationDueToGravity(body, alt);
            stabDerivExport.sitmach        = machNumber;
            stabDerivExport.sitdynpres     = q;
            stabDerivExport.siteffg        = _instantCondition.CalculateEffectiveGravity(body, alt, u0);

            return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport));
        }
Exemplo n.º 19
0
        public StabilityDerivOutput CalculateStabilityDerivs(
            CelestialBody body,
            double alt,
            double machNumber,
            int flapSetting,
            bool spoilers,
            double alpha,
            double beta,
            double phi
            )
        {
            GasProperties properties = FARAtmosphere.GetGasProperties(body,
                                                                      new Vector3d(0, 0, alt),
                                                                      Planetarium.GetUniversalTime());

            double pressure    = properties.Pressure;
            double temperature = properties.Temperature;
            double density     = properties.Density;
            double sspeed      = properties.SpeedOfSound;
            double u0          = sspeed * machNumber;
            double q           = u0 * u0 * density * 0.5f;

            var stabDerivOutput = new StabilityDerivOutput
            {
                nominalVelocity = u0,
                altitude        = alt,
                body            = body
            };

            Vector3d CoM  = Vector3d.zero;
            double   mass = 0;

            double MAC  = 0;
            double b    = 0;
            double area = 0;

            double Ix  = 0;
            double Iy  = 0;
            double Iz  = 0;
            double Ixy = 0;
            double Iyz = 0;
            double Ixz = 0;

            var input      = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
            var pertOutput = new InstantConditionSimOutput();

            _instantCondition.GetClCdCmSteady(input, out InstantConditionSimOutput nominalOutput, true);

            List <Part> partsList = EditorLogic.SortedShipList;

            foreach (Part p in partsList)
            {
                if (FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM  += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
                FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
                if (w == null)
                {
                    continue;
                }
                if (w.isShielded)
                {
                    continue;
                }

                area += w.S;
                MAC  += w.GetMAC() * w.S;
                b    += w.Getb_2() * w.S;
                if (w is FARControllableSurface controllableSurface)
                {
                    controllableSurface.SetControlStateEditor(CoM,
                                                              p.transform.up,
                                                              0,
                                                              0,
                                                              0,
                                                              input.flaps,
                                                              input.spoilers);
                }
            }

            if (area.NearlyEqual(0))
            {
                area = _instantCondition._maxCrossSectionFromBody;
                MAC  = _instantCondition._bodyLength;
                b    = 1;
            }

            MAC  /= area;
            b    /= area;
            CoM  /= mass;
            mass *= 1000;

            stabDerivOutput.b    = b;
            stabDerivOutput.MAC  = MAC;
            stabDerivOutput.area = area;

            foreach (Part p in partsList)
            {
                if (p == null || FARAeroUtil.IsNonphysical(p))
                {
                    continue;
                }
                //This section handles the parallel axis theorem
                Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double  x2     = relPos.z * relPos.z;
                double  y2     = relPos.x * relPos.x;
                double  z2     = relPos.y * relPos.y;
                double  x      = relPos.z;
                double  y      = relPos.x;
                double  z      = relPos.y;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                {
                    partMass += p.GetResourceMass();
                }

                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass

                Ix += (y2 + z2) * partMass;
                Iy += (x2 + z2) * partMass;
                Iz += (x2 + y2) * partMass;

                Ixy += -x * y * partMass;
                Iyz += -z * y * partMass;
                Ixz += -x * z * partMass;

                //And this handles the part's own moment of inertia
                Vector3    principalInertia = p.Rigidbody.inertiaTensor;
                Quaternion prncInertRot     = p.Rigidbody.inertiaTensorRotation;

                //The rows of the direction cosine matrix for a quaternion
                var Row1 =
                    new Vector3(prncInertRot.x * prncInertRot.x -
                                prncInertRot.y * prncInertRot.y -
                                prncInertRot.z * prncInertRot.z +
                                prncInertRot.w * prncInertRot.w,
                                2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
                                2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));

                var Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
                                       -prncInertRot.x * prncInertRot.x +
                                       prncInertRot.y * prncInertRot.y -
                                       prncInertRot.z * prncInertRot.z +
                                       prncInertRot.w * prncInertRot.w,
                                       2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));

                var Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
                                       2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
                                       -prncInertRot.x * prncInertRot.x -
                                       prncInertRot.y * prncInertRot.y +
                                       prncInertRot.z * prncInertRot.z +
                                       prncInertRot.w * prncInertRot.w);


                //And converting the principal moments of inertia into the coordinate system used by the system
                Ix += principalInertia.x * Row1.x * Row1.x +
                      principalInertia.y * Row1.y * Row1.y +
                      principalInertia.z * Row1.z * Row1.z;
                Iy += principalInertia.x * Row2.x * Row2.x +
                      principalInertia.y * Row2.y * Row2.y +
                      principalInertia.z * Row2.z * Row2.z;
                Iz += principalInertia.x * Row3.x * Row3.x +
                      principalInertia.y * Row3.y * Row3.y +
                      principalInertia.z * Row3.z * Row3.z;

                Ixy += principalInertia.x * Row1.x * Row2.x +
                       principalInertia.y * Row1.y * Row2.y +
                       principalInertia.z * Row1.z * Row2.z;
                Ixz += principalInertia.x * Row1.x * Row3.x +
                       principalInertia.y * Row1.y * Row3.y +
                       principalInertia.z * Row1.z * Row3.z;
                Iyz += principalInertia.x * Row2.x * Row3.x +
                       principalInertia.y * Row2.y * Row3.y +
                       principalInertia.z * Row2.z * Row3.z;
            }

            Ix *= 1000;
            Iy *= 1000;
            Iz *= 1000;

            stabDerivOutput.stabDerivs[0] = Ix;
            stabDerivOutput.stabDerivs[1] = Iy;
            stabDerivOutput.stabDerivs[2] = Iz;

            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;

            //This is the effect of gravity
            double effectiveG = InstantConditionSim.CalculateAccelerationDueToGravity(body, alt);

            //This is the effective reduction of gravity due to high velocity
            effectiveG -= u0 * u0 / (alt + body.Radius);
            double neededCl = mass * effectiveG / (q * area);


            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
            //Longitudinal Mess
            _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
            FARMathUtil.OptimizationResult optResult =
                FARMathUtil.Secant(_instantCondition.FunctionIterateForAlpha,
                                   0,
                                   10,
                                   1e-4,
                                   1e-4,
                                   minLimit: -90,
                                   maxLimit: 90);
            int calls = optResult.FunctionCalls;

            // if stable AoA doesn't exist, calculate derivatives at 0 incidence
            if (!optResult.Converged)
            {
                FARLogger.Info("Stable angle of attack not found, calculating derivatives at 0 incidence instead");
                alpha = 0;
                _instantCondition.FunctionIterateForAlpha(alpha);
                calls += 1;
            }
            else
            {
                alpha = optResult.Result;
            }

            input.alpha   = alpha;
            nominalOutput = _instantCondition.iterationOutput;

            input.alpha = alpha + 2;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            stabDerivOutput.stableCl       = neededCl;
            stabDerivOutput.stableCd       = nominalOutput.Cd;
            stabDerivOutput.stableAoA      = alpha;
            stabDerivOutput.stableAoAState = "";
            if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
            {
                stabDerivOutput.stableAoAState = nominalOutput.Cl > neededCl ? "<" : ">";
            }

            FARLogger.Info("Cl needed: " +
                           neededCl.ToString(CultureInfo.InvariantCulture) +
                           ", AoA: " +
                           stabDerivOutput.stableAoA.ToString(CultureInfo.InvariantCulture) +
                           ", Cl: " +
                           nominalOutput.Cl.ToString(CultureInfo.InvariantCulture) +
                           ", Cd: " +
                           nominalOutput.Cd.ToString(CultureInfo.InvariantCulture) +
                           ", function calls: " +
                           calls.ToString());

            //vert vel derivs
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= nominalOutput.Cl;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
            stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
            stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw

            // Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
            //  is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
            //  here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057

            input.alpha      = alpha;
            input.machNumber = machNumber + 0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);

            //fwd vel derivs
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (u0 * Iy);

            stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
            stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
            stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu

            input.machNumber = machNumber;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.alphaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);

            //pitch rate derivs
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;

            pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
            pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); //  for Zq and Xq match those for Zu and Xu.
            pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);

            stabDerivOutput.stabDerivs[9]  = pertOutput.Cl; //Zq
            stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
            stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq

            input.alphaDot   = 0;
            input.pitchValue = 0.1;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);

            //elevator derivs
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;

            pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
            pertOutput.Cd *= -q * area / mass; //  for Ze and Xe match those for Zu and Xu.
            pertOutput.Cm *= q * area * MAC / Iy;

            stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
            stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
            stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me

            //Lateral Mess

            input.pitchValue = 0;
            input.beta       = beta + 2;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);
            //sideslip angle derivs
            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy     *= q * area / mass;
            pertOutput.Cn     *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            stabDerivOutput.stabDerivs[15] = pertOutput.Cy;     //Yb
            stabDerivOutput.stabDerivs[17] = pertOutput.Cn;     //Nb
            stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb

            input.beta = beta;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.phiDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);

            //roll rate derivs
            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / 0.05;
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[18] = pertOutput.Cy;     //Yp
            stabDerivOutput.stabDerivs[20] = pertOutput.Cn;     //Np
            stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp


            input.phiDot = 0;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.betaDot = -0.05;

            //yaw rate derivs
            _instantCondition.GetClCdCmSteady(input, out pertOutput, true);
            pertOutput.Cy     = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;
            pertOutput.Cn     = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;

            pertOutput.Cy     *= q * area * b / (2 * mass * u0);
            pertOutput.Cn     *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[21] = pertOutput.Cy;     //Yr
            stabDerivOutput.stabDerivs[23] = pertOutput.Cn;     //Nr
            stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr

            return(stabDerivOutput);
        }
Exemplo n.º 20
0
        public GraphData RunTransientSimLateral(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);

            A.PrintToConsole();

            int i   = 0;
            int j   = 0;
            int num = 0;

            double[] Derivs = new double[27];

            vehicleData.stabDerivs.CopyTo(Derivs, 0);

            Derivs[15] = Derivs[15] / vehicleData.nominalVelocity;
            Derivs[18] = Derivs[18] / vehicleData.nominalVelocity;
            Derivs[21] = Derivs[21] / vehicleData.nominalVelocity - 1;

            double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
            Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;

            Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
            Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;

            Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
            Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;

            for (int k = 0; k < Derivs.Length; k++)
            {
                double f = Derivs[k];
                if (num < 15)
                {
                    num++;              //Avoid Ix, Iy, Iz and long derivs
                    continue;
                }
                else
                {
                    num++;
                }
                FARLogger.Info("" + i + "," + j);
                if (i <= 2)
                {
                    A.Add(f, i, j);
                }

                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }
            A.Add(_instantCondition.CalculateAccelerationDueToGravity(vehicleData.body, vehicleData.altitude) * Math.Cos(vehicleData.stableAoA * Math.PI / 180) / vehicleData.nominalVelocity, 3, 0);
            A.Add(1, 1, 3);


            A.PrintToConsole();                //We should have an array that looks like this:

            /*             i --------------->
             *       j  [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) ,  g Cos(θ0) / u0 ]
             *       |  [   Lb    ,    Lp   ,      Lr       ,          0          ]
             *       |  [   Nb    ,    Np   ,      Nr       ,          0          ]
             *      \ / [    0    ,    1    ,      0        ,          0          ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */
            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            GraphData lines = new GraphData();

            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "β", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "p", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "r", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "φ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
             * graph.SetGridScaleUsingValues(1, 5);
             * graph.horizontalLabel = "time";
             * graph.verticalLabel = "value";
             * graph.Update();*/

            return(lines);
        }
        public GraphData RunTransientSimLongitudinal(StabilityDerivOutput vehicleData, double endTime, double initDt, double[] InitCond)
        {
            SimMatrix A = new SimMatrix(4, 4);

            int i = 0;
            int j = 0;

            double[] Derivs = new double[27];

            vehicleData.stabDerivs.CopyTo(Derivs, 0);

            double MAC2u = vehicleData.MAC / (2 * vehicleData.nominalVelocity);
            double effg  = _instantCondition.CalculateEffectiveGravity(vehicleData.body, vehicleData.altitude, vehicleData.nominalVelocity);

            FARLogger.Info("MAC/(2u)= " + MAC2u + " IGNORED!");
            FARLogger.Info("effg= " + effg);

            // Rodhern: For possible backward compability the rotation (moment) derivatives can be
            //  scaled by "mac/(2u)" (pitch) and "b/(2u)" (roll and yaw).
            //for (int h = 9; h <= 11; h++)
            //    Derivs[h] = Derivs[h] * MAC2u;

            Derivs[9] = Derivs[9] + vehicleData.nominalVelocity;

            for (int k = 3; k < 15 && k < Derivs.Length; k++)
            {
                double f = Derivs[k];

                if (i <= 2)
                {
                    FARLogger.Info("A[" + i + "," + j + "]= f_" + k + " = " + f);
                    A.Add(f, i, j);
                }
                else
                {
                    FARLogger.Debug("Ignore B[0," + j + "]= " + f);
                }

                if (j < 2)
                {
                    j++;
                }
                else
                {
                    j = 0;
                    i++;
                }
            }
            A.Add(-effg, 3, 1);
            A.Add(1, 2, 3);

            A.PrintToConsole();                //We should have an array that looks like this:

            /*            i --------------->
             *       j  [ Z w , Z u , Z q  + u,  0 ]
             *       |  [ X w , X u , X q     , -g ]
             *       |  [ M w , M u , M q     ,  0 ]
             *      \ / [  0  ,  0  ,  1      ,  0 ]
             *       V
             */
            //And one that looks like this: (Unused)

            /*
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             */

            RungeKutta4 transSolve = new RungeKutta4(endTime, initDt, A, InitCond);

            transSolve.Solve();

            GraphData lines = new GraphData();

            lines.xValues = transSolve.time;

            double[] yVal = transSolve.GetSolution(0);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(3), "w", true);

            yVal = transSolve.GetSolution(1);
            ScaleAndClampValues(yVal, 1, 50);
            lines.AddData(yVal, GUIColors.GetColor(2), "u", true);

            yVal = transSolve.GetSolution(2);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(1), "q", true);

            yVal = transSolve.GetSolution(3);
            ScaleAndClampValues(yVal, 180 / Math.PI, 50);
            lines.AddData(yVal, GUIColors.GetColor(0), "θ", true);

            /*graph.SetBoundaries(0, endTime, -10, 10);
             * graph.SetGridScaleUsingValues(1, 5);
             * graph.horizontalLabel = "time";
             * graph.verticalLabel = "value";
             * graph.Update();*/

            return(lines);
        }