Merge pull request #9 from mic-e/thrustdirectionerror
Merge pull request #9 from mic-e/thrustdirectionerror

thrust torque + angle readout

--- a/KerbalEngineer/Editor/BuildAdvanced.cs
+++ b/KerbalEngineer/Editor/BuildAdvanced.cs
@@ -534,6 +534,23 @@
         }
 
         /// <summary>
+        ///     Draws the torque column.
+        /// </summary>
+        private void DrawTorque()
+        {
+            GUILayout.BeginVertical(GUILayout.Width(75.0f * GuiDisplaySize.Offset));
+            GUILayout.Label("TORQUE", this.titleStyle);
+            foreach (var stage in this.stages)
+            {
+                if (this.showAllStages || stage.deltaV > 0)
+                {
+                    GUILayout.Label(stage.maxThrustTorque.ToTorque(), this.infoStyle);
+                }
+            }
+            GUILayout.EndVertical();
+        }
+
+        /// <summary>
         ///     Drwas the thrust to weight ratio column.
         /// </summary>
         private void DrawTwr()
@@ -738,6 +755,7 @@
                     this.DrawMass();
                     this.DrawIsp();
                     this.DrawThrust();
+                    this.DrawTorque();
                     this.DrawTwr();
                     this.DrawDeltaV();
                     this.DrawBurnTime();

--- a/KerbalEngineer/Editor/BuildOverlayPartInfo.cs
+++ b/KerbalEngineer/Editor/BuildOverlayPartInfo.cs
@@ -317,9 +317,9 @@
 
             var reactionWheel = this.selectedPart.GetModule<ModuleReactionWheel>();
             this.infoItems.Add(new PartInfoItem("Reaction Wheel Torque"));
-            this.infoItems.Add(new PartInfoItem("\tPitch", reactionWheel.PitchTorque.ToForce()));
-            this.infoItems.Add(new PartInfoItem("\tRoll", reactionWheel.RollTorque.ToForce()));
-            this.infoItems.Add(new PartInfoItem("\tYaw", reactionWheel.YawTorque.ToForce()));
+            this.infoItems.Add(new PartInfoItem("\tPitch", reactionWheel.PitchTorque.ToTorque()));
+            this.infoItems.Add(new PartInfoItem("\tRoll", reactionWheel.RollTorque.ToTorque()));
+            this.infoItems.Add(new PartInfoItem("\tYaw", reactionWheel.YawTorque.ToTorque()));
             foreach (var resource in reactionWheel.inputResources)
             {
                 this.infoItems.Add(new PartInfoItem("\t" + resource.name, resource.rate.ToRate()));

--- a/KerbalEngineer/Extensions/DoubleExtensions.cs
+++ b/KerbalEngineer/Extensions/DoubleExtensions.cs
@@ -49,6 +49,11 @@
             return Units.ToDistance(value);
         }
 
+        public static string ToTorque(this double value)
+        {
+            return Units.ToTorque(value);
+        }
+
         public static string ToForce(this double value)
         {
             return Units.ToForce(value);

--- a/KerbalEngineer/Extensions/FloatExtensions.cs
+++ b/KerbalEngineer/Extensions/FloatExtensions.cs
@@ -49,6 +49,11 @@
             return Units.ToForce(value);
         }
 
