src/FDM/YASim/Rotor.cpp

   1 #include <simgear/debug/logstream.hxx>
   2
   3 #include "Math.hpp"
   4 #include "Surface.hpp"
   5 #include "Rotorpart.hpp"
   6 #include "Glue.hpp"
   7 #include "Ground.hpp"
   8 #include "Rotor.hpp"
   9
  10 #include STL_IOSTREAM
  11 #include STL_IOMANIP
  12
  13 SG_USING_STD(setprecision);
  14
  15 #ifdef TEST_DEBUG
  16 #include <stdio.h>
  17 #endif
  18 #include <string.h>
  19 #include <iostream>
  20 #include <sstream>
  21
  22
  23
  24 namespace yasim {
  25
  26 const float pi=3.14159;
  27
  28 static inline float sqr(float a) { return a * a; }
  29
  30 Rotor::Rotor()
  31 {
  32     int i;
  33     _alpha0=-.05;
  34     _alpha0factor=1;
  35     _alphamin=-.1;
  36     _alphamax= .1;
  37     _alphaoutput[0][0]=0;
  38     _alphaoutput[1][0]=0;
  39     _alphaoutput[2][0]=0;
  40     _alphaoutput[3][0]=0;
  41     _alphaoutput[4][0]=0;
  42     _alphaoutput[5][0]=0;
  43     _alphaoutput[6][0]=0;
  44     _alphaoutput[7][0]=0;
  45     _base[0] = _base[1] = _base[2] = 0;
  46     _ccw=0;
  47     _delta=1;
  48     _delta3=0;
  49     _diameter =10;
  50     _dynamic=1;
  51     _engineon=0;
  52     _force_at_pitch_a=0;
  53     _forward[0]=1;
  54     _forward[1]=_forward[2]=0;
  55     _max_pitch=13./180*pi;
  56     _maxcyclicail=10./180*pi;
  57     _maxcyclicele=10./180*pi;
  58     _maxteeterdamp=0;
  59     _mincyclicail=-10./180*pi;
  60     _mincyclicele=-10./180*pi;
  61     _min_pitch=-.5/180*pi;
  62     _cyclicele=0;
  63     _cyclicail=0;
  64     _name[0] = 0;
  65     _normal[0] = _normal[1] = 0;
  66     _normal[2] = 1;
  67     _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
  68     _normal_with_yaw_roll[2]=1;
  69     _number_of_blades=4;
  70     _omega=_omegan=_omegarel=_omegarelneu=0;
  71     _phi_null=0;
  72     _ddt_omega=0;
  73     _pitch_a=0;
  74     _pitch_b=0;
  75     _power_at_pitch_0=0;
  76     _power_at_pitch_b=0;
  77     _no_torque=0;
  78     _rel_blade_center=.7;
  79     _rel_len_hinge=0.01;
  80     _shared_flap_hinge=false;
  81     _rellenteeterhinge=0.01;
  82     _rotor_rpm=442;
  83     _sim_blades=0;
  84     _teeterdamp=0.00001;
  85     _translift=0.05;
  86     _weight_per_blade=42;
  87     _translift_ve=20;
  88     _translift_maxfactor=1.3;
  89     _ground_effect_constant=0.1;
  90     _vortex_state_lift_factor=0.4;
  91     _vortex_state_c1=0.1;
  92     _vortex_state_c2=0;
  93     _vortex_state_c3=0;
  94     _vortex_state_e1=1;
  95     _vortex_state_e2=1;
  96     _vortex_state_e3=1;
  97     _vortex_state=0;
  98     _lift_factor=1;
  99     _liftcoef=0.1;
 100     _dragcoef0=0.1;
 101     _dragcoef1=0.1;
 102     _twist=0;
 103     _number_of_segments=1;
 104     _number_of_parts=4;
 105     _rel_len_where_incidence_is_measured=0.7;
 106     _torque_of_inertia=1;
 107     _torque=0;
 108     _chord=0.3;
 109     _taper=1;
 110     _airfoil_incidence_no_lift=0;
 111     _rel_len_blade_start=0;
 112     _airfoil_lift_coefficient=0;
 113     _airfoil_drag_coefficient0=0;
 114     _airfoil_drag_coefficient1=0;
 115     for(i=0; i<2; i++)
 116         _global_ground[i] =  0;
 117     _global_ground[2] = 1;
 118     _global_ground[3] = -1e3;
 119     _incidence_stall_zero_speed=18*pi/180.;
 120     _incidence_stall_half_sonic_speed=14*pi/180.;
 121     _lift_factor_stall=0.28;
 122     _stall_change_over=2*pi/180.;
 123     _drag_factor_stall=8;
 124     _stall_sum=1;
 125     _stall_v2sum=1;
 126     _collective=0;
 127     _yaw=_roll=0;
 128     for (int k=0;k<4;k++)
 129         for (i=0;i<3;i++)
 130             _groundeffectpos[k][i]=0;
 131     _ground_effect_altitude=1;
 132     _cyclic_factor=1;
 133     _lift_factor=_f_ge=_f_vs=_f_tl=1;
 134     _rotor_correction_factor=.65;
 135 }
 136
 137 Rotor::~Rotor()
 138 {
 139     int i;
 140     for(i=0; i<_rotorparts.size(); i++) {
 141         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 142         delete r;
 143     }
 144 }
 145
 146 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
 147 {
 148     _stall_sum=0;
 149     _stall_v2sum=0;
 150     _omegarel=omegarel;
 151     _omega=_omegan*_omegarel;
 152     _ddt_omega=_omegan*ddt_omegarel;
 153     int i;
 154     for(i=0; i<_rotorparts.size(); i++) {
 155         float s = Math::sin(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
 156         float c = Math::cos(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
 157         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 158         r->setOmega(_omega);
 159         r->setDdtOmega(_ddt_omega);
 160         r->inititeration(dt,rot);
 161         r->setCyclic(_cyclicail*c+_cyclicele*s);
 162     }
 163
 164     //calculate the normal of the rotor disc, for calcualtion of the downwash
 165     float side[3],help[3];
 166     Math::cross3(_normal,_forward,side);
 167     Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
 168
 169     Math::mul3(Math::sin(_yaw),_forward,help);
 170     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 171
 172     Math::mul3(Math::sin(_roll),side,help);
 173     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 174 }
 175
 176 float Rotor::calcStall(float incidence,float speed)
 177 {
 178     float stall_incidence=_incidence_stall_zero_speed
 179         +(_incidence_stall_half_sonic_speed
 180         -_incidence_stall_zero_speed)*speed/(343./2);
 181     //missing: Temeperature dependency of sonic speed
 182     incidence = Math::abs(incidence);
 183     if (incidence > (90./180.*pi))
 184         incidence = pi-incidence;
 185     float stall = (incidence-stall_incidence)/_stall_change_over;
 186     stall = Math::clamp(stall,0,1);
 187
 188     _stall_sum+=stall*speed*speed;
 189     _stall_v2sum+=speed*speed;
 190
 191     return stall;
 192 }
 193
 194 float Rotor::getLiftCoef(float incidence,float speed)
 195 {
 196     float stall=calcStall(incidence,speed);
 197     /* the next shold look like this, but this is the inner loop of
 198            the rotor simulation. For small angles (and we hav only small
 199            angles) the first order approximation works well
 200     float c1=  Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
 201     for c2 we would need higher order, because in stall the angle can be large
 202     */
 203     float i2;
 204     if (incidence > (pi/2))
 205         i2 = incidence-pi;
 206     else if (incidence <-(pi/2))
 207         i2 = (incidence+pi);
 208     else
 209         i2 = incidence;
 210     float c1=  (i2-_airfoil_incidence_no_lift)*_liftcoef;
 211     if (stall > 0)
 212     {
 213     float c2=  Math::sin(2*(incidence-_airfoil_incidence_no_lift))
 214         *_liftcoef*_lift_factor_stall;
 215     return (1-stall)*c1 + stall *c2;
 216     }
 217     else
 218         return c1;
 219 }
 220
 221 float Rotor::getDragCoef(float incidence,float speed)
 222 {
 223     float stall=calcStall(incidence,speed);
 224     float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
 225         *_dragcoef1+_dragcoef0);
 226     float c2= c1*_drag_factor_stall;
 227     return (1-stall)*c1 + stall *c2;
 228 }
 229
 230 int Rotor::getValueforFGSet(int j,char *text,float *f)
 231 {
 232     if (_name[0]==0) return 0;
 233     if (4>numRotorparts()) return 0; //compile first!
 234     if (j==0)
 235     {
 236         sprintf(text,"/rotors/%s/cone-deg", _name);
 237         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 238             +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
 239             +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
 240             +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
 241             )/4*180/pi;
 242     }
 243     else
 244         if (j==1)
 245         {
 246             sprintf(text,"/rotors/%s/roll-deg", _name);
 247             _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 248                 -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
 249                 )/2*(_ccw?-1:1);
 250             *f=_roll *180/pi;
 251         }
 252         else
 253             if (j==2)
 254             {
 255                 sprintf(text,"/rotors/%s/yaw-deg", _name);
 256                 _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
 257                     -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
 258                     )/2;
 259                 *f=_yaw*180/pi;
 260             }
 261             else
 262                 if (j==3)
 263                 {
 264                     sprintf(text,"/rotors/%s/rpm", _name);
 265                     *f=_omega/2/pi*60;
 266                 }
 267                 else
 268                     if (j==4)
 269                     {
 270                         sprintf(text,"/rotors/%s/debug/debugfge",_name);
 271                         *f=_f_ge;
 272                     }
 273                     else if (j==5)
 274                     {
 275                         sprintf(text,"/rotors/%s/debug/debugfvs",_name);
 276                         *f=_f_vs;
 277                     }
 278                     else if (j==6)
 279                     {
 280                         sprintf(text,"/rotors/%s/debug/debugftl",_name);
 281                         *f=_f_tl;
 282                     }
 283                     else if (j==7)
 284                     {
 285                         sprintf(text,"/rotors/%s/debug/vortexstate",_name);
 286                         *f=_vortex_state;
 287                     }
 288                     else if (j==8)
 289                     {
 290                         sprintf(text,"/rotors/%s/stall",_name);
 291                         *f=getOverallStall();
 292                     }
 293                     else if (j==9)
 294                     {
 295                         sprintf(text,"/rotors/%s/torque",_name);
 296                         *f=-_torque;;
 297                     }
 298                     else
 299                     {
 300                         int b=(j-10)/3;
 301                         if (b>=_number_of_blades)
 302                         {
 303                             return 0;
 304                         }
 305                         int w=j%3;
 306                         sprintf(text,"/rotors/%s/blade[%i]/%s",
 307                             _name,b,
 308                             w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
 309                         *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
 310                             +360*b/_number_of_blades*(_ccw?1:-1);
 311                         if (*f>360) *f-=360;
 312                         if (*f<0) *f+=360;
 313                         int k,l;
 314                         float rk,rl,p;
 315                         p=(*f/90);
 316                         k=int(p);
 317                         l=k+1;
 318                         rk=l-p;
 319                         rk=Math::clamp(rk,0,1);//Delete this
 320                         rl=1-rk;
 321                         if(w==2) {k+=2;l+=2;}
 322                         else
 323                             if(w==1) {k+=1;l+=1;}
 324                             k%=4;
 325                             l%=4;
 326                             if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
 327                                 +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
 328                             else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
 329                                 +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
 330                     }
 331                     return j+1;
 332 }
 333
 334 void Rotorgear::setEngineOn(int value)
 335 {
 336     _engineon=value;
 337 }
 338
 339 void Rotor::setAlpha0(float f)
 340 {
 341     _alpha0=f;
 342 }
 343
 344 void Rotor::setAlphamin(float f)
 345 {
 346     _alphamin=f;
 347 }
 348
 349 void Rotor::setAlphamax(float f)
 350 {
 351     _alphamax=f;
 352 }
 353
 354 void Rotor::setAlpha0factor(float f)
 355 {
 356     _alpha0factor=f;
 357 }
 358
 359 int Rotor::numRotorparts()
 360 {
 361     return _rotorparts.size();
 362 }
 363
 364 Rotorpart* Rotor::getRotorpart(int n)
 365 {
 366     return ((Rotorpart*)_rotorparts.get(n));
 367 }
 368
 369 int Rotorgear::getEngineon()
 370 {
 371     return _engineon;
 372 }
 373
 374 float Rotor::getTorqueOfInertia()
 375 {
 376     return _torque_of_inertia;
 377 }
 378
 379 void Rotor::setTorque(float f)
 380 {
 381     _torque=f;
 382 }
 383
 384 void Rotor::addTorque(float f)
 385 {
 386     _torque+=f;
 387 }
 388
 389 void Rotor::strncpy(char *dest,const char *src,int maxlen)
 390 {
 391     int n=0;
 392     while(src[n]&&n<(maxlen-1))
 393     {
 394         dest[n]=src[n];
 395         n++;
 396     }
 397     dest[n]=0;
 398 }
 399
 400 void Rotor::setNormal(float* normal)
 401 {
 402     int i;
 403     float invsum,sqrsum=0;
 404     for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
 405     if (sqrsum!=0)
 406         invsum=1/Math::sqrt(sqrsum);
 407     else
 408         invsum=1;
 409     for(i=0; i<3; i++)
 410     {
 411         _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
 412     }
 413 }
 414
 415 void Rotor::setForward(float* forward)
 416 {
 417     int i;
 418     float invsum,sqrsum=0;
 419     for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
 420     if (sqrsum!=0)
 421         invsum=1/Math::sqrt(sqrsum);
 422     else
 423         invsum=1;
 424     for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
 425 }
 426
 427 void Rotor::setForceAtPitchA(float force)
 428 {
 429     _force_at_pitch_a=force;
 430 }
 431
 432 void Rotor::setPowerAtPitch0(float value)
 433 {
 434     _power_at_pitch_0=value;
 435 }
 436
 437 void Rotor::setPowerAtPitchB(float value)
 438 {
 439     _power_at_pitch_b=value;
 440 }
 441
 442 void Rotor::setPitchA(float value)
 443 {
 444     _pitch_a=value/180*pi;
 445 }
 446
 447 void Rotor::setPitchB(float value)
 448 {
 449     _pitch_b=value/180*pi;
 450 }
 451
 452 void Rotor::setBase(float* base)
 453 {
 454     int i;
 455     for(i=0; i<3; i++) _base[i] = base[i];
 456 }
 457
 458 void Rotor::setMaxCyclicail(float value)
 459 {
 460     _maxcyclicail=value/180*pi;
 461 }
 462
 463 void Rotor::setMaxCyclicele(float value)
 464 {
 465     _maxcyclicele=value/180*pi;
 466 }
 467
 468 void Rotor::setMinCyclicail(float value)
 469 {
 470     _mincyclicail=value/180*pi;
 471 }
 472
 473 void Rotor::setMinCyclicele(float value)
 474 {
 475     _mincyclicele=value/180*pi;
 476 }
 477
 478 void Rotor::setMinCollective(float value)
 479 {
 480     _min_pitch=value/180*pi;
 481 }
 482
 483 void Rotor::setMaxCollective(float value)
 484 {
 485     _max_pitch=value/180*pi;
 486 }
 487
 488 void Rotor::setDiameter(float value)
 489 {
 490     _diameter=value;
 491 }
 492
 493 void Rotor::setWeightPerBlade(float value)
 494 {
 495     _weight_per_blade=value;
 496 }
 497
 498 void Rotor::setNumberOfBlades(float value)
 499 {
 500     _number_of_blades=int(value+.5);
 501 }
 502
 503 void Rotor::setRelBladeCenter(float value)
 504 {
 505     _rel_blade_center=value;
 506 }
 507
 508 void Rotor::setDynamic(float value)
 509 {
 510     _dynamic=value;
 511 }
 512
 513 void Rotor::setDelta3(float value)
 514 {
 515     _delta3=value;
 516 }
 517
 518 void Rotor::setDelta(float value)
 519 {
 520     _delta=value;
 521 }
 522
 523 void Rotor::setTranslift(float value)
 524 {
 525     _translift=value;
 526 }
 527
 528 void Rotor::setSharedFlapHinge(bool s)
 529 {
 530     _shared_flap_hinge=s;
 531 }
 532
 533 void Rotor::setC2(float value)
 534 {
 535     _c2=value;
 536 }
 537
 538 void Rotor::setStepspersecond(float steps)
 539 {
 540     _stepspersecond=steps;
 541 }
 542
 543 void Rotor::setRPM(float value)
 544 {
 545     _rotor_rpm=value;
 546 }
 547
 548 void Rotor::setPhiNull(float value)
 549 {
 550     _phi_null=value;
 551 }
 552
 553 void Rotor::setRelLenHinge(float value)
 554 {
 555     _rel_len_hinge=value;
 556 }
 557
 558 void Rotor::setAlphaoutput(int i, const char *text)
 559 {
 560     strncpy(_alphaoutput[i],text,255);
 561 }
 562
 563 void Rotor::setName(const char *text)
 564 {
 565     strncpy(_name,text,256);//256: some space needed for settings
 566 }
 567
 568 void Rotor::setCcw(int ccw)
 569 {
 570     _ccw=ccw;
 571 }
 572
 573 void Rotor::setNotorque(int value)
 574 {
 575     _no_torque=value;
 576 }
 577
 578 void Rotor::setRelLenTeeterHinge(float f)
 579 {
 580     _rellenteeterhinge=f;
 581 }
 582
 583 void Rotor::setTeeterdamp(float f)
 584 {
 585     _teeterdamp=f;
 586 }
 587
 588 void Rotor::setMaxteeterdamp(float f)
 589 {
 590     _maxteeterdamp=f;
 591 }
 592
 593 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
 594 {
 595     int i;
 596     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 597 }
 598
 599 void Rotor::setParameter(char *parametername, float value)
 600 {
 601 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
 602     p(translift_ve,1)
 603         p(translift_maxfactor,1)
 604         p(ground_effect_constant,1)
 605         p(vortex_state_lift_factor,1)
 606         p(vortex_state_c1,1)
 607         p(vortex_state_c2,1)
 608         p(vortex_state_c3,1)
 609         p(vortex_state_e1,1)
 610         p(vortex_state_e2,1)
 611         p(vortex_state_e3,1)
 612         p(twist,pi/180.)
 613         p(number_of_segments,1)
 614         p(number_of_parts,1)
 615         p(rel_len_where_incidence_is_measured,1)
 616         p(chord,1)
 617         p(taper,1)
 618         p(airfoil_incidence_no_lift,pi/180.)
 619         p(rel_len_blade_start,1)
 620         p(incidence_stall_zero_speed,pi/180.)
 621         p(incidence_stall_half_sonic_speed,pi/180.)
 622         p(lift_factor_stall,1)
 623         p(stall_change_over,pi/180.)
 624         p(drag_factor_stall,1)
 625         p(airfoil_lift_coefficient,1)
 626         p(airfoil_drag_coefficient0,1)
 627         p(airfoil_drag_coefficient1,1)
 628         p(cyclic_factor,1)
 629         p(rotor_correction_factor,1)
 630         SG_LOG(SG_INPUT, SG_ALERT,
 631             "internal error in parameter set up for rotor: '" <<
 632             parametername <<"'" << endl);
 633 #undef p
 634 }
 635
 636 float Rotor::getLiftFactor()
 637 {
 638     return _lift_factor;
 639 }
 640
 641 void Rotorgear::setRotorBrake(float lval)
 642 {
 643     lval = Math::clamp(lval, 0, 1);
 644     _rotorbrake=lval;
 645 }
 646
 647 void Rotor::setCollective(float lval)
 648 {
 649     lval = Math::clamp(lval, -1, 1);
 650     int i;
 651     _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
 652     for(i=0; i<_rotorparts.size(); i++) {
 653         ((Rotorpart*)_rotorparts.get(i))->setCollective(_collective);
 654     }
 655 }
 656
 657 void Rotor::setCyclicele(float lval,float rval)
 658 {
 659     lval = Math::clamp(lval, -1, 1);
 660     _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
 661 }
 662
 663 void Rotor::setCyclicail(float lval,float rval)
 664 {
 665     lval = Math::clamp(lval, -1, 1);
 666     if (_ccw) lval *=-1;
 667     _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
 668 }
 669
 670 void Rotor::getPosition(float* out)
 671 {
 672     int i;
 673     for(i=0; i<3; i++) out[i] = _base[i];
 674 }
 675
 676 void Rotor::calcLiftFactor(float* v, float rho, State *s)
 677 {
 678     //calculates _lift_factor, which is a foactor for the lift of the rotor
 679     //due to
 680     //- ground effect (_f_ge)
 681     //- vortex state (_f_vs)
 682     //- translational lift (_f_tl)
 683     _f_ge=1;
 684     _f_tl=1;
 685     _f_vs=1;
 686
 687     // Calculate ground effect
 688     _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
 689
 690     // Now calculate translational lift
 691     float v_vert = Math::dot3(v,_normal);
 692     float help[3];
 693     Math::cross3(v,_normal,help);
 694     float v_horiz = Math::mag3(help);
 695     _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
 696         *(_translift_maxfactor-1)+1)/_translift_maxfactor;
 697
 698     _lift_factor = _f_ge*_f_tl*_f_vs;
 699
 700     //store the gravity direction
 701     Glue::geodUp(s->pos, _grav_direction);
 702 }
 703
 704 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
 705 {
 706     _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
 707         _groundeffectpos[0],_groundeffectpos[1],
 708         _groundeffectpos[2],_groundeffectpos[3]);
 709 }
 710
 711 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
 712         float *pos0,float *pos1,float *pos2,float *pos3,
 713         int iteration,float a0,float a1,float a2,float a3)
 714 {
 715     float a[5];
 716     float *p[5],pos4[3];
 717     a[0]=a0;
 718     a[1]=a1;
 719     a[2]=a2;
 720     a[3]=a3;
 721     a[4]=-1;
 722     p[0]=pos0;
 723     p[1]=pos1;
 724     p[2]=pos2;
 725     p[3]=pos3;
 726     p[4]=pos4;
 727     Math::add3(p[0],p[2],p[4]);
 728     Math::mul3(0.5,p[4],p[4]);//the center
 729
 730     float mina=100*_diameter;
 731     float suma=0;
 732     for (int i=0;i<5;i++)
 733     {
 734         if (a[i]==-1)//in the first iteration,(iteration==0) no height is
 735                      //passed to this function, these missing values are
 736                      //marked by ==-1
 737         {
 738             double pt[3];
 739             s->posLocalToGlobal(p[i], pt);
 740
 741             // Ask for the ground plane in the global coordinate system
 742             double global_ground[4];
 743             float global_vel[3];
 744             ground_cb->getGroundPlane(pt, global_ground, global_vel);
 745             // find h, the distance to the ground
 746             // The ground plane transformed to the local frame.
 747             float ground[4];
 748             s->planeGlobalToLocal(global_ground, ground);
 749
 750             a[i] = ground[3] - Math::dot3(p[i], ground);
 751             // Now a[i] is the distance from p[i] to ground
 752         }
 753         suma+=a[i];
 754         if (a[i]<mina)
 755             mina=a[i];
 756     }
 757     if (mina>=10*_diameter)
 758         return mina; //the ground effect will be zero
 759
 760     //check if further recursion is neccessary
 761     //if the height does not differ more than 20%, than
 762     //we can return then mean height, if not split
 763     //zhe square to four parts and calcualte the height
 764     //for each part
 765     //suma * 0.2 is the mean
 766     //0.15 is the maximum allowed difference from the mean
 767     //to the height at the center
 768     if ((iteration>2)
 769        ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
 770         return suma*0.2;
 771     suma=0;
 772     float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
 773     for (int i=0;i<4;i++)
 774     {
 775         Math::add3(p[i],p[(i+1)&3],pc[i]);
 776         Math::mul3(0.5,pc[i],pc[i]);
 777         double pt[3];
 778         s->posLocalToGlobal(pc[i], pt);
 779
 780         // Ask for the ground plane in the global coordinate system
 781         double global_ground[4];
 782         float global_vel[3];
 783         ground_cb->getGroundPlane(pt, global_ground, global_vel);
 784         // find h, the distance to the ground
 785         // The ground plane transformed to the local frame.
 786         float ground[4];
 787         s->planeGlobalToLocal(global_ground, ground);
 788
 789         ac[i] = ground[3] - Math::dot3(p[i], ground);
 790         // Now ac[i] is the distance from pc[i] to ground
 791     }
 792     return 0.25*
 793         (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
 794             iteration+1,a[0],ac[0],a[4],ac[3])
 795         +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
 796             iteration+1,a[1],ac[0],a[4],ac[1])
 797         +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
 798             iteration+1,a[2],ac[1],a[4],ac[2])
 799         +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
 800             iteration+1,a[3],ac[2],a[4],ac[3])
 801         );
 802 }
 803
 804 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
 805 {
 806     float pos2rotor[3],tmp[3];
 807     Math::sub3(_base,pos,pos2rotor);
 808     float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 809     //calculate incidence at 0.7r;
 810     float inc = _collective+_twist *0.7
 811                 - _twist*_rel_len_where_incidence_is_measured;
 812     if (inc < 0)
 813         dist *=-1;
 814     if (dist<0) // we are not in the downwash region
 815     {
 816         downwash[0]=downwash[1]=downwash[2]=0.;
 817         return;
 818     }
 819
 820     //calculate the mean downwash speed directly beneath the rotor disk
 821     float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
 822     //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
 823     //0.8 the slip of the rotor.
 824
 825     //calculate the time the wind needed from thr rotor to here
 826     if (v1bar< 1) v1bar = 1;
 827     float time=dist/v1bar;
 828
 829     //calculate the pos2rotor, where the rotor was, "time" ago
 830     Math::mul3(time,v_heli,tmp);
 831     Math::sub3(pos2rotor,tmp,pos2rotor);
 832
 833     //and again calculate dist
 834     dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 835     //missing the normal is offen not pointing to the normal of the rotor
 836     //disk. Rotate the normal by yaw and tilt angle
 837
 838     if (inc < 0)
 839         dist *=-1;
 840     if (dist<0) // we are not in the downwash region
 841     {
 842         downwash[0]=downwash[1]=downwash[2]=0.;
 843         return;
 844     }
 845     //of course this could be done in a runge kutta integrator, but it's such
 846     //a approximation that I beleave, it would'nt be more realistic
 847
 848     //calculate the dist to the rotor-axis
 849     Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
 850     float r= Math::mag3(tmp);
 851     //calculate incidence at r;
 852     float rel_r = r *2 /_diameter;
 853     float inc_r = _collective+_twist * r /_diameter * 2
 854         - _twist*_rel_len_where_incidence_is_measured;
 855
 856     //calculate the downwash speed directly beneath the rotor disk
 857     float v1=0;
 858     if (rel_r<1)
 859         v1 = Math::sin(inc_r) *_omega * r * 0.8;
 860
 861     //calcualte the downwash speed in a distance "dist" to the rotor disc,
 862     //for large dist. The speed is assumed do follow a gausian distribution
 863     //with sigma increasing with dist^2:
 864     //sigma is assumed to be half of the rotor diameter directly beneath the
 865     //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
 866
 867     float sigma=_diameter/2 + dist * dist / _diameter /4.;
 868     float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
 869         * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
 870
 871     //calculate the weight of the two downwash velocities.
 872     //Directly beneath the disc it is v1, far away it is v2
 873     float g = Math::pow(2.7183,-2*dist/_diameter);
 874     //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
 875
 876     float v = g * v1 + (1-g) * v2;
 877     Math::mul3(-v,_normal_with_yaw_roll,downwash);
 878     //the downwash is calculated in the opposite direction of the normal
 879 }
 880
 881 void Rotor::compile()
 882 {
 883     // Have we already been compiled?
 884     if(_rotorparts.size() != 0) return;
 885
 886     //rotor is divided into _number_of_parts parts
 887     //each part is calcualted at _number_of_segments points
 888
 889     //clamp to 4..256
 890     //and make it a factor of 4
 891     _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
 892
 893     _dynamic=_dynamic*(1/                          //inverse of the time
 894         ( (60/_rotor_rpm)/4         //for rotating 90 deg
 895         +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
 896                                                //will pass a given point
 897         ));
 898     float directions[5][3];
 899     //pointing forward, right, ... the 5th is ony for calculation
 900     directions[0][0]=_forward[0];
 901     directions[0][1]=_forward[1];
 902     directions[0][2]=_forward[2];
 903     int i;
 904     for (i=1;i<5;i++)
 905     {
 906         if (!_ccw)
 907             Math::cross3(directions[i-1],_normal,directions[i]);
 908         else
 909             Math::cross3(_normal,directions[i-1],directions[i]);
 910         Math::unit3(directions[i],directions[i]);
 911     }
 912     Math::set3(directions[4],directions[0]);
 913     // now directions[0] is perpendicular to the _normal.and has a length
 914     // of 1. if _forward is already normalized and perpendicular to the
 915     // normal, directions[0] will be the same
 916     for (i=0;i<4;i++)
 917     {
 918         Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
 919         Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
 920     }
 921     float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
 922     //was pounds -> now kg
 923
 924     _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
 925         * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
 926     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 927     float lentocenter=_diameter*_rel_blade_center*0.5;
 928     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 929     float zentforce=rotorpartmass*speed*speed/lentocenter;
 930     float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
 931     // was pounds of force, now N, devided by _number_of_parts
 932     //(so its now per rotorpart)
 933
 934     float torque0=0,torquemax=0,torqueb=0;
 935     float omega=_rotor_rpm/60*2*pi;
 936     _omegan=omega;
 937     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 938     float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
 939     _delta*=delta_theoretical;
 940
 941     float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
 942         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
 943     float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega)
 944         +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor;
 945     _phi=Math::acos(_rel_len_hinge);
 946     _phi-=Math::atan(_delta3);
 947     if (!_no_torque)
 948     {
 949         torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
 950         // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
 951         torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
 952         torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
 953
 954         if(_ccw)
 955         {
 956             torque0*=-1;
 957             torquemax*=-1;
 958             torqueb*=-1;
 959         }
 960     }
 961
 962     Rotorpart* rps[256];
 963     for (i=0;i<_number_of_parts;i++)
 964     {
 965         float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
 966         float s = Math::sin(2*pi*i/_number_of_parts);
 967         float c = Math::cos(2*pi*i/_number_of_parts);
 968         float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi));
 969         float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi));
 970         float direction[3],nextdirection[3],help[3],direction90deg[3];
 971         Math::mul3(c ,directions[0],help);
 972         Math::mul3(s ,directions[1],direction);
 973         Math::add3(help,direction,direction);
 974
 975         Math::mul3(c ,directions[1],help);
 976         Math::mul3(s ,directions[2],direction90deg);
 977         Math::add3(help,direction90deg,direction90deg);
 978
 979         Math::mul3(cp ,directions[1],help);
 980         Math::mul3(sp ,directions[2],nextdirection);
 981         Math::add3(help,nextdirection,nextdirection);
 982
 983         Math::mul3(lentocenter,direction,lpos);
 984         Math::add3(lpos,_base,lpos);
 985         Math::mul3(lentoforceattac,nextdirection,lforceattac);
 986         //nextdirection: +90deg (gyro)!!!
 987
 988         Math::add3(lforceattac,_base,lforceattac);
 989         Math::mul3(speed,direction90deg,lspeed);
 990         Math::mul3(1,nextdirection,dirzentforce);
 991
 992         Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
 993             lspeed,dirzentforce,zentforce,pitchaforce,_delta3,rotorpartmass,
 994             _translift,_rel_len_hinge,lentocenter);
 995         int k = i*4/_number_of_parts;
 996         rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
 997         rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
 998         _rotorparts.add(rp);
 999         rp->setTorque(torquemax,torque0);
