Fix aerodynamic torque simulations

FerramAerospaceResearch/FARAPI.cs

@@ -382,6 +382,25 @@ public static bool VesselSpoilerSetting(Vessel v)
             return false;
         }
 
+        /// <summary>
+        /// Returns the current aerodynamic force being experienced by the vehicle in world space
+        /// </summary>
+        /// <param name="v">The vessel that force is being queried</param>
+        /// <returns>The force on the vessel in world space</returns>
+        public static Vector3 VesselAerodynamicForce(Vessel v)
+        {
+            return VesselFlightInfo(v)?.InfoParameters.aerodynamicForce ?? Vector3.zero;
+        }
+
+        /// <summary>
+        /// Returns the current aerodynamic torque being experienced by the vehicle in world space
+        /// </summary>
+        /// <param name="v">The vessel that force is being queried</param>
+        /// <returns>The torque on the vessel in world space</returns>
+        public static Vector3 VesselAerodynamicTorque(Vessel v)
+        {
+            return VesselFlightInfo(v)?.InfoParameters.aerodynamicTorque ?? Vector3.zero;
+        }
         #endregion
 
         #region AeroPredictions

FerramAerospaceResearch/FARAeroComponents/FARAeroSection.cs

@@ -49,6 +49,7 @@ You should have received a copy of the GNU General Public License
 using UnityEngine;
 using FerramAerospaceResearch.FARPartGeometry;
 using FerramAerospaceResearch.FARUtils;
+using ferram4;
 
 namespace FerramAerospaceResearch.FARAeroComponents
 {
@@ -285,182 +286,103 @@ public void ClearAeroSection()
             handledAeroModulesIndexDict.Clear();
         }
 
-        public void PredictionCalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag, Vector3 vel, ferram4.FARCenterQuery center)
+        #region Force application contexts
+        private interface IForceContext
         {
-            if (partData.Count == 0)
-                return;
+            /// <summary>
+            /// The part-relative velocity of the part whose force is being computed
+            /// </summary>
+            /// <param name="pd">The part data for which to compute the local velocity</param>
+            Vector3 LocalVelocity(PartData pd);
+            /// <summary>
+            /// Apply a calculated force to a part.
+            /// </summary>
+            /// <param name="pd">The part data of the part that the force should be applied to</param>
+            /// <param name="forceVector">The calculated force vector to be applied to the part</param>
+            /// <param name="torqueVector">The calculated torque vector to be applied to the part</param>
+            void ApplyForce(PartData pd, Vector3 forceVector, Vector3 torqueVector);
+        }
 