+        public static string ToTorque(this float value)
+        {
+            return Units.ToTorque(value);
+        }
+
         public static string ToMass(this float value)
         {
             return Units.ToMass(value);

--- a/KerbalEngineer/Flight/Readouts/ReadoutLibrary.cs
+++ b/KerbalEngineer/Flight/Readouts/ReadoutLibrary.cs
@@ -132,6 +132,8 @@
                 readouts.Add(new Mass());
                 readouts.Add(new Thrust());
                 readouts.Add(new ThrustToWeight());
+                readouts.Add(new ThrustOffsetAngle());
+                readouts.Add(new ThrustTorque());
                 readouts.Add(new SurfaceThrustToWeight());
                 readouts.Add(new Acceleration());
                 readouts.Add(new SuicideBurnAltitude());

--- /dev/null
+++ b/KerbalEngineer/Flight/Readouts/Vessel/ThrustOffsetAngle.cs
@@ -1,1 +1,66 @@
+// 
+//     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 KerbalEngineer.Flight.Sections;
+using KerbalEngineer.Helpers;
+
+#endregion
+
+namespace KerbalEngineer.Flight.Readouts.Vessel
+{
+    public class ThrustOffsetAngle : ReadoutModule
+    {
+        #region Constructors
+
+        public ThrustOffsetAngle()
+        {
+            this.Name = "Thrust offset angle";
+            this.Category = ReadoutCategory.GetCategory("Vessel");
+            this.HelpString = "Thrust angle offset due to vessel asymmetries and gimballing";
+            this.IsDefault = true;
+        }
+
+        #endregion
+
+        #region Methods: public
+
+        public override void Draw(SectionModule section)
+        {
+            if (SimulationProcessor.ShowDetails)
+            {
+                this.DrawLine(Units.ToAngle(SimulationProcessor.LastStage.thrustOffsetAngle, 1), section.IsHud);
+            }
+        }
+
+        public override void Reset()
+        {
+            FlightEngineerCore.Instance.AddUpdatable(SimulationProcessor.Instance);
+        }
+
+        public override void Update()
+        {
+            SimulationProcessor.RequestUpdate();
+        }
+
+        #endregion
+    }
+}
+

--- /dev/null
+++ b/KerbalEngineer/Flight/Readouts/Vessel/ThrustTorque.cs
@@ -1,1 +1,66 @@
+// 
+//     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 KerbalEngineer.Flight.Sections;
+using KerbalEngineer.Helpers;
+
+#endregion
+
+namespace KerbalEngineer.Flight.Readouts.Vessel
+{
+    public class ThrustTorque : ReadoutModule
+    {
+        #region Constructors
+
+        public ThrustTorque()
+        {
+            this.Name = "Thrust torque";
+            this.Category = ReadoutCategory.GetCategory("Vessel");
+            this.HelpString = "Thrust torque due to vessel asymmetries and gimballing";
+            this.IsDefault = true;
+        }
+
+        #endregion
+
+        #region Methods: public
+
+        public override void Draw(SectionModule section)
+        {
+            if (SimulationProcessor.ShowDetails)
+            {
+                this.DrawLine(Units.ToTorque(SimulationProcessor.LastStage.maxThrustTorque), section.IsHud);
+            }
+        }
+
+        public override void Reset()
+        {
+            FlightEngineerCore.Instance.AddUpdatable(SimulationProcessor.Instance);
+        }
+
+        public override void Update()
+        {
+            SimulationProcessor.RequestUpdate();
+        }
+
+        #endregion
+    }
+}
+

--- /dev/null
+++ b/KerbalEngineer/Helpers/Averager.cs
@@ -1,1 +1,67 @@
+// 
+//     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/>.
+// 
 
+using System;
+
+namespace KerbalEngineer
+{ 
+    public class VectorAverager
+    {
+        private Vector3d sum = Vector3d.zero;
+        private uint count = 0;
+
+        public void Add(Vector3d v) {
+            sum += v;
+            count += 1;
+        }
+
+        public Vector3d Get() {
+            if (count > 0) {
+                return sum / count;
+            } else {
+                return Vector3d.zero;
+            }
+        }
+    }
+
+    public class WeightedVectorAverager
+    {
+        private Vector3d sum = Vector3d.zero;
+        private double totalweight = 0;
+
+        public void Add(Vector3d v, double weight) {
+            sum += v * weight;
+            totalweight += weight;
+        }
+
+        public Vector3d Get() {
+            if (totalweight > 0) {
+                return sum / totalweight;
+            } else {
+                return Vector3d.zero;
+            }
+        }
+
+        public double GetTotalWeight() {
+            return totalweight;
+        }
+    }
+}
+
+

--- /dev/null
+++ b/KerbalEngineer/Helpers/ForceAccumulator.cs
@@ -1,1 +1,103 @@
+// 
+//     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/>.
+// 
 
+using System;
+using System.Collections.Generic;
+
+namespace KerbalEngineer
+{
+    // a (force, application point) tuple
+    public class AppliedForce
+    {
+        public Vector3d vector;
+        public Vector3d applicationPoint;
+
+        public AppliedForce(Vector3d vector, Vector3d applicationPoint) {
+            this.vector = vector;
+            this.applicationPoint = applicationPoint;
+        }
+    }
+
+	// This class was mostly adapted from FARCenterQuery, part of FAR, by ferram4, GPLv3
+	// https://github.com/ferram4/Ferram-Aerospace-Research/blob/master/FerramAerospaceResearch/FARCenterQuery.cs
+    // Also see https://en.wikipedia.org/wiki/Resultant_force
+
+	// It accumulates forces and their points of applications, and provides methods for
+    // calculating the effective torque at any position, as well as the minimum-torque net force application point.
+    //
+    // The latter is a non-trivial issue; there is a 1-dimensional line of physically-equivalent solutions parallel
+    // to the resulting force vector; the solution closest to the weighted average of force positions is chosen.
+	// In the case of non-parallel forces, there usually is an infinite number of such lines, all of which have
+	// some amount of residual torque. The line with the least amount of residual torque is chosen.
+	public class ForceAccumulator
+	{
+		// Total force.
+		private Vector3d totalForce = Vector3d.zero;
+		// Torque needed to compensate if force were applied at origin.
+		private Vector3d totalZeroOriginTorque = Vector3d.zero;
+
+		// Weighted average of force application points.
+		private WeightedVectorAverager avgApplicationPoint = new WeightedVectorAverager();
+
+		// Feed an force to the accumulator.
+		public void AddForce(Vector3d applicationPoint, Vector3d force)
+		{
+			totalForce += force;
+			totalZeroOriginTorque += Vector3d.Cross(applicationPoint, force);
+			avgApplicationPoint.Add(applicationPoint, force.magnitude);
+		}
+
+        public Vector3d GetAverageForceApplicationPoint() {
+            return avgApplicationPoint.Get();
+        }
+
+        public void AddForce(AppliedForce force) {
+            AddForce(force.applicationPoint, force.vector);
+        }
+
+		// Residual torque for given force application point.
+		public Vector3d TorqueAt(Vector3d origin)
+		{
+			return totalZeroOriginTorque - Vector3d.Cross(origin, totalForce);
+		}
+
+        // Total force vector.
+        public Vector3d GetTotalForce()
+        {
+            return totalForce;
+        }
+
+        // Returns the minimum-residual-torque force application point that is closest to origin.
+        // Note that TorqueAt(GetMinTorquePos()) is always parallel to totalForce.
+        public Vector3d GetMinTorqueForceApplicationPoint(Vector3d origin)
+        {
+            double fmag = totalForce.sqrMagnitude;
+            if (fmag <= 0) {
+                return origin;
+            }
+
+            return origin + Vector3d.Cross(totalForce, TorqueAt(origin)) / fmag;
+        }
+
+        public Vector3d GetMinTorqueForceApplicationPoint()
+        {
+            return GetMinTorqueForceApplicationPoint(avgApplicationPoint.Get());
+        }
+	}
+}

--- a/KerbalEngineer/Helpers/Units.cs
+++ b/KerbalEngineer/Helpers/Units.cs
@@ -83,6 +83,11 @@
             return value.ToString("N" + decimals) + "Mm";
         }
 