1000         rp->setRelamp(relamp);
1001         rp->setDirectionofRotorPart(direction);
1002         rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
1003     }
1004     for (i=0;i<_number_of_parts;i++)
1005     {
1006         rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
1007             rps[(i+1)%_number_of_parts],
1008             rps[(i+_number_of_parts/2)%_number_of_parts],
1009             rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
1010             rps[(i+_number_of_parts/4)%_number_of_parts]);
1011     }
1012     for (i=0;i<_number_of_parts;i++)
1013     {
1014         rps[i]->setCompiled();
1015     }
1016     float lift[4],torque[4], v_wind[3];
1017     v_wind[0]=v_wind[1]=v_wind[2]=0;
1018     rps[0]->setOmega(_omegan);
1019
1020     if (_airfoil_lift_coefficient==0)
1021     {
1022         //calculate the lift and drag coefficients now
1023         _dragcoef0=1;
1024         _dragcoef1=1;
1025         _liftcoef=1;
1026         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1027             &(torque[0]),&(lift[0])); //max_pitch a
1028         _liftcoef = pitchaforce/lift[0];
1029         _dragcoef0=1;
1030         _dragcoef1=0;
1031         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1032         //0 degree, c0
1033
1034         _dragcoef0=0;
1035         _dragcoef1=1;
1036         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1037         //0 degree, c1
1038
1039         _dragcoef0=1;
1040         _dragcoef1=0;
1041         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1042         //picth b, c0
1043
1044         _dragcoef0=0;
1045         _dragcoef1=1;
1046         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1047         //picth b, c1
1048
1049         if (torque[0]==0)
1050         {
1051             _dragcoef1=torque0/torque[1];
1052             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1053         }
1054         else
1055         {
1056             _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1057                 /(torque[1]/torque[0]-torque[3]/torque[2]);
1058             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1059         }
1060     }
1061     else
1062     {
1063         _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1064         _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1065         _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1066     }
1067
1068     //Check
1069     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1070         &(torque[0]),&(lift[0])); //pitch a
1071     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1072         &(torque[1]),&(lift[1])); //pitch b
1073     rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1074         &(torque[3]),&(lift[3])); //pitch 0
1075     SG_LOG(SG_GENERAL, SG_INFO,
1076         "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1077         << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1078         << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1079         << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1080         << endl
1081         << "at 10 deg:" << endl
1082         << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1083             *_number_of_parts/_number_of_blades/_c2
1084         << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1085         << endl
1086         << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1087         << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1088             << lift[3]*_number_of_parts << endl
1089         << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1090             << "kW " << lift[0]*_number_of_parts << endl
1091         << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1092             << "kW " << lift[1]*_number_of_parts << endl << endl );
1093
1094     //first calculation of relamp is wrong
1095     //it used pitchaforce, but this was unknown and
1096     //on the default value
1097     _delta*=lift[0]/pitchaforce;
1098     relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1099         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1100     for (i=0;i<_number_of_parts;i++)
1101     {
1102         rps[i]->setRelamp(relamp);
1103     }
1104     rps[0]->setOmega(0);
1105     writeInfo();
1106 }
1107 std::ostream &  operator<<(std::ostream & out, Rotor& r)
1108 {
1109 #define i(x) << #x << ":" << r.x << endl
1110 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1111     out << "Writing Info on Rotor "
1112     i(_name)
1113     i(_torque)
1114     i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1115     i (_chord)
1116     i( _taper)
1117     i( _airfoil_incidence_no_lift)
1118     i( _collective)
1119     i( _airfoil_lift_coefficient)
1120     i( _airfoil_drag_coefficient0)
1121     i( _airfoil_drag_coefficient1)
1122     i( _ccw)
1123     i( _number_of_segments)
1124     i( _number_of_parts)
1125     iv( _base)
1126     iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1127     i( _ground_effect_altitude)
1128     iv( _normal)
1129     iv( _normal_with_yaw_roll)
1130     iv( _forward)
1131     i( _diameter)
1132     i( _number_of_blades)
1133     i( _weight_per_blade)
1134     i( _rel_blade_center)
1135     i( _min_pitch)
1136     i( _max_pitch)
1137     i( _force_at_pitch_a)
1138     i( _pitch_a)
1139     i( _power_at_pitch_0)
1140     i( _power_at_pitch_b)
1141     i( _no_torque)
1142     i( _sim_blades)
1143     i( _pitch_b)
1144     i( _rotor_rpm)
1145     i( _rel_len_hinge)
1146     i( _maxcyclicail)
1147     i( _maxcyclicele)
1148     i( _mincyclicail)
1149     i( _mincyclicele)
1150     i( _delta3)
1151     i( _delta)
1152     i( _dynamic)
1153     i( _translift)
1154     i( _c2)
1155     i( _stepspersecond)
1156     i( _engineon)
1157     i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1158     i( _teeterdamp) i(_maxteeterdamp)
1159     i( _rellenteeterhinge)
1160     i( _translift_ve)
1161     i( _translift_maxfactor)
1162     i( _ground_effect_constant)
1163     i( _vortex_state_lift_factor)
1164     i( _vortex_state_c1)
1165     i( _vortex_state_c2)
1166     i( _vortex_state_c3)
1167     i( _vortex_state_e1)
1168     i( _vortex_state_e2)
1169     i( _vortex_state_e3)
1170     i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1171     i( _vortex_state)
1172     i( _liftcoef)
1173     i( _dragcoef0)
1174     i( _dragcoef1)
1175     i( _twist) //outer incidence = inner inner incidence + _twist
1176     i( _rel_len_where_incidence_is_measured)
1177     i( _torque_of_inertia)
1178     i( _rel_len_blade_start)
1179     i( _incidence_stall_zero_speed)
1180     i( _incidence_stall_half_sonic_speed)
1181     i( _lift_factor_stall)
1182     i( _stall_change_over)
1183     i( _drag_factor_stall)
1184     i( _stall_sum)
1185     i( _stall_v2sum)
1186     i( _yaw)
1187     i( _roll)
1188     i( _cyclicail)
1189     i( _cyclicele)
1190     i( _cyclic_factor) <<endl;
1191     int j;
1192     for(j=0; j<r._rotorparts.size(); j++) {
1193         out << *((Rotorpart*)r._rotorparts.get(j));
1194     }
1195     out <<endl << endl;
1196 #undef i
1197 #undef iv
1198     return out;
1199 }
1200 void Rotor:: writeInfo()
1201 {
1202 #ifdef TEST_DEBUG
1203     std::ostringstream buffer;
1204     buffer << *this;
1205     FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1206     if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1207     if (f)
1208     {
1209         fprintf(f,"%s",(const char *)buffer.str().c_str());
1210         fclose (f);
1211     }
1212 #endif
1213 }
1214 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1215     float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1216     float delta3,float mass,float translift,float rellenhinge,float len)
1217 {
1218     Rotorpart *r = new Rotorpart();
1219     r->setPosition(pos);
1220     r->setNormal(normal);
1221     r->setZentipetalForce(zentforce);
1222     r->setPositionForceAttac(posforceattac);
1223     r->setSpeed(speed);
1224     r->setDirectionofZentipetalforce(dirzentforce);
1225     r->setDelta3(delta3);
1226     r->setDynamic(_dynamic);
1227     r->setTranslift(_translift);
1228     r->setC2(_c2);
1229     r->setWeight(mass);
1230     r->setRelLenHinge(rellenhinge);
1231     r->setSharedFlapHinge(_shared_flap_hinge);
1232     r->setOmegaN(_omegan);
1233     r->setPhi(_phi_null);
1234     r->setAlpha0(_alpha0);
1235     r->setAlphamin(_alphamin);
1236     r->setAlphamax(_alphamax);
1237     r->setAlpha0factor(_alpha0factor);
1238     r->setLen(len);
1239     r->setDiameter(_diameter);
1240     r->setRotor(this);
1241 #define p(a) r->setParameter(#a,_##a);
1242     p(twist)
1243     p(number_of_segments)
1244     p(rel_len_where_incidence_is_measured)
1245     p(rel_len_blade_start)
1246     p(rotor_correction_factor)
1247 #undef p
1248     return r;
1249 }
1250
1251 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1252 {
1253     out[0] = v1[0] + frac*(v2[0]-v1[0]);
1254     out[1] = v1[1] + frac*(v2[1]-v1[1]);
1255     out[2] = v1[2] + frac*(v2[2]-v1[2]);
1256 }
1257
1258 void Rotorgear::initRotorIteration(float *lrot,float dt)
1259 {
1260     int i;
1261     float omegarel;
1262     if (!_rotors.size()) return;
1263     Rotor* r0 = (Rotor*)_rotors.get(0);
1264     omegarel= r0->getOmegaRelNeu();
1265     for(i=0; i<_rotors.size(); i++) {
1266         Rotor* r = (Rotor*)_rotors.get(i);
1267         r->inititeration(dt,omegarel,0,lrot);
1268     }
1269 }
1270
1271 void Rotorgear::calcForces(float* torqueOut)
1272 {
1273     int i,j;
1274     torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1275     // check,<if the engine can handle the torque of the rotors.
1276     // If not reduce the torque to the fueselage and change rotational
1277     // speed of the rotors instead
1278     if (_rotors.size())
1279     {
1280         float omegarel,omegan;
1281         Rotor* r0 = (Rotor*)_rotors.get(0);
1282         omegarel= r0->getOmegaRel();
1283
1284         float total_torque_of_inertia=0;
1285         float total_torque=0;
1286         for(i=0; i<_rotors.size(); i++) {
1287             Rotor* r = (Rotor*)_rotors.get(i);
1288             omegan=r->getOmegan();
1289             total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1290             //FIXME: this is constant, so this can be done in compile
1291
1292             total_torque+=r->getTorque()*omegan;
1293         }
1294         float max_torque_of_engine=0;
1295         if (_engineon)
1296         {
1297             max_torque_of_engine=_max_power_engine;
1298             float df=1-omegarel;
1299             df/=_engine_prop_factor;
1300             df = Math::clamp(df, 0, 1);
1301             max_torque_of_engine = df * _max_power_engine;
1302         }
1303         total_torque*=-1;
1304         _ddt_omegarel=0;
1305         float rel_torque_engine=1;
1306         if (total_torque<=0)
1307             rel_torque_engine=0;
1308         else
1309             if (max_torque_of_engine>0)
1310                 rel_torque_engine=1/max_torque_of_engine*total_torque;
1311             else
1312                 rel_torque_engine=0;
1313
1314         //add the rotor brake and the gear fritcion
1315         float dt=0.1f;
1316         if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1317
1318         float rotor_brake_torque;
1319         rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1320         //clamp it to the value you need to stop the rotor
1321         //to avod accelerate the rotor to neagtive rpm:
1322         rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1323             total_torque_of_inertia/dt*omegarel);
1324         max_torque_of_engine-=rotor_brake_torque;
1325
1326         //change the rotation of the rotors
1327         if ((max_torque_of_engine<total_torque) //decreasing rotation
1328             ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1329             //increasing rotation due to engine
1330             ||(total_torque<0) ) //increasing rotation due to autorotation
1331         {
1332             _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1333             if(max_torque_of_engine>total_torque)
1334             {
1335                 //check if the acceleration is due to the engine. If yes,
1336                 //the engine self limits the accel.
1337                 float lim1=-total_torque/total_torque_of_inertia;
1338                 //accel. by autorotation
1339
1340                 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1341                 //if the accel by autorotation greater than the max. engine
1342                 //accel, then this is the limit, if not: the engine is the limit
1343                 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1344             }
1345             if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1346             //clamp it to avoid overflow. Should never be reached
1347             if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1348
1349             if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1350             //for debug: negative or no maxpower will result
1351             //in permanet 100% rotation
1352
1353             omegarel+=dt*_ddt_omegarel;
1354
1355             if (omegarel>2.5) omegarel=2.5;
1356             //clamp it to avoid overflow. Should never be reached
1357             if (omegarel<-.5) omegarel=-.5;
1358
1359             r0->setOmegaRelNeu(omegarel);
1360             //calculate the torque, which is needed to accelerate the rotors.
1361             //Add this additional torque to the body
1362             for(j=0; j<_rotors.size(); j++) {
1363                 Rotor* r = (Rotor*)_rotors.get(j);
1364                 for(i=0; i<r->_rotorparts.size(); i++) {
1365                     float torque_scalar=0;
1366                     Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1367                     float torque[3];
1368                     rp->getAccelTorque(_ddt_omegarel,torque);
1369                     Math::add3(torque,torqueOut,torqueOut);
1370                 }
1371             }
1372         }
1373         _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1374     }
1375 }
1376
1377 void Rotorgear::addRotor(Rotor* rotor)
1378 {
1379     _rotors.add(rotor);
1380     _in_use = 1;
1381 }
1382
1383 void Rotorgear::compile()
1384 {
1385     float wgt = 0;
1386     for(int j=0; j<_rotors.size(); j++) {
1387         Rotor* r = (Rotor*)_rotors.get(j);
1388         r->compile();
1389     }
1390 }
1391
1392 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1393 {
1394     float tmp[3];
1395     downwash[0]=downwash[1]=downwash[2]=0;
1396     for(int i=0; i<_rotors.size(); i++) {
1397         Rotor* ro = (Rotor*)_rotors.get(i);
1398         ro->getDownWash(pos,v_heli,tmp);
1399         Math::add3(downwash,tmp,downwash);    //  + downwash
1400     }
1401 }
1402
1403 void Rotorgear::setParameter(char *parametername, float value)
1404 {
1405 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1406         p(max_power_engine,1000)
1407         p(engine_prop_factor,1)
1408         p(yasimdragfactor,1)
1409         p(yasimliftfactor,1)
1410         p(max_power_rotor_brake,1000)
1411         p(rotorgear_friction,1000)
1412         p(engine_accel_limit,0.01)
1413         SG_LOG(SG_INPUT, SG_ALERT,
1414             "internal error in parameter set up for rotorgear: '"
1415             << parametername <<"'" << endl);
1416 #undef p
1417 }
1418 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1419 {
1420     if (j==0)
1421     {
1422         sprintf(text,"/rotors/gear/total-torque");
1423         *f=_total_torque_on_engine;
1424     } else return 0;
1425     return j+1;
1426 }
1427 Rotorgear::Rotorgear()
1428 {
1429     _in_use=0;
1430     _engineon=0;
1431     _rotorbrake=0;
1432     _max_power_rotor_brake=1;
1433     _rotorgear_friction=1;
1434     _max_power_engine=1000*450;
1435     _engine_prop_factor=0.05f;
1436     _yasimdragfactor=1;
1437     _yasimliftfactor=1;
1438     _ddt_omegarel=0;
1439     _engine_accel_limit=0.05f;
1440     _total_torque_on_engine=0;
1441 }
1442
1443 Rotorgear::~Rotorgear()
1444 {
1445     for(int i=0; i<_rotors.size(); i++)
1446         delete (Rotor*)_rotors.get(i);
1447 }
1448
1449 }; // namespace yasim