-            PartData data = partData[0];
-            FARAeroPartModule aeroModule = null;
-            for (int i = 0; i < partData.Count; i++)
+        private class SimulatedForceContext : IForceContext
+        {
+            /// <summary>
+            /// The world-space velocity of the part whose force is being simulated
+            /// </summary>
+            private Vector3 worldVel;
+
+            /// <summary>
+            /// The center with which force should be accumulated
+            /// </summary>
+            private ferram4.FARCenterQuery center;
+
+            /// <summary>
+            /// The atmospheric density that the force is being simulated at
+            /// </summary>
+            private float atmDensity;
+
+            public SimulatedForceContext(Vector3 worldVel, FARCenterQuery center, float atmDensity)
             {
-                data = partData[i];
-                aeroModule = data.aeroModule;
-                if (aeroModule.part == null || aeroModule.part.partTransform == null)
-                {
-                    continue;
-                }
-                break;
+                this.worldVel = worldVel;
+                this.center = center;
+                this.atmDensity = atmDensity;
             }
-            if (aeroModule == null || aeroModule.part == null || aeroModule.part.transform == null)
+
+            public void UpdateSimulationContext(Vector4 worldVel, FARCenterQuery center, float atmDensity)
             {
-                return;
+                this.worldVel = worldVel;
+                this.center = center;
+                this.atmDensity = atmDensity;
             }
-            double skinFrictionForce = skinFrictionDrag * xForceSkinFriction.Evaluate(machNumber);      //this will be the same for each part, so why recalc it multiple times?
-            double xForceAoA0 = xForcePressureAoA0.Evaluate(machNumber);
-            double xForceAoA180 = xForcePressureAoA180.Evaluate(machNumber);
-
-            Vector3 xRefVector = data.xRefVectorPartSpace;
-            Vector3 nRefVector = data.nRefVectorPartSpace;
-
-            Vector3 velLocal = aeroModule.part.partTransform.worldToLocalMatrix.MultiplyVector(vel);
-            Vector3 angVelLocal = aeroModule.partLocalAngVel;
-
-            //Vector3 angVelLocal = aeroModule.partLocalAngVel;
-
-            //velLocal += Vector3.Cross(angVelLocal, data.centroidPartSpace);       //some transform issue here, needs investigation
-            Vector3 velLocalNorm = velLocal.normalized;
-
-            Vector3 localNormalForceVec = Vector3.ProjectOnPlane(-velLocalNorm, xRefVector).normalized;
 
-            double cosAoA = Vector3.Dot(xRefVector, velLocalNorm);
-            double cosSqrAoA = cosAoA * cosAoA;
-            double sinSqrAoA = Math.Max(1 - cosSqrAoA, 0);
-            double sinAoA = Math.Sqrt(sinSqrAoA);
-            double sin2AoA = 2 * sinAoA * Math.Abs(cosAoA);
-            double cosHalfAoA = Math.Sqrt(0.5 + 0.5 * Math.Abs(cosAoA));
-
-
-            double nForce = 0;
-            nForce = potentialFlowNormalForce * Math.Sign(cosAoA) * cosHalfAoA * sin2AoA;  //potential flow normal force
-            if (nForce < 0)     //potential flow is not significant over the rear face of things
-                nForce = 0;
-            //if (machNumber > 3)
-            //    nForce *= 2d - machNumber * 0.3333333333333333d;
-
-            float normalForceFactor = Math.Abs(Vector3.Dot(localNormalForceVec, nRefVector));
-            normalForceFactor *= normalForceFactor;
-
-            normalForceFactor = invFlatnessRatio * (1 - normalForceFactor) + flatnessRatio * normalForceFactor;     //accounts for changes in relative flatness of shape
-
-
-            float crossFlowMach, crossFlowReynolds;
-            crossFlowMach = machNumber * (float)sinAoA;
-            crossFlowReynolds = reynoldsPerUnitLength * diameter * (float)sinAoA / normalForceFactor;
-
-            nForce += viscCrossflowDrag * sinSqrAoA * CalculateCrossFlowDrag(crossFlowMach, crossFlowReynolds);            //viscous crossflow normal force
-
-            nForce *= normalForceFactor;
-
-            double xForce = -skinFrictionForce * Math.Sign(cosAoA) * cosSqrAoA;
-            double localVelForce = xForce * pseudoKnudsenNumber;
-            xForce -= localVelForce;
-
-            localVelForce = Math.Abs(localVelForce);
-
-            float moment = (float)(cosAoA * sinAoA);
-            float dampingMoment = 4f * moment;
-
-
-            if (cosAoA > 0)
+            public Vector3 LocalVelocity(PartData pd)
             {
-                xForce += cosSqrAoA * xForceAoA0;
-                float momentFactor;
-                if (machNumber > 6)
-                    momentFactor = hypersonicMomentForward;
-                else if (machNumber < 0.6)
-                    momentFactor = 0.6f * hypersonicMomentBackward;
-                else
-                {
-                    float tmp = (-0.185185185f * machNumber + 1.11111111111f);
-                    momentFactor = tmp * hypersonicMomentBackward * 0.6f + (1 - tmp) * hypersonicMomentForward;
-                }
-                //if (machNumber < 1.5)
-                //    momentFactor += hypersonicMomentBackward * (0.5f - machNumber * 0.33333333333333333333333333333333f) * 0.2f;
-
-                moment *= momentFactor;
-                dampingMoment *= momentFactor;
+                return pd.aeroModule.part.partTransform.InverseTransformVector(worldVel);
             }
-            else
+
+            public void ApplyForce(PartData pd, Vector3 forceVector, Vector3 torqueVector)
             {
-                xForce += cosSqrAoA * xForceAoA180;
-                float momentFactor;     //negative to deal with the ref vector facing the opposite direction, causing the moment vector to point in the opposite direction
-                if (machNumber > 6)
-                    momentFactor = hypersonicMomentBackward;
-                else if (machNumber < 0.6)
-                    momentFactor = 0.6f * hypersonicMomentForward;
-                else
+                var localVel = pd.aeroModule.part.partTransform.InverseTransformVector(worldVel);
+                var tmp = 0.0005 * Vector3.SqrMagnitude(localVel);
+                var dynamicPressurekPa = tmp * atmDensity;
+                var dragFactor = dynamicPressurekPa * Mathf.Max(PhysicsGlobals.DragCurvePseudoReynolds.Evaluate(atmDensity * Vector3.Magnitude(localVel)), 1.0f);
+                var liftFactor = dynamicPressurekPa;
+
+                var localVelNorm = Vector3.Normalize(localVel);
+                Vector3 localForceTemp = Vector3.Dot(localVelNorm, forceVector) * localVelNorm;
+                var partLocalForce = (localForceTemp * (float)dragFactor + (forceVector - localForceTemp) * (float)liftFactor);
+                forceVector = pd.aeroModule.part.transform.TransformDirection(partLocalForce);
+                torqueVector = pd.aeroModule.part.transform.TransformDirection(torqueVector * (float)dynamicPressurekPa);
+                if (!float.IsNaN(forceVector.x) && !float.IsNaN(torqueVector.x))
                 {
-                    float tmp = (-0.185185185f * machNumber + 1.11111111111f);
-                    momentFactor = tmp * hypersonicMomentForward * 0.6f + (1 - tmp) * hypersonicMomentBackward;
+                    Vector3 centroid = pd.aeroModule.part.transform.TransformPoint(pd.centroidPartSpace - pd.aeroModule.part.CoMOffset);
+                    center.AddForce(centroid, forceVector);
+                    center.AddTorque(torqueVector);
                 }
-                //if (machNumber < 1.5)
-                //    momentFactor += hypersonicMomentForward * (0.5f - machNumber * 0.33333333333333333333333333333333f) * 0.2f;
-
-                moment *= momentFactor;
-                dampingMoment *= momentFactor;
             }
-            moment /= normalForceFactor;
-            dampingMoment = Math.Abs(dampingMoment) * 0.1f;
-            //dampingMoment += (float)Math.Abs(skinFrictionForce) * 0.1f;
-            float rollDampingMoment = (float)(skinFrictionForce * 0.5 * diameter);      //skin friction force times avg moment arm for vehicle
-            rollDampingMoment *= (0.75f + flatnessRatio * 0.25f);     //this is just an approximation for now
-
-            Vector3 forceVector = (float)xForce * xRefVector + (float)nForce * localNormalForceVec;
-            forceVector -= (float)localVelForce * velLocalNorm;
-
-            Vector3 torqueVector = Vector3.Cross(xRefVector, localNormalForceVec) * moment;
-
-            Vector3 axialAngLocalVel = Vector3.Dot(xRefVector, angVelLocal) * xRefVector;
-            Vector3 nonAxialAngLocalVel = angVelLocal - axialAngLocalVel;
-
-            if (velLocal.sqrMagnitude > 0.001f)
-                torqueVector -= (dampingMoment * nonAxialAngLocalVel) + (rollDampingMoment * axialAngLocalVel * axialAngLocalVel.magnitude) / velLocal.sqrMagnitude;
-            else
-                torqueVector -= (dampingMoment * nonAxialAngLocalVel) + (rollDampingMoment * axialAngLocalVel * axialAngLocalVel.magnitude) / 0.001f;
-
-            Matrix4x4 localToWorld = aeroModule.part.partTransform.localToWorldMatrix;
-
-            float dynPresAndScaling = 0.0005f * atmDensity * velLocal.sqrMagnitude;        //dyn pres and N -> kN conversion
-
-            forceVector *= dynPresAndScaling;
-            torqueVector *= dynPresAndScaling;
-
-            forceVector = localToWorld.MultiplyVector(forceVector);
-            torqueVector = localToWorld.MultiplyVector(torqueVector);
-            Vector3 centroid = Vector3.zero;
+        }
 