+        public static string ToTorque(double value)
+        {
+            return value.ToString((value < 100.0) ? (Math.Abs(value) < Double.Epsilon) ? "N0" : "N1" : "N0") + "kNm";
+        }
+
         public static string ToForce(double value)
         {
             return value.ToString((value < 100000.0) ? (value < 10000.0) ? (value < 100.0) ? (Math.Abs(value) < Double.Epsilon) ? "N0" : "N3" : "N2" : "N1" : "N0") + "kN";

--- a/KerbalEngineer/VesselSimulator/EngineSim.cs
+++ b/KerbalEngineer/VesselSimulator/EngineSim.cs
@@ -38,6 +38,7 @@
         public bool isActive = false;
         public double isp = 0;
         public PartSim partSim;
+        public List<AppliedForce> appliedForces;
 
         public double thrust = 0;
 
@@ -58,7 +59,8 @@
                          bool throttleLocked,
                          List<Propellant> propellants,
                          bool active,
-                         bool correctThrust)
+                         bool correctThrust,
+                         List<Transform> thrustTransforms)
         {
             StringBuilder buffer = null;
             //MonoBehaviour.print("Create EngineSim for " + theEngine.name);
@@ -213,6 +215,14 @@
             {
                 MonoBehaviour.print(buffer);
             }
+
+            appliedForces = new List<AppliedForce>();
+            double thrustPerThrustTransform = thrust / thrustTransforms.Count;
+            foreach (Transform thrustTransform in thrustTransforms) {
+                Vector3d direction = thrustTransform.forward.normalized;
+                Vector3d position = thrustTransform.position;
+                appliedForces.Add(new AppliedForce(direction * thrustPerThrustTransform, position));
+            }
         }
 
         public ResourceContainer ResourceConsumptions

--- a/KerbalEngineer/VesselSimulator/PartSim.cs
+++ b/KerbalEngineer/VesselSimulator/PartSim.cs
@@ -35,6 +35,7 @@
     public class PartSim
     {
         private readonly List<AttachNodeSim> attachNodes = new List<AttachNodeSim>();
+        public Vector3d centerOfMass;
         public double baseMass = 0d;
         public double cost;
         public int decoupledInStage;
@@ -71,6 +72,7 @@
         public PartSim(Part thePart, int id, double atmosphere, LogMsg log)
         {
             this.part = thePart;
+            this.centerOfMass = thePart.transform.TransformPoint(thePart.CoMOffset);
             this.partId = id;
             this.name = this.part.partInfo.name;
 
@@ -206,7 +208,8 @@
                                                             engine.throttleLocked,
                                                             engine.propellants,
                                                             engine.isOperational,
-                                                            correctThrust);
+                                                            correctThrust,
+                                                            engine.thrustTransforms);
                         allEngines.Add(engineSim);
                     }
                 }
