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-deg", _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-deg", _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-deg", _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,
 305                             w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
 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     // now directions[0] is perpendicular to the _normal.and has a length
 898     // of 1. if _forward is already normalized and perpendicular to the
 899     // normal, directions[0] will be the same
 900     for (i=0;i<4;i++)
 901     {
 902         Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
 903         Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
 904     }
 905     float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
 906     //was pounds -> now kg
 907
 908     _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
 909         * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
 910     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 911     float lentocenter=_diameter*_rel_blade_center*0.5;
 912     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 913     float zentforce=rotorpartmass*speed*speed/lentocenter;
 914     float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
 915     // was pounds of force, now N, devided by _number_of_parts
 916     //(so its now per rotorpart)
 917
 918     float torque0=0,torquemax=0,torqueb=0;
 919     float omega=_rotor_rpm/60*2*pi;
 920     _omegan=omega;
 921     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 922     _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
 923
 924     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 925     float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
 926         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
 927     if (!_no_torque)
 928     {
 929         torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
 930         // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
 931         torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
 932         torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
 933
 934         if(_ccw)
 935         {
 936             torque0*=-1;
 937             torquemax*=-1;
 938             torqueb*=-1;
 939         }
 940     }
 941
 942     Rotorpart* rps[256];
 943     for (i=0;i<_number_of_parts;i++)
 944     {
 945         float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
 946         float s = Math::sin(2*pi*i/_number_of_parts);
 947         float c = Math::cos(2*pi*i/_number_of_parts);
 948         float direction[3],nextdirection[3],help[3];
 949         Math::mul3(c ,directions[0],help);
 950         Math::mul3(s ,directions[1],direction);
 951         Math::add3(help,direction,direction);
 952
 953         Math::mul3(c ,directions[1],help);
 954         Math::mul3(s ,directions[2],nextdirection);
 955         Math::add3(help,nextdirection,nextdirection);
 956
 957         Math::mul3(lentocenter,direction,lpos);
 958         Math::add3(lpos,_base,lpos);
 959         Math::mul3(lentoforceattac,nextdirection,lforceattac);
 960         //nextdirection: +90deg (gyro)!!!
 961
 962         Math::add3(lforceattac,_base,lforceattac);
 963         Math::mul3(speed,nextdirection,lspeed);
 964         Math::mul3(1,nextdirection,dirzentforce);
 965
 966
 967         float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
 968         float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
 969
 970         Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
 971             lspeed,dirzentforce,zentforce,pitchaforce, _max_pitch,_min_pitch,
 972             mincyclic,maxcyclic,_delta3,rotorpartmass,_translift,
 973             _rel_len_hinge,lentocenter);
 974         int k = i*4/_number_of_parts;
 975         rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
 976         rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
 977         _rotorparts.add(rp);
 978         rp->setTorque(torquemax,torque0);
 979         rp->setRelamp(relamp);
 980         rp->setDirectionofRotorPart(direction);
 981         rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
 982     }
 983     for (i=0;i<_number_of_parts;i++)
 984     {
 985         rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
 986             rps[(i+1)%_number_of_parts],
 987             rps[(i+_number_of_parts/2)%_number_of_parts],
 988             rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
 989             rps[(i+_number_of_parts/4)%_number_of_parts]);
 990     }
 991     for (i=0;i<_number_of_parts;i++)
 992     {
 993         rps[i]->setCompiled();
 994     }
 995     float lift[4],torque[4], v_wind[3];
 996     v_wind[0]=v_wind[1]=v_wind[2]=0;
 997     rps[0]->setOmega(_omegan);
 998
 999     if (_airfoil_lift_coefficient==0)