-            if (!float.IsNaN(forceVector.x) && !float.IsNaN(torqueVector.x))
+        private class FlightForceContext : IForceContext
+        {
+            public Vector3 LocalVelocity(PartData pd)
             {
-                for (int i = 0; i < partData.Count; i++)
-                {
-                    PartData data2 = partData[i];
-                    FARAeroPartModule module = data2.aeroModule;
-                    if ((object)module == null)
-                        continue;
-
-                    if (module.part == null || module.part.partTransform == null)
-                    {
-                        continue;
-                    }
+                return pd.aeroModule.partLocalVel;
+            }
 
-                    centroid = module.part.partTransform.localToWorldMatrix.MultiplyPoint3x4(data2.centroidPartSpace);
-                    center.AddForce(centroid, forceVector * data2.dragFactor);
-                }
-                center.AddTorque(torqueVector);
+            public void ApplyForce(PartData pd, Vector3 forceVector, Vector3 torqueVector)
+            {
+                pd.aeroModule.AddLocalForceAndTorque(forceVector, torqueVector, pd.centroidPartSpace);
             }
-            else
-                FARLogger.Error("NaN Prediction Section Error: Inputs: AtmDen: " + atmDensity + " Mach: " + machNumber + " Re: " + reynoldsPerUnitLength + " Kn: " + pseudoKnudsenNumber + " skin: " + skinFrictionDrag + " vel: " + vel);
         }
