Fixed issue with sepratrons on solid boosters
[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
@@ -30,28 +30,35 @@
 
 namespace KerbalEngineer.VesselSimulator
 {
+    using CompoundParts;
+    using Extensions;
+    using Helpers;
+
     public class Simulation
     {
-        private const double STD_GRAVITY = 9.81d;
-        private const double SECONDS_PER_DAY = 86400d;
+        private const double SECONDS_PER_DAY = 86400;
         private readonly Stopwatch _timer = new Stopwatch();
-        private List<EngineSim> activeEngines;
-        private List<EngineSim> allEngines;
-        private List<PartSim> allFuelLines;
-        private List<PartSim> allParts;
+        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;
-        private HashSet<PartSim> drainingParts;
-        private HashSet<int> drainingResources;
+        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;
+        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;
@@ -59,12 +66,15 @@
         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 velocity;
+        private double mach;
+        private float maxMach;
         public String vesselName;
         public VesselType vesselType;
+        private WeightedVectorAverager vectorAverager = new WeightedVectorAverager();
 
         public Simulation()
         {
@@ -74,33 +84,33 @@
             }
         }
 
-        private double ShipStartMass
+        private double ShipMass
         {
             get
             {
                 double mass = 0d;
 
-                foreach (PartSim partSim in this.allParts)
-                {
-                    mass += partSim.GetStartMass();
+                for (int i = 0; i < allParts.Count; ++i) { 
+                    mass += allParts[i].GetMass();
                 }
 
                 return mass;
             }
         }
 
-        private double ShipMass
+        private Vector3d ShipCom
         {
             get
             {
-                double mass = 0d;
-
-                foreach (PartSim partSim in this.allParts)
-                {
-                    mass += partSim.GetMass();
-                }
-
-                return mass;
+                vectorAverager.Reset();
+
+                for (int i = 0; i < allParts.Count; ++i)
+                {
+                    PartSim partSim = allParts[i];
+                    vectorAverager.Add(partSim.centerOfMass, partSim.GetMass());
+                }
+
+                return vectorAverager.Get();
             }
         }
 
@@ -108,7 +118,7 @@
         // 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)
@@ -123,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)
             {
@@ -146,8 +160,10 @@
 
             // 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))
                 {
@@ -159,7 +175,7 @@
                 }
 
                 // 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);
@@ -170,10 +186,15 @@
                 }
                 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();
@@ -188,12 +209,15 @@
             // 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)
                             {
@@ -221,8 +245,10 @@
             }
 
             //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)
                 {
@@ -232,9 +258,9 @@
 
             // 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
@@ -262,6 +288,7 @@
             {
                 MonoBehaviour.print("RunSimulation started");
             }
+
             this._timer.Start();
 
             LogMsg log = null;
@@ -278,8 +305,10 @@
             // currently active engines then generate an extra stage
             // Loop through all the engines
             bool anyActive = false;
-            foreach (EngineSim engine in this.allEngines)
-            {
+            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);
@@ -334,7 +363,7 @@
             }
 
             // Create a list of lists of PartSims that prevent decoupling
-            List<List<PartSim>> dontStagePartsLists = this.BuildDontStageLists(log);
+            BuildDontStageLists(log);
 
             if (log != null)
             {
@@ -343,6 +372,7 @@
 
             // Create the array of stages that will be returned
             Stage[] stages = new Stage[this.currentStage + 1];
+
 
             // Loop through the stages
             while (this.currentStage >= 0)
@@ -364,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;
 
@@ -380,10 +413,33 @@
                 stage.actualThrust = this.totalStageActualThrust;
                 stage.actualThrustToWeight = this.totalStageActualThrust / (this.stageStartMass * this.gravity);
 
+                // 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
-                foreach (PartSim partSim in this.allParts)
-                {
+                for (int i = 0; i < allParts.Count; ++i)
+                {
+                    PartSim partSim = allParts[i];
+
                     if (partSim.decoupledInStage == this.currentStage - 1)
                     {
                         stage.cost += partSim.cost;
@@ -461,7 +517,7 @@
                     // 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
@@ -484,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;
                     }
 
@@ -495,12 +555,13 @@
 
                 // 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
                 {
@@ -510,7 +571,8 @@
                 // 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.partCount = this.allParts.Count;
+                stage.totalPartCount = this.allParts.Count;
+                stage.maxMach = maxMach;
                 stages[this.currentStage] = stage;
 
                 // Now activate the next stage
@@ -548,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++)
@@ -572,20 +635,30 @@
             return stages;
         }
 
-        private List<List<PartSim>> BuildDontStageLists(LogMsg log)
+        private void BuildDontStageLists(LogMsg log)
         {
             if (log != null)
             {
                 log.buf.AppendLine("Creating list with capacity of " + (this.currentStage + 1));
             }
-            List<List<PartSim>> lists = new List<List<PartSim>>();
+
+            dontStagePartsLists.Clear();
             for (int i = 0; i <= this.currentStage; i++)
             {
-                lists.Add(new List<PartSim>());
-            }
-
-            foreach (PartSim partSim in this.allParts)
-            {
+                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)
@@ -602,28 +675,28 @@
                     }
                     else
                     {
-                        lists[partSim.decoupledInStage + 1].Add(partSim);
+                        dontStagePartsLists[partSim.decoupledInStage + 1].Add(partSim);
                     }
                 }
             }
 
             for (int i = 1; i <= this.lastStage; i++)
             {
-                if (lists[i].Count == 0)
-                {
-                    lists[i] = lists[i - 1];
-                }
-            }
-
-            return lists;
+                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);
@@ -641,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);
@@ -690,15 +771,17 @@
             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]);
                     }
                 }
             }
@@ -732,29 +815,32 @@
 
             if (this.activeEngines.Count > 0)
             {
-                foreach (PartSim partSim in this.dontStageParts)
-                {
+                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.Resources.EmptyOf(this.drainingResources))
+                    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)
@@ -792,9 +878,11 @@
         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);
@@ -824,15 +912,15 @@
             }
 
             // 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;