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