@@ -238,7 +241,8 @@
                                                             engine.throttleLocked,
                                                             engine.propellants,
                                                             engine.isOperational,
-                                                            correctThrust);
+                                                            correctThrust,
+                                                            engine.thrustTransforms);
                         allEngines.Add(engineSim);
                     }
                 }
@@ -268,7 +272,8 @@
                                                             engine.throttleLocked,
                                                             engine.propellants,
                                                             engine.isOperational,
-                                                            correctThrust);
+                                                            correctThrust,
+                                                            engine.thrustTransforms);
                         allEngines.Add(engineSim);
                     }
                 }

--- a/KerbalEngineer/VesselSimulator/Simulation.cs
+++ b/KerbalEngineer/VesselSimulator/Simulation.cs
@@ -52,6 +52,7 @@
         private List<Part> partList;
         private double simpleTotalThrust;
         private double stageStartMass;
+        private Vector3d stageStartCom;
         private double stageTime;
         private double stepEndMass;
         private double stepStartMass;
@@ -59,6 +60,7 @@
         private double totalStageFlowRate;
         private double totalStageIspFlowRate;
         private double totalStageThrust;
+        private ForceAccumulator totalStageThrustForce;
         private Vector3 vecActualThrust;
         private Vector3 vecStageDeltaV;
         private Vector3 vecThrust;
@@ -74,7 +76,7 @@
             }
         }
 
-        private double ShipStartMass
+        private double ShipMass
         {
             get
             {
@@ -82,25 +84,25 @@
 
                 foreach (PartSim partSim in this.allParts)
                 {
-                    mass += partSim.GetStartMass();
+                    mass += partSim.GetMass();
                 }
 
                 return mass;
             }
         }
 
-        private double ShipMass
+        private Vector3d ShipCom
         {
             get
             {
-                double mass = 0d;
+                WeightedVectorAverager averager = new WeightedVectorAverager();
 
                 foreach (PartSim partSim in this.allParts)
                 {
-                    mass += partSim.GetMass();
-                }
-
-                return mass;
+                    averager.Add(partSim.centerOfMass, partSim.GetMass());
+                }
+
+                return averager.Get();
             }
         }
 
@@ -262,6 +264,7 @@
             {
                 MonoBehaviour.print("RunSimulation started");
             }
+
             this._timer.Start();
 
             LogMsg log = null;
@@ -365,7 +368,10 @@
 
                 this.stageTime = 0d;
                 this.vecStageDeltaV = Vector3.zero;
+
                 this.stageStartMass = this.ShipMass;
+                this.stageStartCom = this.ShipCom;
+
                 this.stepStartMass = this.stageStartMass;
                 this.stepEndMass = 0;
 
@@ -381,6 +387,27 @@
                 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)
@@ -644,6 +671,7 @@
             this.totalStageActualThrust = 0d;
             this.totalStageFlowRate = 0d;
             this.totalStageIspFlowRate = 0d;
+            this.totalStageThrustForce = new ForceAccumulator();
 
             // Loop through all the active engines totalling the thrust, actual thrust and mass flow rates
             // The thrust is totalled as vectors
@@ -655,6 +683,10 @@
 
                 this.totalStageFlowRate += engine.ResourceConsumptions.Mass;
                 this.totalStageIspFlowRate += engine.ResourceConsumptions.Mass * engine.isp;
+
+                foreach (AppliedForce f in engine.appliedForces) {
+                    this.totalStageThrustForce.AddForce(f);
+                }
             }
 
             //MonoBehaviour.print("vecThrust = " + vecThrust.ToString() + "   magnitude = " + vecThrust.magnitude);

--- a/KerbalEngineer/VesselSimulator/Stage.cs
+++ b/KerbalEngineer/VesselSimulator/Stage.cs
@@ -48,6 +48,8 @@
         public int totalPartCount = 0;
         public int partCount = 0;
         public double resourceMass = 0.0;
+        public double maxThrustTorque = 0f;
+        public double thrustOffsetAngle = 0f;
 
         public void Dump()
         {
@@ -65,6 +67,8 @@
             str.AppendFormat("thrustToWeight: {0:g6}\n", this.thrustToWeight);
             str.AppendFormat("maxTWR        : {0:g6}\n", this.maxThrustToWeight);
             str.AppendFormat("actualTWR     : {0:g6}\n", this.actualThrustToWeight);
+            str.AppendFormat("ThrustTorque  : {0:g6}\n", this.maxThrustTorque);
+            str.AppendFormat("ThrustOffset  : {0:g6}\n", this.thrustOffsetAngle);
             str.AppendFormat("deltaV        : {0:g6}\n", this.deltaV);
             str.AppendFormat("totalDeltaV   : {0:g6}\n", this.totalDeltaV);
             str.AppendFormat("invTotDeltaV  : {0:g6}\n", this.inverseTotalDeltaV);