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