Updated simulation logic.
[VesselSimulator.git] / KerbalEngineer / VesselSimulator / Simulation.cs
blob:a/KerbalEngineer/VesselSimulator/Simulation.cs -> blob:b/KerbalEngineer/VesselSimulator/Simulation.cs
--- a/KerbalEngineer/VesselSimulator/Simulation.cs
+++ b/KerbalEngineer/VesselSimulator/Simulation.cs
@@ -1,70 +1,124 @@
-// Kerbal Engineer Redux
-// Author:  CYBUTEK
-// License: Attribution-NonCommercial-ShareAlike 3.0 Unported
-//
-// This class has taken a lot of inspiration from r4m0n's MuMech FuelFlowSimulator.  Although extremely
-// similar to the code used within MechJeb, it is a clean re-write.  The similarities are a testiment
-// to how well the MuMech code works and the robustness of the simulation algorithem used.

-

-using System;

-using System.Collections.Generic;

-using System.Diagnostics;

-using System.Text;

-

-using UnityEngine;

-

+// 
+//     Kerbal Engineer Redux
+// 
+//     Copyright (C) 2014 CYBUTEK
+// 
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+// 
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+// 
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+
+#region Using Directives
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Text;
+
+using UnityEngine;
+
+#endregion
+
 namespace KerbalEngineer.VesselSimulator
 {
+    using CompoundParts;
+    using Extensions;
+    using Helpers;
+
     public class Simulation
     {
-        private List<Part> partList;
-
-        private List<PartSim> allParts;
-        private List<PartSim> allFuelLines;
-        private HashSet<PartSim> drainingParts;
-        private List<EngineSim> allEngines;
-        private List<EngineSim> activeEngines;
-        private HashSet<int> drainingResources;
-
-        private int lastStage = 0;
-        private int currentStage = 0;
-        private bool doingCurrent = false;
-
+        private const double SECONDS_PER_DAY = 86400;
+        private readonly Stopwatch _timer = new Stopwatch();
+        private List<EngineSim> activeEngines = new List<EngineSim>();
+        private List<EngineSim> allEngines = new List<EngineSim>();
+        private List<PartSim> allFuelLines = new List<PartSim>();
+        private List<PartSim> allParts = new List<PartSim>();
+        private Dictionary<Part, PartSim> partSimLookup = new Dictionary<Part, PartSim>();
+        private double atmosphere;
+        private int currentStage;
+        private double currentisp;
+        private bool doingCurrent;
+        private List<PartSim> dontStageParts = new List<PartSim>();
+        List<List<PartSim>> dontStagePartsLists = new List<List<PartSim>>();
+        private HashSet<PartSim> drainingParts = new HashSet<PartSim>();
+        private HashSet<int> drainingResources = new HashSet<int>();
+        private HashSet<PartSim> decoupledParts = new HashSet<PartSim>();
+        private double gravity;
+
+        private int lastStage;
+        private List<Part> partList = new List<Part>();
+        private double simpleTotalThrust;
+        private double stageStartMass;
+        private Vector3d stageStartCom;
+        private double stageTime;
+        private double stepEndMass;
+        private double stepStartMass;
+        private double totalStageActualThrust;
+        private double totalStageFlowRate;
+        private double totalStageIspFlowRate;
+        private double totalStageThrust;
+        private ForceAccumulator totalStageThrustForce = new ForceAccumulator();
+        private Vector3 vecActualThrust;
+        private Vector3 vecStageDeltaV;
+        private Vector3 vecThrust;
+        private double mach;
+        private float maxMach;
         public String vesselName;
         public VesselType vesselType;
-
-        private double stageTime = 0d;
-        private Vector3 vecStageDeltaV;
-        private double simpleTotalThrust = 0d;
-        private double totalStageThrust = 0d;
-        private double totalStageActualThrust = 0d;
-        private Vector3 vecThrust;
-        private Vector3 vecActualThrust;
-        private double totalStageFlowRate = 0d;
-        private double totalStageIspFlowRate = 0d;
-        private double currentisp = 0d;
-        private double stageStartMass = 0d;
-        private double stepStartMass = 0d;
-        private double stepEndMass = 0d;
-
-        private double gravity = 0d;
-        private double atmosphere = 0d;
-        private double velocity = 0d;
-        private Stopwatch _timer = new Stopwatch();
-        private const double STD_GRAVITY = 9.81d;
-        private const double SECONDS_PER_DAY = 86400d;
-        
+        private WeightedVectorAverager vectorAverager = new WeightedVectorAverager();
+
         public Simulation()
         {
             if (SimManager.logOutput)
+            {
                 MonoBehaviour.print("Simulation created");
+            }
+        }
+
+        private double ShipMass
+        {
+            get
+            {
+                double mass = 0d;
+
+                for (int i = 0; i < allParts.Count; ++i) { 
+                    mass += allParts[i].GetMass();
+                }
+
+                return mass;
+            }
+        }
+
+        private Vector3d ShipCom
+        {
+            get
+            {
+                vectorAverager.Reset();
+
+                for (int i = 0; i < allParts.Count; ++i)
+                {
+                    PartSim partSim = allParts[i];
+                    vectorAverager.Add(partSim.centerOfMass, partSim.GetMass());
+                }
+
+                return vectorAverager.Get();
+            }
         }
 
         // This function prepares the simulation by creating all the necessary data structures it will 
         // need during the simulation.  All required data is copied from the core game data structures 
         // so that the simulation itself can be run in a background thread without having issues with 
         // the core game changing the data while the simulation is running.
-        public bool PrepareSimulation(List<Part> parts, double theGravity, double theAtmosphere = 0, double theVelocity = 0, bool dumpTree = false, bool vectoredThrust = false)
+        public bool PrepareSimulation(List<Part> parts, double theGravity, double theAtmosphere = 0, double theMach = 0, bool dumpTree = false, bool vectoredThrust = false, bool fullThrust = false)
         {
             LogMsg log = null;
             if (SimManager.logOutput)
@@ -79,20 +133,24 @@
             this.partList = parts;
             this.gravity = theGravity;
             this.atmosphere = theAtmosphere;
-            this.velocity = theVelocity;
+            this.mach = theMach;
             this.lastStage = Staging.lastStage;
             //MonoBehaviour.print("lastStage = " + lastStage);
 
-            // Create the lists for our simulation parts
-            this.allParts = new List<PartSim>();
-            this.allFuelLines = new List<PartSim>();
-            this.drainingParts = new HashSet<PartSim>();
-            this.allEngines = new List<EngineSim>();
-            this.activeEngines = new List<EngineSim>();
-            this.drainingResources = new HashSet<int>();
+            // Clear the lists for our simulation parts
+            allParts.Clear();
+            allFuelLines.Clear();
+            drainingParts.Clear();
+            allEngines.Clear();
+            activeEngines.Clear();
+            drainingResources.Clear();
+
+            PartSim.ReleaseAll();
+            EngineSim.ReleaseAll();
+            AttachNodeSim.ReleaseAll();
 
             // A dictionary for fast lookup of Part->PartSim during the preparation phase
-            Dictionary<Part, PartSim> partSimLookup = new Dictionary<Part, PartSim>();
+            partSimLookup.Clear();
 
             if (this.partList.Count > 0 && this.partList[0].vessel != null)
             {
@@ -102,28 +160,41 @@
 
             // First we create a PartSim for each Part (giving each a unique id)
             int partId = 1;
-            foreach (Part part in this.partList)
-            {
+            for (int i = 0; i < partList.Count; ++i)
+            {
+                Part part = partList[i];
+
                 // If the part is already in the lookup dictionary then log it and skip to the next part
                 if (partSimLookup.ContainsKey(part))
                 {
                     if (log != null)
+                    {
                         log.buf.AppendLine("Part " + part.name + " appears in vessel list more than once");
+                    }
                     continue;
                 }
 
                 // Create the PartSim
-                PartSim partSim = new PartSim(part, partId, this.atmosphere, log);
+                PartSim partSim = PartSim.GetPoolObject().Initialise(part, partId, this.atmosphere, log);
 
                 // Add it to the Part lookup dictionary and the necessary lists
                 partSimLookup.Add(part, partSim);
                 this.allParts.Add(partSim);
                 if (partSim.isFuelLine)
+                {
                     this.allFuelLines.Add(partSim);
+                }
                 if (partSim.isEngine)
-                    partSim.CreateEngineSims(this.allEngines, this.atmosphere, this.velocity, vectoredThrust, log);
+                {
+                    partSim.CreateEngineSims(this.allEngines, this.atmosphere, this.mach, vectoredThrust, fullThrust, log);
+                }
 
                 partId++;
+            }
+
+            for (int i = 0; i < allEngines.Count; ++i)
+            {
+                maxMach = Mathf.Max(maxMach, allEngines[i].maxMach);
             }
 
             this.UpdateActiveEngines();
@@ -134,48 +205,63 @@
             {
                 partSim.SetupParent(partSimLookup, log);
             }
-            
+
             // Then, in the VAB/SPH, we add the parent of each fuel line to the fuelTargets list of their targets
             if (HighLogic.LoadedSceneIsEditor)
             {
-                foreach (PartSim partSim in this.allFuelLines)
-                {
-                    if ((partSim.part as FuelLine).target != null)
+                for (int i = 0; i < allFuelLines.Count; ++i)
+                {
+                    PartSim partSim = allFuelLines[i];
+
+                    CModuleFuelLine fuelLine = partSim.part.GetModule<CModuleFuelLine>();
+                    if (fuelLine.target != null)
                     {
                         PartSim targetSim;
-                        if (partSimLookup.TryGetValue((partSim.part as FuelLine).target, out targetSim))
+                        if (partSimLookup.TryGetValue(fuelLine.target, out targetSim))
                         {
                             if (log != null)
+                            {
                                 log.buf.AppendLine("Fuel line target is " + targetSim.name + ":" + targetSim.partId);
+                            }
 
                             targetSim.fuelTargets.Add(partSim.parent);
                         }
                         else
                         {
                             if (log != null)
+                            {
                                 log.buf.AppendLine("No PartSim for fuel line target (" + partSim.part.partInfo.name + ")");
+                            }
                         }
                     }
                     else
                     {
                         if (log != null)
+                        {
                             log.buf.AppendLine("Fuel line target is null");
+                        }
                     }
                 }
             }
 
             //MonoBehaviour.print("SetupAttachNodes and count stages");
-            foreach (PartSim partSim in this.allParts)
-            {
+            for (int i = 0; i < allParts.Count; ++i)
+            {
+                PartSim partSim = allParts[i];
+
                 partSim.SetupAttachNodes(partSimLookup, log);
                 if (partSim.decoupledInStage >= this.lastStage)
+                {
                     this.lastStage = partSim.decoupledInStage + 1;
+                }
             }
 
             // And finally release the Part references from all the PartSims
             //MonoBehaviour.print("ReleaseParts");
-            foreach (PartSim partSim in this.allParts)
-                partSim.ReleasePart();
+            for (int i = 0; i < allParts.Count; ++i)
+            { 
+                allParts[i].ReleasePart();
+            }
 
             // And dereference the core's part list
             this.partList = null;
@@ -188,48 +274,66 @@
             }
 
             if (dumpTree)
+            {
                 this.Dump();
+            }
 
             return true;
         }
 
-        
         // This function runs the simulation and returns a newly created array of Stage objects
         public Stage[] RunSimulation()
         {
             if (SimManager.logOutput)
+            {
                 MonoBehaviour.print("RunSimulation started");
+            }
+
             this._timer.Start();
+
+            LogMsg log = null;
+            if (SimManager.logOutput)
+            {
+                log = new LogMsg();
+            }
+
             // Start with the last stage to simulate
             // (this is in a member variable so it can be accessed by AllowedToStage and ActivateStage)
             this.currentStage = this.lastStage;
 
-            LogMsg log = null;
-            if (SimManager.logOutput)
-                log = new LogMsg();
-
             // Work out which engines would be active if just doing the staging and if this is different to the 
             // currently active engines then generate an extra stage
             // Loop through all the engines
-            foreach (EngineSim engine in this.allEngines)
-            {
+            bool anyActive = false;
+            for (int i = 0; i < allEngines.Count; ++i)
+            {
+                EngineSim engine = allEngines[i];
+
                 if (log != null)
+                {
                     log.buf.AppendLine("Testing engine mod of " + engine.partSim.name + ":" + engine.partSim.partId);
+                }
                 bool bActive = engine.isActive;
                 bool bStage = (engine.partSim.inverseStage >= this.currentStage);
                 if (log != null)
+                {
                     log.buf.AppendLine("bActive = " + bActive + "   bStage = " + bStage);
+                }
                 if (HighLogic.LoadedSceneIsFlight)
                 {
+                    if (bActive)
+                    {
+                        anyActive = true;
+                    }
                     if (bActive != bStage)
                     {
                         // If the active state is different to the state due to staging
                         if (log != null)
+                        {
                             log.buf.AppendLine("Need to do current active engines first");
+                        }
 
                         this.doingCurrent = true;
-                        this.currentStage++;
-                        break;
                     }
                 }
                 else
@@ -237,18 +341,38 @@
                     if (bStage)
                     {
                         if (log != null)
+                        {
                             log.buf.AppendLine("Marking as active");
+                        }
 
                         engine.isActive = true;
                     }
                 }
             }
 
+            // If we need to do current because of difference in engine activation and there actually are active engines
+            // then we do the extra stage otherwise activate the next stage and don't treat it as current
+            if (this.doingCurrent && anyActive)
+            {
+                this.currentStage++;
+            }
+            else
+            {
+                this.ActivateStage();
+                this.doingCurrent = false;
+            }
+
+            // Create a list of lists of PartSims that prevent decoupling
+            BuildDontStageLists(log);
+
             if (log != null)
+            {
                 log.Flush();
+            }
 
             // Create the array of stages that will be returned
             Stage[] stages = new Stage[this.currentStage + 1];
+
 
             // Loop through the stages
             while (this.currentStage >= 0)
@@ -270,7 +394,10 @@
 
                 this.stageTime = 0d;
                 this.vecStageDeltaV = Vector3.zero;
+
                 this.stageStartMass = this.ShipMass;
+                this.stageStartCom = this.ShipCom;
+
                 this.stepStartMass = this.stageStartMass;
                 this.stepEndMass = 0;
 
@@ -286,9 +413,33 @@
                 stage.actualThrust = this.totalStageActualThrust;
                 stage.actualThrustToWeight = this.totalStageActualThrust / (this.stageStartMass * this.gravity);
 
-                // Calculate the cost and mass of this stage
-                foreach (PartSim partSim in this.allParts)
-                {
+                // calculate torque and associates
+                stage.maxThrustTorque = this.totalStageThrustForce.TorqueAt(this.stageStartCom).magnitude;
+
+                // torque divided by thrust. imagine that all engines are at the end of a lever that tries to turn the ship.
+                // this numerical value, in meters, would represent the length of that lever.
+                double torqueLeverArmLength = (stage.thrust <= 0) ? 0 : stage.maxThrustTorque / stage.thrust;
+
+                // how far away are the engines from the CoM, actually?
+                double thrustDistance = (this.stageStartCom - this.totalStageThrustForce.GetAverageForceApplicationPoint()).magnitude;
+
+                // the combination of the above two values gives an approximation of the offset angle.
+                double sinThrustOffsetAngle = 0;
+                if (thrustDistance > 1e-7) {
+                    sinThrustOffsetAngle = torqueLeverArmLength / thrustDistance;
+                    if (sinThrustOffsetAngle > 1) {
+                        sinThrustOffsetAngle = 1;
+                    }
+                }
+
+                stage.thrustOffsetAngle = Math.Asin(sinThrustOffsetAngle) * 180 / Math.PI;
+
+                // Calculate the cost and mass of this stage and add all engines and tanks that are decoupled
+                // in the next stage to the dontStageParts list
+                for (int i = 0; i < allParts.Count; ++i)
+                {
+                    PartSim partSim = allParts[i];
+
                     if (partSim.decoupledInStage == this.currentStage - 1)
                     {
                         stage.cost += partSim.cost;
@@ -296,8 +447,27 @@
                     }
                 }
 
+                this.dontStageParts = dontStagePartsLists[this.currentStage];
+
                 if (log != null)
-                    MonoBehaviour.print("Stage setup took " + this._timer.ElapsedMilliseconds + "ms");
+                {
+                    log.buf.AppendLine("Stage setup took " + this._timer.ElapsedMilliseconds + "ms");
+
+                    if (this.dontStageParts.Count > 0)
+                    {
+                        log.buf.AppendLine("Parts preventing staging:");
+                        foreach (PartSim partSim in this.dontStageParts)
+                        {
+                            partSim.DumpPartToBuffer(log.buf, "");
+                        }
+                    }
+                    else
+                    {
+                        log.buf.AppendLine("No parts preventing staging");
+                    }
+
+                    log.Flush();
+                }
 
                 // Now we will loop until we are allowed to stage
                 int loopCounter = 0;
@@ -320,10 +490,14 @@
                     }
 
                     if (log != null)
+                    {
                         MonoBehaviour.print("Drain time = " + resourceDrainTime + " (" + partMinDrain.name + ":" + partMinDrain.partId + ")");
+                    }
 
                     foreach (PartSim partSim in this.drainingParts)
+                    {
                         partSim.DrainResources(resourceDrainTime);
+                    }
 
                     // Get the mass after draining
                     this.stepEndMass = this.ShipMass;
@@ -334,13 +508,17 @@
                     //MonoBehaviour.print("currentThrust = " + totalStageThrust);
                     //MonoBehaviour.print("currentTWR = " + stepEndTWR);
                     if (stepEndTWR > stage.maxThrustToWeight)
+                    {
                         stage.maxThrustToWeight = stepEndTWR;
+                    }
 
                     //MonoBehaviour.print("newMaxTWR = " + stage.maxThrustToWeight);
 
                     // If we have drained anything and the masses make sense then add this step's deltaV to the stage total
                     if (resourceDrainTime > 0d && this.stepStartMass > this.stepEndMass && this.stepStartMass > 0d && this.stepEndMass > 0d)
-                        this.vecStageDeltaV += this.vecThrust * (float)((this.currentisp * STD_GRAVITY * Math.Log(this.stepStartMass / this.stepEndMass)) / this.simpleTotalThrust);
+                    {
+                        this.vecStageDeltaV += this.vecThrust * (float)((this.currentisp * Units.GRAVITY * Math.Log(this.stepStartMass / this.stepEndMass)) / this.simpleTotalThrust);
+                    }
 
                     // Update the active engines and resource drains for the next step
                     this.UpdateResourceDrains();
@@ -362,6 +540,10 @@
                         MonoBehaviour.print("stageStartMass = " + this.stageStartMass);
                         MonoBehaviour.print("stepStartMass = " + this.stepStartMass);
                         MonoBehaviour.print("StepEndMass   = " + this.stepEndMass);
+                        Logger.Log("exceeded loop count");
+                        Logger.Log("stageStartMass = " + this.stageStartMass);
+                        Logger.Log("stepStartMass = " + this.stepStartMass);
+                        Logger.Log("StepEndMass   = " + this.stepEndMass);
                         break;
                     }
 
@@ -373,17 +555,24 @@
 
                 // Store the magnitude of the deltaV vector
                 stage.deltaV = this.vecStageDeltaV.magnitude;
-                
+                stage.resourceMass = this.stageStartMass - this.stepEndMass;
+
                 // Recalculate effective stage isp from the stage deltaV (flip the standard deltaV calculation around)
                 // Note: If the mass doesn't change then this is a divide by zero
                 if (this.stageStartMass != this.stepStartMass)
-                    stage.isp = stage.deltaV / (STD_GRAVITY * Math.Log(this.stageStartMass / this.stepStartMass));
+                {
+                    stage.isp = stage.deltaV / (Units.GRAVITY * Math.Log(this.stageStartMass / this.stepStartMass));
+                }
                 else
+                {
                     stage.isp = 0;
+                }
 
                 // Zero stage time if more than a day (this should be moved into the window code)
                 stage.time = (this.stageTime < SECONDS_PER_DAY) ? this.stageTime : 0d;
-                stage.number = this.doingCurrent ? -1 : this.currentStage;                // Set the stage number to -1 if doing current engines
+                stage.number = this.doingCurrent ? -1 : this.currentStage; // Set the stage number to -1 if doing current engines
+                stage.totalPartCount = this.allParts.Count;
+                stage.maxMach = maxMach;
                 stages[this.currentStage] = stage;
 
                 // Now activate the next stage
@@ -421,6 +610,7 @@
                     stages[i].totalMass += stages[j].mass;
                     stages[i].totalDeltaV += stages[j].deltaV;
                     stages[i].totalTime += stages[j].time;
+                    stages[i].partCount = i > 0 ? stages[i].totalPartCount - stages[i - 1].totalPartCount : stages[i].totalPartCount;
                 }
                 // We also total up the deltaV for stage and all stages below
                 for (int j = i; j < stages.Length; j++)
@@ -431,7 +621,9 @@
                 // Zero the total time if the value will be huge (24 hours?) to avoid the display going weird
                 // (this should be moved into the window code)
                 if (stages[i].totalTime > SECONDS_PER_DAY)
+                {
                     stages[i].totalTime = 0d;
+                }
             }
 
             if (log != null)
@@ -443,14 +635,72 @@
             return stages;
         }
 
+        private void BuildDontStageLists(LogMsg log)
+        {
+            if (log != null)
+            {
+                log.buf.AppendLine("Creating list with capacity of " + (this.currentStage + 1));
+            }
+
+            dontStagePartsLists.Clear();
+            for (int i = 0; i <= this.currentStage; i++)
+            {
+                if (i < dontStagePartsLists.Count)
+                {
+                    dontStagePartsLists[i].Clear();
+                }
+                else
+                {
+                    dontStagePartsLists.Add(new List<PartSim>());
+                }
+            }
+
+            for (int i = 0; i < allParts.Count; ++i)
+            {
+                PartSim partSim = allParts[i];
+
+                if (partSim.isEngine || !partSim.Resources.Empty)
+                {
+                    if (log != null)
+                    {
+                        log.buf.AppendLine(partSim.name + ":" + partSim.partId + " is engine or tank, decoupled = " + partSim.decoupledInStage);
+                    }
+
+                    if (partSim.decoupledInStage < -1 || partSim.decoupledInStage > this.currentStage - 1)
+                    {
+                        if (log != null)
+                        {
+                            log.buf.AppendLine("decoupledInStage out of range");
+                        }
+                    }
+                    else
+                    {
+                        dontStagePartsLists[partSim.decoupledInStage + 1].Add(partSim);
+                    }
+                }
+            }
+
+            for (int i = 1; i <= this.lastStage; i++)
+            {
+                if (dontStagePartsLists[i].Count == 0)
+                {
+                    dontStagePartsLists[i] = dontStagePartsLists[i - 1];
+                }
+            }
+        }
+
         // This function simply rebuilds the active engines by testing the isActive flag of all the engines
         private void UpdateActiveEngines()
         {
             this.activeEngines.Clear();
-            foreach (EngineSim engine in this.allEngines)
-            {
+            for (int i = 0; i < allEngines.Count; ++i)
+            {
+                EngineSim engine = allEngines[i];
+
                 if (engine.isActive)
+                {
                     this.activeEngines.Add(engine);
+                }
             }
         }
 
@@ -464,17 +714,25 @@
             this.totalStageActualThrust = 0d;
             this.totalStageFlowRate = 0d;
             this.totalStageIspFlowRate = 0d;
+            this.totalStageThrustForce.Reset();
 
             // Loop through all the active engines totalling the thrust, actual thrust and mass flow rates
             // The thrust is totalled as vectors
-            foreach (EngineSim engine in this.activeEngines)
-            {
+            for (int i = 0; i < activeEngines.Count; ++i)
+            {
+                EngineSim engine = activeEngines[i];
+
                 this.simpleTotalThrust += engine.thrust;
                 this.vecThrust += ((float)engine.thrust * engine.thrustVec);
                 this.vecActualThrust += ((float)engine.actualThrust * engine.thrustVec);
 
                 this.totalStageFlowRate += engine.ResourceConsumptions.Mass;
                 this.totalStageIspFlowRate += engine.ResourceConsumptions.Mass * engine.isp;
+
+                for (int j = 0; j < engine.appliedForces.Count; ++j)
+                {
+                    this.totalStageThrustForce.AddForce(engine.appliedForces[j]);
+                }
             }
 
             //MonoBehaviour.print("vecThrust = " + vecThrust.ToString() + "   magnitude = " + vecThrust.magnitude);
@@ -484,9 +742,13 @@
 
             // Calculate the effective isp at this point
             if (this.totalStageFlowRate > 0d && this.totalStageIspFlowRate > 0d)
+            {
                 this.currentisp = this.totalStageIspFlowRate / this.totalStageFlowRate;
+            }
             else
+            {
                 this.currentisp = 0;
+            }
         }
 
         // This function does all the hard work of working out which engines are burning, which tanks are being drained 
@@ -495,26 +757,32 @@
         {
             // Update the active engines
             this.UpdateActiveEngines();
-            
+
             // Empty the draining resources set
             this.drainingResources.Clear();
 
             // Reset the resource drains of all draining parts
             foreach (PartSim partSim in this.drainingParts)
+            {
                 partSim.ResourceDrains.Reset();
+            }
 
             // Empty the draining parts set
             this.drainingParts.Clear();
 
             // Loop through all the active engine modules
-            foreach (EngineSim engine in this.activeEngines)
-            {
+            for (int i = 0; i < activeEngines.Count; ++i)
+            {
+                EngineSim engine = activeEngines[i];
+
                 // Set the resource drains for this engine
                 if (engine.SetResourceDrains(this.allParts, this.allFuelLines, this.drainingParts))
                 {
                     // If it is active then add the consumed resource types to the set
-                    foreach (int type in engine.ResourceConsumptions.Types)
-                        this.drainingResources.Add(type);
+                    for (int j = 0; j < engine.ResourceConsumptions.Types.Count; ++j)
+                    { 
+                        drainingResources.Add(engine.ResourceConsumptions.Types[j]);
+                    }
                 }
             }
 
@@ -527,7 +795,9 @@
                 buffer.AppendFormat("Active engines = {0:d}\n", this.activeEngines.Count);
                 int i = 0;
                 foreach (EngineSim engine in this.activeEngines)
+                {
                     engine.DumpEngineToBuffer(buffer, "Engine " + (i++) + ":");
+                }
                 MonoBehaviour.print(buffer);
             }
         }
@@ -543,43 +813,34 @@
                 buffer.AppendFormat("currentStage = {0:d}\n", this.currentStage);
             }
 
-            if (this.activeEngines.Count == 0)
-            {
-                if (SimManager.logOutput)
-                {
-                    buffer.AppendLine("No active engines => true");
-                    MonoBehaviour.print(buffer);
-                }
-
-                return true;
-            }
-
-            bool partDecoupled = false;
-            bool engineDecoupled = false;
-
-            foreach (PartSim partSim in this.allParts)
-            {
-                //partSim.DumpPartToBuffer(buffer, "Testing: ", allParts);
-                //buffer.AppendFormat("isSepratron = {0}\n", partSim.isSepratron ? "true" : "false");
-                if (partSim.decoupledInStage == (this.currentStage - 1) && (!partSim.isSepratron || partSim.decoupledInStage < partSim.inverseStage))
-                {
-                    partDecoupled = true;
-                    
-                    if (!partSim.Resources.EmptyOf(this.drainingResources))
+            if (this.activeEngines.Count > 0)
+            {
+                for (int i = 0; i < dontStageParts.Count; ++i)
+                {
+                    PartSim partSim = dontStageParts[i];
+
+                    if (SimManager.logOutput)
+                    {
+                        partSim.DumpPartToBuffer(buffer, "Testing: ");
+                    }
+                    //buffer.AppendFormat("isSepratron = {0}\n", partSim.isSepratron ? "true" : "false");
+
+                    if (!partSim.isSepratron && !partSim.EmptyOf(this.drainingResources))
                     {
                         if (SimManager.logOutput)
                         {
                             partSim.DumpPartToBuffer(buffer, "Decoupled part not empty => false: ");
                             MonoBehaviour.print(buffer);
                         }
-
                         return false;
                     }
 
                     if (partSim.isEngine)
                     {
-                        foreach (EngineSim engine in this.activeEngines)
-                        {
+                        for (int j = 0; j < activeEngines.Count; ++j)
+                        {
+                            EngineSim engine = activeEngines[j];
+
                             if (engine.partSim == partSim)
                             {
                                 if (SimManager.logOutput)
@@ -590,38 +851,26 @@
                                 return false;
                             }
                         }
-
-                        engineDecoupled = true;
-                    }
-                }
-            }
-
-            if (!partDecoupled)
+                    }
+                }
+            }
+
+            if (this.currentStage == 0 && this.doingCurrent)
             {
                 if (SimManager.logOutput)
                 {
-                    buffer.AppendLine("No engine decoupled but something is => false");
+                    buffer.AppendLine("Current stage == 0 && doingCurrent => false");
                     MonoBehaviour.print(buffer);
                 }
                 return false;
             }
 
-            if (this.currentStage > 0 && !this.doingCurrent)
-            {
-                if (SimManager.logOutput)
-                {
-                    buffer.AppendLine("Current stage > 0 && !doingCurrent => true");
-                    MonoBehaviour.print(buffer);
-                }
-                return true;
-            }
-
             if (SimManager.logOutput)
             {
-                buffer.AppendLine("Returning false");
+                buffer.AppendLine("Returning true");
                 MonoBehaviour.print(buffer);
             }
-            return false;
+            return true;
         }
 
         // This function activates the next stage
@@ -629,11 +878,15 @@
         private void ActivateStage()
         {
             // Build a set of all the parts that will be decoupled
-            HashSet<PartSim> decoupledParts = new HashSet<PartSim>();
-            foreach (PartSim partSim in this.allParts)
-            {
+            decoupledParts.Clear();
+            for (int i = 0; i < allParts.Count; ++i)
+            {
+                PartSim partSim = allParts[i];
+
                 if (partSim.decoupledInStage >= this.currentStage)
+                {
                     decoupledParts.Add(partSim);
+                }
             }
 
             foreach (PartSim partSim in decoupledParts)
@@ -646,56 +899,32 @@
                     for (int i = this.allEngines.Count - 1; i >= 0; i--)
                     {
                         if (this.allEngines[i].partSim == partSim)
+                        {
                             this.allEngines.RemoveAt(i);
+                        }
                     }
                 }
                 // If it is a fuel line then remove it from the list of all fuel lines
                 if (partSim.isFuelLine)
+                {
                     this.allFuelLines.Remove(partSim);
+                }
             }
 
             // Loop through all the (remaining) parts
-            foreach (PartSim partSim in this.allParts)
-            {
+            for (int i = 0; i < allParts.Count; ++i) { 
                 // Ask the part to remove all the parts that are decoupled
-                partSim.RemoveAttachedParts(decoupledParts);
+                allParts[i].RemoveAttachedParts(decoupledParts);
             }
 
             // Now we loop through all the engines and activate those that are ignited in this stage
-            foreach(EngineSim engine in this.allEngines)
-            {
+            for (int i = 0; i < allEngines.Count; ++i)
+            {
+                EngineSim engine = allEngines[i];
                 if (engine.partSim.inverseStage == this.currentStage)
+                {
                     engine.isActive = true;
-            }
-        }
-
-        private double ShipStartMass
-        {
-            get
-            {
-                double mass = 0d;
-
-                foreach (PartSim partSim in this.allParts)
-                {
-                    mass += partSim.GetStartMass();
-                }
-
-                return mass;
-            }
-        }
-
-        private double ShipMass
-        {
-            get
-            {
-                double mass = 0d;
-
-                foreach (PartSim partSim in this.allParts)
-                {
-                    mass += partSim.GetMass();
-                }
-
-                return mass;
+                }
             }
         }
 
@@ -709,10 +938,14 @@
             {
                 PartSim root = this.allParts[0];
                 while (root.parent != null)
+                {
                     root = root.parent;
+                }
 
                 if (root.hasVessel)
+                {
                     buffer.AppendFormat("vesselName = '{0}'  vesselType = {1}\n", this.vesselName, SimManager.GetVesselTypeString(this.vesselType));
+                }
 
                 root.DumpPartToBuffer(buffer, "", this.allParts);
             }
@@ -721,4 +954,3 @@
         }
     }
 }
-