src/FDM/YASim/Rotor.cpp

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