+        #endregion
 
-        public void FlightCalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag)
+        private void CalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag, 
+            IForceContext forceContext)
         {
-
             double skinFrictionForce = skinFrictionDrag * xForceSkinFriction.Evaluate(machNumber);      //this will be the same for each part, so why recalc it multiple times?
             double xForceAoA0 = xForcePressureAoA0.Evaluate(machNumber);
             double xForceAoA180 = xForcePressureAoA180.Evaluate(machNumber);
 
-            for(int i = 0; i < partData.Count; i++)
+            for (int i = 0; i < partData.Count; i++)
             {
                 PartData data = partData[i];
                 FARAeroPartModule aeroModule = data.aeroModule;
@@ -472,7 +394,7 @@ public void FlightCalculateAeroForces(float atmDensity, float machNumber, float
                 Vector3 xRefVector = data.xRefVectorPartSpace;
                 Vector3 nRefVector = data.nRefVectorPartSpace;
 
-                Vector3 velLocal = aeroModule.partLocalVel;
+                Vector3 velLocal = forceContext.LocalVelocity(data);
 
                 Vector3 angVelLocal = aeroModule.partLocalAngVel;
 
@@ -490,7 +412,7 @@ public void FlightCalculateAeroForces(float atmDensity, float machNumber, float
 
 
                 double nForce = 0;
-                    nForce = potentialFlowNormalForce * Math.Sign(cosAoA) * cosHalfAoA * sin2AoA;  //potential flow normal force
+                nForce = potentialFlowNormalForce * Math.Sign(cosAoA) * cosHalfAoA * sin2AoA;  //potential flow normal force
                 if (nForce < 0)     //potential flow is not significant over the rear face of things
                     nForce = 0;
 
@@ -585,9 +507,28 @@ public void FlightCalculateAeroForces(float atmDensity, float machNumber, float
                 forceVector *= data.dragFactor;
                 torqueVector *= data.dragFactor;
 
-                aeroModule.AddLocalForceAndTorque(forceVector, torqueVector, data.centroidPartSpace);
+                forceContext.ApplyForce(data, forceVector, torqueVector);
             }
         }
+
+        private SimulatedForceContext simContext;
+        public void PredictionCalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag, Vector3 vel, ferram4.FARCenterQuery center)
+        {
+            if (simContext == null)
+                simContext = new SimulatedForceContext(vel, center, atmDensity);
+            else
+                simContext.UpdateSimulationContext(vel, center, atmDensity);
+            CalculateAeroForces(atmDensity, machNumber, reynoldsPerUnitLength, pseudoKnudsenNumber, skinFrictionDrag, simContext);
+        }
+
+        private FlightForceContext flightContext;
+        public void FlightCalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag)
+        {
+            if (flightContext == null)
+                flightContext = new FlightForceContext();
+            CalculateAeroForces(atmDensity, machNumber, reynoldsPerUnitLength, pseudoKnudsenNumber, skinFrictionDrag, flightContext);
+
+        }
 
         public static void GenerateCrossFlowDragCurve()
         {

FerramAerospaceResearch/FARAeroComponents/FARVesselAero.cs

@@ -310,6 +310,7 @@ private void CalculateAndApplyVesselAeroProperties()
         public void SimulateAeroProperties(out Vector3 aeroForce, out Vector3 aeroTorque, Vector3 velocityWorldVector, double altitude)
         {
             FARCenterQuery center = new FARCenterQuery();
+            FARCenterQuery dummy = new FARCenterQuery();
 
             float pressure;
             float density;
@@ -337,7 +338,7 @@ public void SimulateAeroProperties(out Vector3 aeroForce, out Vector3 aeroTorque
             float skinFriction = (float)FARAeroUtil.SkinFrictionDrag(reynoldsNumber, machNumber);
 
             float pseudoKnudsenNumber = machNumber / (reynoldsNumber + machNumber);
-
+            
             if (_currentAeroSections != null)
             {
                 for (int i = 0; i < _currentAeroSections.Count; i++)
@@ -351,7 +352,7 @@ public void SimulateAeroProperties(out Vector3 aeroForce, out Vector3 aeroTorque
                 {
                     FARWingAerodynamicModel curWing = _legacyWingModels[i];
                     if ((object)curWing != null)
-                        curWing.PrecomputeCenterOfLift(velocityWorldVector, machNumber, density, center);
+                        center.AddForce(curWing.transform.position, curWing.PrecomputeCenterOfLift(velocityWorldVector, machNumber, density, dummy));
                 }
             }