Port over remaining Point3D usage to the more type and unit safe SG* classes.

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

diff --git a/src/FDM/YASim/Rotorpart.cpp b/src/FDM/YASim/Rotorpart.cpp
index 417904c9c932f45303352591aa9d0a380ae2547a..77520dee98d95aabe1cf4a9efa710271f806bbb5 100644 (file)
--- a/src/FDM/YASim/Rotorpart.cpp
+++ b/src/FDM/YASim/Rotorpart.cpp
@@ -1,3 +1,5 @@
+#include <ostream>
+
 #include <simgear/debug/logstream.hxx>
 
 #include "Math.hpp"
@@ -6,6 +8,8 @@
 #include <stdio.h>
 #include <string.h>
 namespace yasim {
+using std::endl;
+
 const float pi=3.14159;
 float _help = 0;
 Rotorpart::Rotorpart()
@@ -63,6 +67,8 @@ Rotorpart::Rotorpart()
     _rel_len_blade_start=0;
     _torque=0;
     _rotor_correction_factor=0.6;
+    _direction=0;
+    _balance=1;
 }
 
 void Rotorpart::inititeration(float dt,float *rot)
@@ -88,6 +94,16 @@ void Rotorpart::inititeration(float dt,float *rot)
     a=Math::dot3(rot,dir);
     _alphaalt -= a;
     _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
+
+    //unbalance
+    float b;
+    b=_rotor->getBalance();
+    float s =Math::sin(_phi+_direction);
+    float c =Math::cos(_phi+_direction);
+    if (s>0)
+        _balance=(b>0)?(1.-s*(1.-b)):(1.-s)*(1.+b);
+    else
+        _balance=(b>0)?1.:1.+b;
 }
 
 void Rotorpart::setRotor(Rotor *rotor)
@@ -95,7 +111,7 @@ void Rotorpart::setRotor(Rotor *rotor)
     _rotor=rotor;
 }
 
-void Rotorpart::setParameter(char *parametername, float value)
+void Rotorpart::setParameter(const char *parametername, float value)
 {
 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
 
@@ -179,6 +195,11 @@ void Rotorpart::setDirectionofRotorPart(float* p)
     for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
 }
 
+void Rotorpart::setDirection(float direction)
+{
+    _direction=direction;
+}
+
 void Rotorpart::setOmega(float value)
 {
     _omega=value;
@@ -432,22 +453,13 @@ float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
         //angle between blade movement caused by rotor-rotation and the
         //total movement of the blade
 
-        /* the next shold look like this, but this is the inner loop of
-           the rotor simulation. For small angles (and we hav only small
-           angles) the first order approximation works well
         lift_moment += r*(lift * Math::cos(angle) 
             - drag * Math::sin(angle));
         *torque     += r*(drag * Math::cos(angle) 
             + lift * Math::sin(angle));
-            */
-        lift_moment += r*(lift * (1-angle*angle) 
-            - drag * angle);
-        *torque     += r*(drag * (1-angle*angle) 
-            + lift * angle);
-        
         if (returnlift!=NULL) *returnlift+=lift;
     }
-    //as above, use 1st order approximation
+    //use 1st order approximation for alpha
     //float alpha=Math::atan2(lift_moment,_centripetalforce * _len); 
     float alpha;
     if (_shared_flap_hinge)
@@ -456,15 +468,27 @@ float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
         if (Math::abs(_alphaalt) >1e-6)
             div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
         if (Math::abs(div)>1e-6)
+        {
             alpha=lift_moment/div;
+        }
         else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
         {
             float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
             if (Math::abs(div)>1e-6)
+            {
                 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
+            }
+            else
+                alpha=_alphaalt;
         }
         else
             alpha=_alphaalt;
+        if (_omega/_omegan<0.2)
+        {
+            float frac = 0.001+_omega/_omegan*4.995;
+            alpha=Math::clamp(alpha,_alphamin,_alphamax);
+            alpha=_alphaalt*(1-frac)+frac*alpha;
+        }
     }
     else
     {
@@ -535,6 +559,11 @@ void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
 
     //missing: consideration of rellenhinge
+
+    //add the unbalance
+    _centripetalforce*=_balance;
+    scalar_torque*=_balance;
+
     float xforce = Math::cos(alpha)*_centripetalforce;
     float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
     *torque_scalar=scalar_torque;