From af12bf6da8ded493282a41a2e5d3b1912035a767 Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Fri, 26 Apr 2024 12:05:12 +0200
Subject: [PATCH] [refactoring-ode] Optimisation

---
 classes/components/Environment.m       |  20 +++--
 functions/miscellaneous/interpCoeffs.m | 117 ++++++++++++-------------
 functions/miscellaneous/interpLinear.m |  24 ++++-
 functions/odeFunctions/ballistic.m     |   6 +-
 4 files changed, 92 insertions(+), 75 deletions(-)

diff --git a/classes/components/Environment.m b/classes/components/Environment.m
index 2f76a64..4e36921 100644
--- a/classes/components/Environment.m
+++ b/classes/components/Environment.m
@@ -25,7 +25,7 @@ classdef Environment < Component
         rho             double       % [Kg/m^3] Gorund air density
     end
 
-    properties(Dependent)
+    properties(SetAccess = private)
         g0                      double       % [-] Gravity costant at launch latitude and altitude
         pinDistance             double       % [m] Distance of the upper pin from the rail base (upper pin-boat + boat-rail base)
         effectiveRampLength     double       % [m] Total launchpad length
@@ -51,21 +51,25 @@ classdef Environment < Component
             %% Update angles
             obj.omega = deg2rad(obj.omega);
             obj.phi = deg2rad(obj.phi);
+
+            obj.updateG0;
+            obj.updatePinDistance;
+            obj.updateEffectiveRampLength;
         end
     end
 
-    methods
-        function g0 = get.g0(obj)
-            g0 = gravitywgs84(obj.z0, obj.lat0);
+    methods % Updaters
+        function obj = updateG0(obj)
+            obj.g0 = gravitywgs84(obj.z0, obj.lat0);
         end
 
-        function pinDistance = get.pinDistance(obj)
-            pinDistance = obj.pin1Length + obj.pin2Length ... 
+        function obj = updatePinDistance(obj)
+            obj.pinDistance = obj.pin1Length + obj.pin2Length ... 
                 + obj.motor.tankLength;
         end
 
-        function effectiveRampLength = get.effectiveRampLength(obj)
-            effectiveRampLength = obj.rampLength - obj.pinDistance;
+        function obj = updateEffectiveRampLength(obj)
+            obj.effectiveRampLength = obj.rampLength - obj.pinDistance;
         end
     end
 end
