latest updates from JSBSim

[flightgear.git] / src / FDM / YASim / Turbulence.cpp

diff --git a/src/FDM/YASim/Turbulence.cpp b/src/FDM/YASim/Turbulence.cpp
index 8862e5503a632356a7ea2eb09808e3dc0498e10e..4a618368098f69889b156339615fb65d79051e55 100644 (file)
--- a/src/FDM/YASim/Turbulence.cpp
+++ b/src/FDM/YASim/Turbulence.cpp
@@ -25,7 +25,7 @@ const double MAGNITUDE_EXP = 2.0;
 // bandwidth to the higher frequency components.  A turbulence field
 // will swing between maximal values over a distance of approximately
 // 2^(MEANINGFUL_GENS-1).
-const int MEANINGFUL_GENS = 8;
+const int MEANINGFUL_GENS = 7;
 
 static const float FT2M = 0.3048;
 
@@ -130,15 +130,16 @@ void Turbulence::offset(float* offset)
         _off[i] += offset[i];
 }
 
-void Turbulence::getTurbulence(double* loc, float* turbOut)
+void Turbulence::getTurbulence(double* loc, float alt, float* up,
+                               float* turbOut)
 {
     // Convert to integer 2D coordinates; wrap to [0:_sz].
     double a = (loc[0] + _off[0]) + (loc[2] + _off[2]);
     double b = (loc[1] + _off[1]) + _timeOff;
     a -= _sz * Math::floor(a * (1.0/_sz));
     b -= _sz * Math::floor(b * (1.0/_sz));
-    int x = (int)Math::floor(a);
-    int y = (int)Math::floor(b);
+    int x = ((int)Math::floor(a))&(_sz-1);
+    int y = ((int)Math::floor(b))&(_sz-1);
 
     // Convert to fractional interpolation factors
     a -= x;
@@ -158,6 +159,20 @@ void Turbulence::getTurbulence(double* loc, float* turbOut)
         float avg1 = (1-a)*turb10[i] + a*turb11[i];
         turbOut[i] = mag * ((1-b)*avg0 + b*avg1);
     }
+
+    // Adjust for altitude effects
+    if(alt < 300) {
+        float altmul = 0.5 + (1-0.5) * (alt*(1.0/300));
+        if(alt < 100) {
+            float vmul = alt * (1.0/100);
+            vmul = vmul / altmul; // pre-correct for the pending altmul
+            float dot = Math::dot3(turbOut, up);
+            float off[3];
+            Math::mul3(dot * (vmul-1), up, off);
+            Math::add3(turbOut, off, turbOut);
+        }
+        Math::mul3(altmul, turbOut, turbOut);
+    }
 }
 
 // Associates a random number in the range [-1:1] with a given lattice