src/FDM/YASim/Rotor.cpp

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