\ No newline at end of file
diff --git a/functions/miscellaneous/interpCoeffs.m b/functions/miscellaneous/interpCoeffs.m
index 583a40c..b70a28e 100644
--- a/functions/miscellaneous/interpCoeffs.m
+++ b/functions/miscellaneous/interpCoeffs.m
@@ -1,69 +1,60 @@
-function [coeffsValues, angle0] = interpCoeffs(t, alpha, M, beta, alt, c, settings)
-%{
-interpCoeffs - interpolation of aerodynamic coefficients.
-
-INPUTS:
-        - t, double [1,1], integration time, [s];
-        - alpha, double[1,1], angle of attack, [];
-        - M, double[1,1], mach number, [];
-        - beta, double[1,1], sideslip angle, [];
-        - alt, double[1,1], altitude, [m];
-        - c, double[1,1], aerobrakes control variable, [];
-        - alphaTot, double[1,1], total angle of attack, [];
-        - settings, struct, rocket and simulation data.
-
-OUTPUTS:
-        - coeffsValues, array [16,1],  aerodynamic coefficients;
-        - angle0, array [2,1], array of the reference aerodynamic angles.
-
-CALLED FUNCTIONS: -
-
-VERSIONS:
-        -0   02/02/2018,   release,    Adriano Filippo Inno
-        -1   12/01/2020,   update,     Giulio Pacifici
-                        all coefficients inteprolation
-        -2   06/11/2021,   update,     Adriano Filippo Inno
-                        faster vectorization
-Copyright © 2021, Skyward Experimental Rocketry, AFD department
-All rights reserved
-
-SPDX-License-Identifier: GPL-3.0-or-later
-
-%}
-
-[nearAlpha, indexAlpha] = nearestValSorted(settings.State.Alphas*pi/180, alpha);
-[~, indexMach] = nearestValSorted(settings.State.Machs, M);
-[nearBeta, indexBeta] = nearestValSorted(settings.State.Betas*pi/180, beta);
-[~, indexAlt] = nearestValSorted(settings.State.Altitudes, alt);
-%[~, indexControl] = nearestValSorted(settings.State.hprot, alt);
-
-if t > settings.timeEngineCut && t < settings.tb
-    t = settings.timeEngineCut;
-end
-
-if t >= settings.tb
-    coeffsValues = settings.Coeffs(:, indexAlpha, indexMach, indexBeta, indexAlt, c, end);
-    % num = 1:1:length(settings.State.hprot)-1;
-    % indexXcg = t >= settings.State.xcgTime(1:end-1) & t < settings.State.xcgTime(2:end);
-    % index1 = num(indexXcg);
-    % index2 = index1 + 1;
-    % 
-    % VF = settings.Coeffs(:, indexAlpha, indexMach, indexBeta, indexAlt, c, index1);
-    % VE = settings.Coeffs(:, indexAlpha, indexMach, indexBeta, indexAlt, c, index2);
-    % 
-    % deltaT = settings.State.xcgTime(index2) - settings.State.xcgTime(index1);
-    % coeffsValues =  ( (t - settings.State.xcgTime(index1))/deltaT)*(VE - VF) + VF;
+function [coeffsValues, angle0] = interpCoeffs(coefficients, t, cutoffTime, alpha, mach, beta, alt, c)
+% interpCoeffs - interpolation of aerodynamic coefficients.
+%
+% INPUTS:
+%         - t, double [1,1], integration time, [s];
+%         - alpha, double[1,1], angle of attack, [];
+%         - mach, double[1,1], mach number, [];
+%         - beta, double[1,1], sideslip angle, [];
+%         - alt, double[1,1], altitude, [m];
+%         - c, double[1,1], aerobrakes control variable, [];
+%
+% OUTPUTS:
+%         - coeffsValues, array [16,1],  aerodynamic coefficients;
+%         - angle0, array [2,1], array of the reference aerodynamic angles.
+
+h = c*coefficients.state.hprot(end);
+
+[nearAlpha, indexAlpha] = nearestValSorted(coefficients.state.alphas*pi/180, alpha);
+[~, indexMach] = nearestValSorted(coefficients.state.machs, mach);
+[nearBeta, indexBeta] = nearestValSorted(coefficients.state.betas*pi/180, beta);
+[~, indexAlt] = nearestValSorted(coefficients.state.altitudes, alt);
+[~, indexControl] = nearestValSorted(coefficients.state.hprot, h);
+
+angle0 = [nearAlpha; nearBeta];
+
+if t >= cutoffTime
+    % Interpolate on airbrake control
+    % Uses end xCg, even if tCutOff < tBurnTime
+    num = length(coefficients.state.hprot);
+    index1 = indexControl;
+    index2 = indexControl + 1;
+    if index2 > num
+        if num == 1
+            coeffsValues = coefficients(:, indexAlpha, indexMach, indexBeta, indexAlt, 1, end);
+            return;
+        else
+            index2 = index2 - 1;
+            index1 = index1 - 1;
+        end
+    end
+
+    VF = coefficients.total(:, indexAlpha, indexMach, indexBeta, indexAlt, index1, end);
+    VE = coefficients.total(:, indexAlpha, indexMach, indexBeta, indexAlt, index2, end);
+
+    deltaH = coefficients.state.hprot(index2) - coefficients.state.hprot(index1);
+    coeffsValues =  ( (h - coefficients.state.hprot(index1))/deltaH)*(VE - VF) + VF;
 else