1000     {
1001         //calculate the lift and drag coefficients now
1002         _dragcoef0=1;
1003         _dragcoef1=1;
1004         _liftcoef=1;
1005         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1006             &(torque[0]),&(lift[0])); //max_pitch a
1007         _liftcoef = pitchaforce/lift[0];
1008         _dragcoef0=1;
1009         _dragcoef1=0;
1010         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1011         //0 degree, c0
1012
1013         _dragcoef0=0;
1014         _dragcoef1=1;
1015         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1016         //0 degree, c1
1017
1018         _dragcoef0=1;
1019         _dragcoef1=0;
1020         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1021         //picth b, c0
1022
1023         _dragcoef0=0;
1024         _dragcoef1=1;
1025         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1026         //picth b, c1
1027
1028         if (torque[0]==0)
1029         {
1030             _dragcoef1=torque0/torque[1];
1031             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1032         }
1033         else
1034         {
1035             _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1036                 /(torque[1]/torque[0]-torque[3]/torque[2]);
1037             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1038         }
1039     }
1040     else
1041     {
1042         _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1043         _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1044         _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1045     }
1046
1047     //Check
1048     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1049         &(torque[0]),&(lift[0])); //pitch a
1050     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1051         &(torque[1]),&(lift[1])); //pitch b
1052     rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1053         &(torque[3]),&(lift[3])); //pitch 0
1054     SG_LOG(SG_GENERAL, SG_INFO,
1055         "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1056         << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1057         << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1058         << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1059         << endl
1060         << "at 10 deg:" << endl
1061         << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1062             *_number_of_parts/_number_of_blades/_c2
1063         << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1064         << endl
1065         << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1066         << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1067             << lift[3]*_number_of_parts << endl
1068         << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1069             << "kW " << lift[0]*_number_of_parts << endl
1070         << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1071             << "kW " << lift[1]*_number_of_parts << endl << endl );
1072
1073     //first calculation of relamp is wrong
1074     //it used pitchaforce, but this was unknown and
1075     //on the default value
1076     _delta*=lift[0]/pitchaforce;
1077     relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1078         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1079     for (i=0;i<_number_of_parts;i++)
1080     {
1081         rps[i]->setRelamp(relamp);
1082     }
1083     rps[0]->setOmega(0);
1084     writeInfo();
1085 }
1086 std::ostream &  operator<<(std::ostream & out, /*const*/ Rotor& r)
1087 {
1088 #define i(x) << #x << ":" << r.x << endl
1089 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1090     out << "Writing Info on Rotor "
1091     i(_name)
1092     i(_torque)
1093     i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1094     i (_chord)
1095     i( _taper)
1096     i( _airfoil_incidence_no_lift)
1097     i( _collective)
1098     i( _airfoil_lift_coefficient)
1099     i( _airfoil_drag_coefficient0)
1100     i( _airfoil_drag_coefficient1)
1101     i( _ccw)
1102     i( _number_of_segments)
1103     i( _number_of_parts)
1104     iv( _base)
1105     iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1106     i( _ground_effect_altitude)
1107     iv( _normal)
1108     iv( _normal_with_yaw_roll)
1109     iv( _forward)
1110     i( _diameter)
1111     i( _number_of_blades)
1112     i( _weight_per_blade)
1113     i( _rel_blade_center)
1114     i( _min_pitch)
1115     i( _max_pitch)
1116     i( _force_at_pitch_a)
1117     i( _pitch_a)
1118     i( _power_at_pitch_0)
1119     i( _power_at_pitch_b)
1120     i( _no_torque)
1121     i( _sim_blades)
1122     i( _pitch_b)
1123     i( _rotor_rpm)
1124     i( _rel_len_hinge)
1125     i( _maxcyclicail)
1126     i( _maxcyclicele)
1127     i( _mincyclicail)
1128     i( _mincyclicele)
1129     i( _delta3)
1130     i( _delta)
1131     i( _dynamic)
1132     i( _translift)
1133     i( _c2)
1134     i( _stepspersecond)
1135     i( _engineon)
1136     i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1137     i( _teeterdamp) i(_maxteeterdamp)
1138     i( _rellenteeterhinge)
1139     i( _translift_ve)
1140     i( _translift_maxfactor)
1141     i( _ground_effect_constant)
1142     i( _vortex_state_lift_factor)
1143     i( _vortex_state_c1)
1144     i( _vortex_state_c2)
1145     i( _vortex_state_c3)
1146     i( _vortex_state_e1)
1147     i( _vortex_state_e2)
1148     i( _vortex_state_e3)
1149     i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1150     i( _vortex_state)
1151     i( _liftcoef)
1152     i( _dragcoef0)
1153     i( _dragcoef1)
1154     i( _twist) //outer incidence = inner inner incidence + _twist
1155     i( _rel_len_where_incidence_is_measured)
1156     i( _torque_of_inertia)
1157     i( _rel_len_blade_start)
1158     i( _incidence_stall_zero_speed)
1159     i( _incidence_stall_half_sonic_speed)
1160     i( _lift_factor_stall)
1161     i( _stall_change_over)
1162     i( _drag_factor_stall)
1163     i( _stall_sum)
1164     i( _stall_v2sum)
1165     i( _yaw)
1166     i( _roll)
1167     i( _cyclicail)
1168     i( _cyclicele)
1169     i( _cyclic_factor) <<endl;
1170     int j;
1171     for(j=0; j<r._rotorparts.size(); j++) {
1172         out << *((Rotorpart*)r._rotorparts.get(j));
1173     }
1174     out <<endl << endl;
1175 #undef i
1176 #undef iv
1177     return out;
1178 }
1179 void Rotor:: writeInfo()
1180 {
1181 #ifdef TEST_DEBUG
1182     std::ostringstream buffer;
1183     buffer << *this;
1184     FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1185     if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1186     if (f)
1187     {
1188         fprintf(f,"%s",(const char *)buffer.str().c_str());
1189         fclose (f);
1190     }
1191 #endif
1192 }
1193 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1194     float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1195     float maxpitch, float minpitch, float mincyclic,float maxcyclic,
1196     float delta3,float mass,float translift,float rellenhinge,float len)
1197 {
1198     Rotorpart *r = new Rotorpart();
1199     r->setPosition(pos);
1200     r->setNormal(normal);
1201     r->setZentipetalForce(zentforce);
1202     r->setPositionForceAttac(posforceattac);
1203     r->setSpeed(speed);
1204     r->setDirectionofZentipetalforce(dirzentforce);
1205     r->setMaxpitch(maxpitch);
1206     r->setMinpitch(minpitch);
1207     r->setMaxcyclic(maxcyclic);
1208     r->setMincyclic(mincyclic);
1209     r->setDelta3(delta3);
1210     r->setDynamic(_dynamic);
1211     r->setTranslift(_translift);
1212     r->setC2(_c2);
1213     r->setWeight(mass);
1214     r->setRelLenHinge(rellenhinge);
1215     r->setOmegaN(_omegan);
1216     r->setAlpha0(_alpha0);
1217     r->setAlphamin(_alphamin);
1218     r->setAlphamax(_alphamax);
1219     r->setAlpha0factor(_alpha0factor);
1220     r->setLen(len);
1221     r->setDiameter(_diameter);
1222     r->setRotor(this);
1223 #define p(a) r->setParameter(#a,_##a);
1224     p(twist)
1225     p(number_of_segments)
1226     p(rel_len_where_incidence_is_measured)
1227     p(rel_len_blade_start)
1228     p(rotor_correction_factor)
1229 #undef p
1230     return r;
1231 }
1232
1233 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1234 {
1235     out[0] = v1[0] + frac*(v2[0]-v1[0]);
1236     out[1] = v1[1] + frac*(v2[1]-v1[1]);
1237     out[2] = v1[2] + frac*(v2[2]-v1[2]);
1238 }
1239
1240 void Rotorgear::initRotorIteration(float *lrot,float dt)
1241 {
1242     int i;
1243     float omegarel;
1244     if (!_rotors.size()) return;
1245     Rotor* r0 = (Rotor*)_rotors.get(0);
1246     omegarel= r0->getOmegaRelNeu();
1247     for(i=0; i<_rotors.size(); i++) {
1248         Rotor* r = (Rotor*)_rotors.get(i);
1249         r->inititeration(dt,omegarel,0,lrot);
1250     }
1251 }
1252
1253 void Rotorgear::calcForces(float* torqueOut)
1254 {
1255     int i,j;
1256     torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1257     // check,<if the engine can handle the torque of the rotors.
1258     // If not reduce the torque to the fueselage and change rotational
1259     // speed of the rotors instead
1260     if (_rotors.size())
1261     {
1262         float omegarel,omegan;
1263         Rotor* r0 = (Rotor*)_rotors.get(0);
1264         omegarel= r0->getOmegaRel();
1265
1266         float total_torque_of_inertia=0;
1267         float total_torque=0;
1268         for(i=0; i<_rotors.size(); i++) {
1269             Rotor* r = (Rotor*)_rotors.get(i);
1270             omegan=r->getOmegan();
1271             total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1272             //FIXME: this is constant, so this can be done in compile
1273
1274             total_torque+=r->getTorque()*omegan;
1275         }
1276         float max_torque_of_engine=0;
1277         if (_engineon)
1278         {
1279             max_torque_of_engine=_max_power_engine;
1280             float df=1-omegarel;
1281             df/=_engine_prop_factor;
1282             df = Math::clamp(df, 0, 1);
1283             max_torque_of_engine = df * _max_power_engine;
1284         }
1285         total_torque*=-1;
1286         _ddt_omegarel=0;
1287         float rel_torque_engine=1;
1288         if (total_torque<=0)
1289             rel_torque_engine=0;
1290         else
1291             if (max_torque_of_engine>0)
1292                 rel_torque_engine=1/max_torque_of_engine*total_torque;
1293             else
1294                 rel_torque_engine=0;
1295
1296         //add the rotor brake and the gear fritcion
1297         float dt=0.1f;
1298         if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1299
1300         float rotor_brake_torque;
1301         rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1302         //clamp it to the value you need to stop the rotor
1303         //to avod accelerate the rotor to neagtive rpm:
1304         rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1305             total_torque_of_inertia/dt*omegarel);
1306         max_torque_of_engine-=rotor_brake_torque;
1307
1308         //change the rotation of the rotors
1309         if ((max_torque_of_engine<total_torque) //decreasing rotation
1310             ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1311             //increasing rotation due to engine
1312             ||(total_torque<0) ) //increasing rotation due to autorotation
1313         {
1314             _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1315             if(max_torque_of_engine>total_torque)
1316             {
1317                 //check if the acceleration is due to the engine. If yes,
1318                 //the engine self limits the accel.
1319                 float lim1=-total_torque/total_torque_of_inertia;
1320                 //accel. by autorotation
1321
1322                 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1323                 //if the accel by autorotation greater than the max. engine
1324                 //accel, then this is the limit, if not: the engine is the limit
1325                 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1326             }
1327             if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1328             //clamp it to avoid overflow. Should never be reached
1329             if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1330
1331             if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1332             //for debug: negative or no maxpower will result
1333             //in permanet 100% rotation
1334
1335             omegarel+=dt*_ddt_omegarel;
1336
1337             if (omegarel>2.5) omegarel=2.5;
1338             //clamp it to avoid overflow. Should never be reached
1339             if (omegarel<-.5) omegarel=-.5;
1340
1341             r0->setOmegaRelNeu(omegarel);
1342             //calculate the torque, which is needed to accelerate the rotors.
1343             //Add this additional torque to the body
1344             for(j=0; j<_rotors.size(); j++) {
1345                 Rotor* r = (Rotor*)_rotors.get(j);
1346                 for(i=0; i<r->_rotorparts.size(); i++) {
1347                     float torque_scalar=0;
1348                     Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1349                     float torque[3];
1350                     rp->getAccelTorque(_ddt_omegarel,torque);
1351                     Math::add3(torque,torqueOut,torqueOut);
1352                 }
1353             }
1354         }
1355         _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1356     }
1357 }
1358
1359 void Rotorgear::addRotor(Rotor* rotor)
1360 {
1361     _rotors.add(rotor);
1362     _in_use = 1;
1363 }
1364
1365 float Rotorgear::compile(RigidBody* body)
1366 {
1367     float wgt = 0;
1368     for(int j=0; j<_rotors.size(); j++) {
1369         Rotor* r = (Rotor*)_rotors.get(j);
1370         r->compile();
1371         int i;
1372         for(i=0; i<r->numRotorparts(); i++) {
1373             Rotorpart* rp = (Rotorpart*)r->getRotorpart(i);
1374             float mass = rp->getWeight();
1375             mass = mass * Math::sqrt(mass);
1376             float pos[3];
1377             rp->getPosition(pos);
1378             body->addMass(mass, pos);
1379             wgt += mass;
1380         }
1381     }
1382     return wgt;
1383 }
1384
1385 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1386 {
1387     float tmp[3];
1388     downwash[0]=downwash[1]=downwash[2]=0;
1389     for(int i=0; i<_rotors.size(); i++) {
1390         Rotor* ro = (Rotor*)_rotors.get(i);
1391         ro->getDownWash(pos,v_heli,tmp);
1392         Math::add3(downwash,tmp,downwash);    //  + downwash
1393     }
1394 }
1395
1396 void Rotorgear::setParameter(char *parametername, float value)
1397 {
1398 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1399         p(max_power_engine,1000)
1400         p(engine_prop_factor,1)
1401         p(yasimdragfactor,1)
1402         p(yasimliftfactor,1)
1403         p(max_power_rotor_brake,1000)
1404         p(rotorgear_friction,1000)
1405         p(engine_accel_limit,0.01)
1406         SG_LOG(SG_INPUT, SG_ALERT,
1407             "internal error in parameter set up for rotorgear: '"
1408             << parametername <<"'" << endl);
1409 #undef p
1410 }
1411 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1412 {
1413     if (j==0)
1414     {
1415         sprintf(text,"/rotors/gear/total-torque");
1416         *f=_total_torque_on_engine;
1417     } else return 0;
1418     return j+1;
1419 }
1420 Rotorgear::Rotorgear()
1421 {
1422     _in_use=0;
1423     _engineon=0;
1424     _rotorbrake=0;
1425     _max_power_rotor_brake=1;
1426     _rotorgear_friction=1;
1427     _max_power_engine=1000*450;
1428     _engine_prop_factor=0.05f;
1429     _yasimdragfactor=1;
1430     _yasimliftfactor=1;
1431     _ddt_omegarel=0;
1432     _engine_accel_limit=0.05f;
1433     _total_torque_on_engine=0;
1434 }
1435
1436 Rotorgear::~Rotorgear()
1437 {
1438     for(int i=0; i<_rotors.size(); i++)
1439         delete (Rotor*)_rotors.get(i);
1440 }
1441
1442 }; // namespace yasim