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