-    num = 1:1:length(settings.State.xcgTime)-1;
-    indexXcg = t >= settings.State.xcgTime(1:end-1) & t < settings.State.xcgTime(2:end);
+    % Interpolate on xCg, close airbrakes
+    num = 1:1:length(coefficients.state.xcgTime)-1;
+    indexXcg = t >= coefficients.state.xcgTime(1:end-1) & t < coefficients.state.xcgTime(2:end);
     index1 = num(indexXcg);
     index2 = index1 + 1;
 
-    VF = settings.Coeffs(:, indexAlpha, indexMach, indexBeta, indexAlt, c, index1);
-    VE = settings.Coeffs(:, indexAlpha, indexMach, indexBeta, indexAlt, c, index2);
+    VF = coefficients.total(:, indexAlpha, indexMach, indexBeta, indexAlt, indexControl, index1);
+    VE = coefficients.total(:, indexAlpha, indexMach, indexBeta, indexAlt, indexControl, index2);
 
-    deltaT = settings.State.xcgTime(index2) - settings.State.xcgTime(index1);
-    coeffsValues =  ( (t - settings.State.xcgTime(index1))/deltaT)*(VE - VF) + VF;
+    deltaT = coefficients.state.xcgTime(index2) - coefficients.state.xcgTime(index1);
+    coeffsValues =  ( (t - coefficients.state.xcgTime(index1))/deltaT)*(VE - VF) + VF;
 end
-
-angle0 = [nearAlpha; nearBeta];
\ No newline at end of file
+end
\ No newline at end of file
diff --git a/functions/miscellaneous/interpLinear.m b/functions/miscellaneous/interpLinear.m
index f3328f1..27667a5 100644
--- a/functions/miscellaneous/interpLinear.m
+++ b/functions/miscellaneous/interpLinear.m
@@ -1,4 +1,10 @@
-function Vq = interpLinear(X, V, Xq)
+function Vq = interpLinear(X, V, Xq, allowExtrapolation)
+arguments
+    X                   double
+    V                   double
+    Xq                  double
+    allowExtrapolation  logical = false
+end
 %{
 This function interpolates linearly.
 It works only with a scalar as point to interpolate (third input),
@@ -24,10 +30,24 @@ All rights reserved
 SPDX-License-Identifier: GPL-3.0-or-later
 
 %}
+
 Lx = size(X, 2);
 
 if Lx ~= size(V, 2), error ('Length of X and V should be the same'); end
-if Xq > X(end) || Xq < X(1), error('It is an extrapolation, requesteed value is out of bound'); end
+
+if ~allowExtrapolation
+    if Xq > X(end) || Xq < X(1)
+        error('It is an extrapolation, requesteed value is out of bound'); 
+    end
+else
+    if Xq >= X(end)
+        Vq = V(end);
+        return;
+    elseif Xq <= X(1)
+        Vq = V(1);
+        return;
+    end
+end
 
 if Xq == X(1), Vq = V(:, 1); return; end
 
diff --git a/functions/odeFunctions/ballistic.m b/functions/odeFunctions/ballistic.m
index 5870b6a..8e6c881 100644
--- a/functions/odeFunctions/ballistic.m
+++ b/functions/odeFunctions/ballistic.m
@@ -4,7 +4,7 @@ arguments
     Y
     rocket      Rocket
     environment Environment
-    wind        WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
+    wind        % WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
     newSettings Settings
 end
 %{
@@ -167,7 +167,9 @@ else
                             [alpha, mach, beta, absoluteAltitude]);
         angle0 = [alpha beta]; 
     else
-        [coeffsValues, angle0] = rocket.interpCoeffs(t, alpha, mach, beta, absoluteAltitude, opening);
+        %[coeffsValues, angle0] = rocket.interpCoeffs(t, alpha, mach, beta, absoluteAltitude, opening);
+        [coeffsValues, angle0] = interpCoeffs(rocket.coefficients, t, rocket.motor.cutoffTime, ...
+            alpha, mach, beta, absoluteAltitude, opening);
     end
 end
 
-- 
GitLab