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 <iostream>
  12 #include <iomanip>
  13
  14 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(const 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::setRotorBalance(float lval)
 795 {
 796     lval = Math::clamp(lval, -1, 1);
 797     _balance2 = lval;
 798 }
 799
 800 void Rotor::getPosition(float* out)
 801 {
 802     int i;
 803     for(i=0; i<3; i++) out[i] = _base[i];
 804 }
 805
 806 void Rotor::calcLiftFactor(float* v, float rho, State *s)
 807 {
 808     //calculates _lift_factor, which is a foactor for the lift of the rotor
 809     //due to
 810     //- ground effect (_f_ge)
 811     //- vortex state (_f_vs)
 812     //- translational lift (_f_tl)
 813     _f_ge=1;
 814     _f_tl=1;
 815     _f_vs=1;
 816
 817     // Calculate ground effect
 818     _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
 819
 820     // Now calculate translational lift
 821     float v_vert = Math::dot3(v,_normal);
 822     float help[3];
 823     Math::cross3(v,_normal,help);
 824     float v_horiz = Math::mag3(help);
 825     _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
 826         *(_translift_maxfactor-1)+1)/_translift_maxfactor;
 827
 828     _lift_factor = _f_ge*_f_tl*_f_vs;
 829
 830     //store the gravity direction
 831     Glue::geodUp(s->pos, _grav_direction);
 832     s->velGlobalToLocal(_grav_direction, _grav_direction);
 833 }
 834
 835 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
 836 {
 837     _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
 838         _groundeffectpos[0],_groundeffectpos[1],
 839         _groundeffectpos[2],_groundeffectpos[3]);
 840     testForRotorGroundContact(ground_cb,s);
 841 }
 842
 843 void Rotor::testForRotorGroundContact(Ground * ground_cb,State *s)
 844 {
 845     int i;
 846     for (i=0;i<_num_ground_contact_pos;i++)
 847     {
 848         double pt[3],h;
 849         s->posLocalToGlobal(_ground_contact_pos[i], pt);
 850
 851         // Ask for the ground plane in the global coordinate system
 852         double global_ground[4];
 853         float global_vel[3];
 854         ground_cb->getGroundPlane(pt, global_ground, global_vel);
 855         // find h, the distance to the ground
 856         // The ground plane transformed to the local frame.
 857         float ground[4];
 858         s->planeGlobalToLocal(global_ground, ground);
 859
 860         h = ground[3] - Math::dot3(_ground_contact_pos[i], ground);
 861         // Now h is the distance from _ground_contact_pos[i] to ground
 862         if (h<0)
 863         {
 864             _balance1 -= (-h)/_diameter/_num_ground_contact_pos;
 865             _balance1 = (_balance1<-1)?-1:_balance1;
 866         }
 867     }
 868 }
 869 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
 870         float *pos0,float *pos1,float *pos2,float *pos3,
 871         int iteration,float a0,float a1,float a2,float a3)
 872 {
 873     float a[5];
 874     float *p[5],pos4[3];
 875     a[0]=a0;
 876     a[1]=a1;
 877     a[2]=a2;
 878     a[3]=a3;
 879     a[4]=-1;
 880     p[0]=pos0;
 881     p[1]=pos1;
 882     p[2]=pos2;
 883     p[3]=pos3;
 884     p[4]=pos4;
 885     Math::add3(p[0],p[2],p[4]);
 886     Math::mul3(0.5,p[4],p[4]);//the center
 887
 888     float mina=100*_diameter;
 889     float suma=0;
 890     for (int i=0;i<5;i++)
 891     {
 892         if (a[i]==-1)//in the first iteration,(iteration==0) no height is
 893                      //passed to this function, these missing values are
 894                      //marked by ==-1
 895         {
 896             double pt[3];
 897             s->posLocalToGlobal(p[i], pt);
 898
 899             // Ask for the ground plane in the global coordinate system
 900             double global_ground[4];
 901             float global_vel[3];
 902             ground_cb->getGroundPlane(pt, global_ground, global_vel);
 903             // find h, the distance to the ground
 904             // The ground plane transformed to the local frame.
 905             float ground[4];
 906             s->planeGlobalToLocal(global_ground, ground);
 907
 908             a[i] = ground[3] - Math::dot3(p[i], ground);
 909             // Now a[i] is the distance from p[i] to ground
 910         }
 911         suma+=a[i];
 912         if (a[i]<mina)
 913             mina=a[i];
 914     }
 915     if (mina>=10*_diameter)
 916         return mina; //the ground effect will be zero
 917
 918     //check if further recursion is neccessary
 919     //if the height does not differ more than 20%, than
 920     //we can return then mean height, if not split
 921     //zhe square to four parts and calcualte the height
 922     //for each part
 923     //suma * 0.2 is the mean
 924     //0.15 is the maximum allowed difference from the mean
 925     //to the height at the center
 926     if ((iteration>2)
 927        ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
 928         return suma*0.2;
 929     suma=0;
 930     float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
 931     for (int i=0;i<4;i++)
 932     {
 933         Math::add3(p[i],p[(i+1)&3],pc[i]);
 934         Math::mul3(0.5,pc[i],pc[i]);
 935         double pt[3];
 936         s->posLocalToGlobal(pc[i], pt);
 937
 938         // Ask for the ground plane in the global coordinate system
 939         double global_ground[4];
 940         float global_vel[3];
 941         ground_cb->getGroundPlane(pt, global_ground, global_vel);
 942         // find h, the distance to the ground
 943         // The ground plane transformed to the local frame.
 944         float ground[4];
 945         s->planeGlobalToLocal(global_ground, ground);
 946
 947         ac[i] = ground[3] - Math::dot3(p[i], ground);
 948         // Now ac[i] is the distance from pc[i] to ground
 949     }
 950     return 0.25*
 951         (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
 952             iteration+1,a[0],ac[0],a[4],ac[3])
 953         +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
 954             iteration+1,a[1],ac[0],a[4],ac[1])
 955         +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
 956             iteration+1,a[2],ac[1],a[4],ac[2])
 957         +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
 958             iteration+1,a[3],ac[2],a[4],ac[3])
 959         );
 960 }
 961
 962 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
 963 {
 964     float pos2rotor[3],tmp[3];
 965     Math::sub3(_base,pos,pos2rotor);
 966     float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 967     //calculate incidence at 0.7r;
 968     float inc = _collective+_twist *0.7
 969                 - _twist*_rel_len_where_incidence_is_measured;
 970     if (inc < 0)
 971         dist *=-1;
 972     if (dist<0) // we are not in the downwash region
 973     {
 974         downwash[0]=downwash[1]=downwash[2]=0.;
 975         return;
 976     }
 977
 978     //calculate the mean downwash speed directly beneath the rotor disk
 979     float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
 980     //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
 981     //0.8 the slip of the rotor.
 982
 983     //calculate the time the wind needed from thr rotor to here
 984     if (v1bar< 1) v1bar = 1;
 985     float time=dist/v1bar;
 986
 987     //calculate the pos2rotor, where the rotor was, "time" ago
 988     Math::mul3(time,v_heli,tmp);
 989     Math::sub3(pos2rotor,tmp,pos2rotor);
 990
 991     //and again calculate dist
 992     dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 993     //missing the normal is offen not pointing to the normal of the rotor
 994     //disk. Rotate the normal by yaw and tilt angle
 995
 996     if (inc < 0)
 997         dist *=-1;
 998     if (dist<0) // we are not in the downwash region
 999     {
1000         downwash[0]=downwash[1]=downwash[2]=0.;
1001         return;
1002     }
1003     //of course this could be done in a runge kutta integrator, but it's such
1004     //a approximation that I beleave, it would'nt be more realistic
1005
1006     //calculate the dist to the rotor-axis
1007     Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
1008     float r= Math::mag3(tmp);
1009     //calculate incidence at r;
1010     float rel_r = r *2 /_diameter;
1011     float inc_r = _collective+_twist * r /_diameter * 2
1012         - _twist*_rel_len_where_incidence_is_measured;
1013
1014     //calculate the downwash speed directly beneath the rotor disk
1015     float v1=0;
1016     if (rel_r<1)
1017         v1 = Math::sin(inc_r) *_omega * r * 0.8;
1018
1019     //calcualte the downwash speed in a distance "dist" to the rotor disc,
1020     //for large dist. The speed is assumed do follow a gausian distribution
1021     //with sigma increasing with dist^2:
1022     //sigma is assumed to be half of the rotor diameter directly beneath the
1023     //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
1024
1025     float sigma=_diameter/2 + dist * dist / _diameter /4.;
1026     float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
1027         * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
1028
1029     //calculate the weight of the two downwash velocities.
1030     //Directly beneath the disc it is v1, far away it is v2
1031     float g = Math::pow(2.7183,-2*dist/_diameter);
1032     //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
1033
1034     float v = g * v1 + (1-g) * v2;
1035     Math::mul3(-v*_downwash_factor,_normal_with_yaw_roll,downwash);
1036     //the downwash is calculated in the opposite direction of the normal
1037 }
1038
1039 void Rotor::euler2orient(float roll, float pitch, float hdg, float* out)
1040 {
1041     // the Glue::euler2orient, inverts y<z due to different bases
1042     // therefore the negation of all "y" and "z" coeffizients
1043     Glue::euler2orient(roll,pitch,hdg,out);
1044     for (int i=3;i<9;i++) out[i]*=-1.0;
1045 }
1046
1047
1048 void Rotor::updateDirectionsAndPositions(float *rot)
1049 {
1050     if (!_directions_and_postions_dirty)
1051     {
1052         rot[0]=rot[1]=rot[2]=0;
1053         return;
1054     }
1055     rot[0]=_old_tilt_roll-_tilt_roll;
1056     rot[1]=_old_tilt_pitch-_tilt_pitch;
1057     rot[2]=_old_tilt_yaw-_tilt_yaw;
1058     _old_tilt_roll=_tilt_roll;
1059     _old_tilt_pitch=_tilt_pitch;
1060     _old_tilt_yaw=_tilt_yaw;
1061     float orient[9];
1062     euler2orient(_tilt_roll, _tilt_pitch, _tilt_yaw, orient);
1063     float forward[3];
1064     float normal[3];
1065     float base[3];
1066     Math::sub3(_base,_tilt_center,base);
1067     Math::vmul33(orient, base, base);
1068     Math::add3(base,_tilt_center,base);
1069     Math::vmul33(orient, _forward, forward);
1070     Math::vmul33(orient, _normal, normal);
1071 #define _base base
1072 #define _forward forward
1073 #define _normal normal
1074     float directions[5][3];
1075     //pointing forward, right, ... the 5th is ony for calculation
1076     directions[0][0]=_forward[0];
1077     directions[0][1]=_forward[1];
1078     directions[0][2]=_forward[2];
1079     int i;
1080     for (i=1;i<5;i++)
1081     {
1082         if (!_ccw)
1083             Math::cross3(directions[i-1],_normal,directions[i]);
1084         else
1085             Math::cross3(_normal,directions[i-1],directions[i]);
1086     }
1087     Math::set3(directions[4],directions[0]);
1088     // now directions[0] is perpendicular to the _normal.and has a length
1089     // of 1. if _forward is already normalized and perpendicular to the
1090     // normal, directions[0] will be the same
1091     //_num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16;
1092     for (i=0;i<_num_ground_contact_pos;i++)
1093     {
1094         float help[3];
1095         float s = Math::sin(pi*2*i/_num_ground_contact_pos);
1096         float c = Math::cos(pi*2*i/_num_ground_contact_pos);
1097         Math::mul3(c*_diameter*0.5,directions[0],_ground_contact_pos[i]);
1098         Math::mul3(s*_diameter*0.5,directions[1],help);
1099         Math::add3(help,_ground_contact_pos[i],_ground_contact_pos[i]);
1100         Math::add3(_base,_ground_contact_pos[i],_ground_contact_pos[i]);
1101     }
1102     for (i=0;i<4;i++)
1103     {
1104         Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
1105         Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
1106     }
1107     for (i=0;i<_number_of_parts;i++)
1108     {
1109         Rotorpart* rp = getRotorpart(i);
1110         float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
1111         float s = Math::sin(2*pi*i/_number_of_parts);
1112         float c = Math::cos(2*pi*i/_number_of_parts);
1113         float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1114         float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1115         float direction[3],nextdirection[3],help[3],direction90deg[3];
1116         float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
1117         float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
1118         float lentocenter=_diameter*_rel_blade_center*0.5;
1119         float lentoforceattac=_diameter*_rel_len_hinge*0.5;
1120         float zentforce=rotorpartmass*speed*speed/lentocenter;
1121
1122         Math::mul3(c ,directions[0],help);
1123         Math::mul3(s ,directions[1],direction);
1124         Math::add3(help,direction,direction);
1125
1126         Math::mul3(c ,directions[1],help);
1127         Math::mul3(s ,directions[2],direction90deg);
1128         Math::add3(help,direction90deg,direction90deg);
1129
1130         Math::mul3(cp ,directions[1],help);
1131         Math::mul3(sp ,directions[2],nextdirection);
1132         Math::add3(help,nextdirection,nextdirection);
1133
1134         Math::mul3(lentocenter,direction,lpos);
1135         Math::add3(lpos,_base,lpos);
1136         Math::mul3(lentoforceattac,nextdirection,lforceattac);
1137         //nextdirection: +90deg (gyro)!!!
1138
1139         Math::add3(lforceattac,_base,lforceattac);
1140         Math::mul3(speed,direction90deg,lspeed);
1141         Math::mul3(1,nextdirection,dirzentforce);
1142         rp->setPosition(lpos);
1143         rp->setNormal(_normal);
1144         rp->setZentipetalForce(zentforce);
1145         rp->setPositionForceAttac(lforceattac);
1146         rp->setSpeed(lspeed);
1147         rp->setDirectionofZentipetalforce(dirzentforce);
1148         rp->setDirectionofRotorPart(direction);
1149     }
1150 #undef _base
1151 #undef _forward
1152 #undef _normal
1153     _directions_and_postions_dirty=false;
1154 }
1155
1156 void Rotor::compile()
1157 {
1158     // Have we already been compiled?
1159     if(_rotorparts.size() != 0) return;
1160
1161     //rotor is divided into _number_of_parts parts
1162     //each part is calcualted at _number_of_segments points
1163
1164     //clamp to 4..256
1165     //and make it a factor of 4
1166     _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
1167
1168     _dynamic=_dynamic*(1/                          //inverse of the time
1169         ( (60/_rotor_rpm)/4         //for rotating 90 deg
1170         +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
1171                                                //will pass a given point
1172         ));
1173     //normalize the directions
1174     Math::unit3(_forward,_forward);
1175     Math::unit3(_normal,_normal);
1176     _num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16;
1177     float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
1178     //was pounds -> now kg
1179
1180     _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
1181         * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
1182     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
1183     float lentocenter=_diameter*_rel_blade_center*0.5;
1184     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
1185     float zentforce=rotorpartmass*speed*speed/lentocenter;
1186     float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
1187     // was pounds of force, now N, devided by _number_of_parts
1188     //(so its now per rotorpart)
1189
1190     float torque0=0,torquemax=0,torqueb=0;
1191     float omega=_rotor_rpm/60*2*pi;
1192     _omegan=omega;
1193     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
1194     float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
1195     _delta*=delta_theoretical;
1196
1197     float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1198         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1199     float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega)
1200         +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor;
1201     _phi=Math::acos(_rel_len_hinge);
1202     _phi-=Math::atan(_delta3);
1203     if (!_no_torque)
1204     {
1205         torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
1206         // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
1207         torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
1208         torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
1209
1210         if(_ccw)
1211         {
1212             torque0*=-1;
1213             torquemax*=-1;
1214             torqueb*=-1;
1215         }
1216     }
1217
1218     Rotorpart* rps[256];
1219     int i;
1220     for (i=0;i<_number_of_parts;i++)
1221     {
1222         Rotorpart* rp=rps[i]=newRotorpart(zentforce,pitchaforce,_delta3,rotorpartmass,
1223             _translift,_rel_len_hinge,lentocenter);
1224         int k = i*4/_number_of_parts;
1225         rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
1226         rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
1227         _rotorparts.add(rp);
1228         rp->setTorque(torquemax,torque0);
1229         rp->setRelamp(relamp);
1230         rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
1231         rp->setDirection(2*pi*i/_number_of_parts);
1232     }
1233     for (i=0;i<_number_of_parts;i++)
1234     {
1235         rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
1236             rps[(i+1)%_number_of_parts],
1237             rps[(i+_number_of_parts/2)%_number_of_parts],
1238             rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
1239             rps[(i+_number_of_parts/4)%_number_of_parts]);
1240     }
1241     float drot[3];
1242     updateDirectionsAndPositions(drot);
1243     for (i=0;i<_number_of_parts;i++)
1244     {
1245         rps[i]->setCompiled();
1246     }
1247     float lift[4],torque[4], v_wind[3];
1248     v_wind[0]=v_wind[1]=v_wind[2]=0;
1249     rps[0]->setOmega(_omegan);
1250
1251     if (_airfoil_lift_coefficient==0)
1252     {
1253         //calculate the lift and drag coefficients now
1254         _dragcoef0=1;
1255         _dragcoef1=1;
1256         _liftcoef=1;
1257         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1258             &(torque[0]),&(lift[0])); //max_pitch a
1259         _liftcoef = pitchaforce/lift[0];
1260         _dragcoef0=1;
1261         _dragcoef1=0;
1262         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1263         //0 degree, c0
1264
1265         _dragcoef0=0;
1266         _dragcoef1=1;
1267         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1268         //0 degree, c1
1269
1270         _dragcoef0=1;
1271         _dragcoef1=0;
1272         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1273         //picth b, c0
1274
1275         _dragcoef0=0;
1276         _dragcoef1=1;
1277         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1278         //picth b, c1
1279
1280         if (torque[0]==0)
1281         {
1282             _dragcoef1=torque0/torque[1];
1283             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1284         }
1285         else
1286         {
1287             _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1288                 /(torque[1]/torque[0]-torque[3]/torque[2]);
1289             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1290         }
1291     }
1292     else
1293     {
1294         _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1295         _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1296         _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1297     }
1298
1299     //Check
1300     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1301         &(torque[0]),&(lift[0])); //pitch a
1302     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1303         &(torque[1]),&(lift[1])); //pitch b
1304     rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1305         &(torque[3]),&(lift[3])); //pitch 0
1306     SG_LOG(SG_GENERAL, SG_INFO,
1307         "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1308         << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1309         << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1310         << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1311         << endl
1312         << "at 10 deg:" << endl
1313         << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1314             *_number_of_parts/_number_of_blades/_c2
1315         << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1316         << endl
1317         << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1318         << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1319             << lift[3]*_number_of_parts << endl
1320         << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1321             << "kW " << lift[0]*_number_of_parts << endl
1322         << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1323             << "kW " << lift[1]*_number_of_parts << endl << endl );
1324
1325     //first calculation of relamp is wrong
1326     //it used pitchaforce, but this was unknown and
1327     //on the default value
1328     _delta*=lift[0]/pitchaforce;
1329     relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1330         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1331     for (i=0;i<_number_of_parts;i++)
1332     {
1333         rps[i]->setRelamp(relamp);
1334     }
1335     rps[0]->setOmega(0);
1336     setCollective(0);
1337     setCyclicail(0,0);
1338     setCyclicele(0,0);
1339
1340     writeInfo();
1341
1342     //tie the properties
1343     /* After reset these values are totally wrong. I have to find out why
1344     SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true);
1345     node->tie("balance_ext",SGRawValuePointer<float>(&_balance2),false);
1346     node->tie("balance_int",SGRawValuePointer<float>(&_balance1));
1347     _properties_tied=1;
1348     */
1349 }
1350 std::ostream &  operator<<(std::ostream & out, Rotor& r)
1351 {
1352 #define i(x) << #x << ":" << r.x << endl
1353 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1354     out << "Writing Info on Rotor "
1355     i(_name)
1356     i(_torque)
1357     i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1358     i (_chord)
1359     i( _taper)
1360     i( _airfoil_incidence_no_lift)
1361     i( _collective)
1362     i( _airfoil_lift_coefficient)
1363     i( _airfoil_drag_coefficient0)
1364     i( _airfoil_drag_coefficient1)
1365     i( _ccw)
1366     i( _number_of_segments)
1367     i( _number_of_parts)
1368     iv( _base)
1369     iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1370     i( _ground_effect_altitude)
1371     iv( _normal)
1372     iv( _normal_with_yaw_roll)
1373     iv( _forward)
1374     i( _diameter)
1375     i( _number_of_blades)
1376     i( _weight_per_blade)
1377     i( _rel_blade_center)
1378     i( _min_pitch)
1379     i( _max_pitch)
1380     i( _force_at_pitch_a)
1381     i( _pitch_a)
1382     i( _power_at_pitch_0)
1383     i( _power_at_pitch_b)
1384     i( _no_torque)
1385     i( _sim_blades)
1386     i( _pitch_b)
1387     i( _rotor_rpm)
1388     i( _rel_len_hinge)
1389     i( _maxcyclicail)
1390     i( _maxcyclicele)
1391     i( _mincyclicail)
1392     i( _mincyclicele)
1393     i( _delta3)
1394     i( _delta)
1395     i( _dynamic)
1396     i( _translift)
1397     i( _c2)
1398     i( _stepspersecond)
1399     i( _engineon)
1400     i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1401     i( _teeterdamp) i(_maxteeterdamp)
1402     i( _rellenteeterhinge)
1403     i( _translift_ve)
1404     i( _translift_maxfactor)
1405     i( _ground_effect_constant)
1406     i( _vortex_state_lift_factor)
1407     i( _vortex_state_c1)
1408     i( _vortex_state_c2)
1409     i( _vortex_state_c3)
1410     i( _vortex_state_e1)
1411     i( _vortex_state_e2)
1412     i( _vortex_state_e3)
1413     i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1414     i( _vortex_state)
1415     i( _liftcoef)
1416     i( _dragcoef0)
1417     i( _dragcoef1)
1418     i( _twist) //outer incidence = inner inner incidence + _twist
1419     i( _rel_len_where_incidence_is_measured)
1420     i( _torque_of_inertia)
1421     i( _rel_len_blade_start)
1422     i( _incidence_stall_zero_speed)
1423     i( _incidence_stall_half_sonic_speed)
1424     i( _lift_factor_stall)
1425     i( _stall_change_over)
1426     i( _drag_factor_stall)
1427     i( _stall_sum)
1428     i( _stall_v2sum)
1429     i( _yaw)
1430     i( _roll)
1431     i( _cyclicail)
1432     i( _cyclicele)
1433     i( _cyclic_factor) <<endl;
1434     int j;
1435     for(j=0; j<r._rotorparts.size(); j++) {
1436         out << *((Rotorpart*)r._rotorparts.get(j));
1437     }
1438     out <<endl << endl;
1439 #undef i
1440 #undef iv
1441     return out;
1442 }
1443 void Rotor:: writeInfo()
1444 {
1445 #ifdef TEST_DEBUG
1446     std::ostringstream buffer;
1447     buffer << *this;
1448     FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1449     if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1450     if (f)
1451     {
1452         fprintf(f,"%s",(const char *)buffer.str().c_str());
1453         fclose (f);
1454     }
1455 #endif
1456 }
1457 Rotorpart* Rotor::newRotorpart(float zentforce,float maxpitchforce,
1458     float delta3,float mass,float translift,float rellenhinge,float len)
1459 {
1460     Rotorpart *r = new Rotorpart();
1461     r->setDelta3(delta3);
1462     r->setDynamic(_dynamic);
1463     r->setTranslift(_translift);
1464     r->setC2(_c2);
1465     r->setWeight(mass);
1466     r->setRelLenHinge(rellenhinge);
1467     r->setSharedFlapHinge(_shared_flap_hinge);
1468     r->setOmegaN(_omegan);
1469     r->setPhi(_phi_null);
1470     r->setAlpha0(_alpha0);
1471     r->setAlphamin(_alphamin);
1472     r->setAlphamax(_alphamax);
1473     r->setAlpha0factor(_alpha0factor);
1474     r->setLen(len);
1475     r->setDiameter(_diameter);
1476     r->setRotor(this);
1477 #define p(a) r->setParameter(#a,_##a);
1478     p(twist)
1479     p(number_of_segments)
1480     p(rel_len_where_incidence_is_measured)
1481     p(rel_len_blade_start)
1482     p(rotor_correction_factor)
1483 #undef p
1484     return r;
1485 }
1486
1487 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1488 {
1489     out[0] = v1[0] + frac*(v2[0]-v1[0]);
1490     out[1] = v1[1] + frac*(v2[1]-v1[1]);
1491     out[2] = v1[2] + frac*(v2[2]-v1[2]);
1492 }
1493
1494 void Rotorgear::initRotorIteration(float *lrot,float dt)
1495 {
1496     int i;
1497     float omegarel;
1498     if (!_rotors.size()) return;
1499     Rotor* r0 = (Rotor*)_rotors.get(0);
1500     omegarel= r0->getOmegaRelNeu();
1501     for(i=0; i<_rotors.size(); i++) {
1502         Rotor* r = (Rotor*)_rotors.get(i);
1503         r->inititeration(dt,omegarel,0,lrot);
1504     }
1505 }
1506
1507 void Rotorgear::calcForces(float* torqueOut)
1508 {
1509     int i,j;
1510     torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1511     // check,<if the engine can handle the torque of the rotors.
1512     // If not reduce the torque to the fueselage and change rotational
1513     // speed of the rotors instead
1514     if (_rotors.size())
1515     {
1516         float omegarel,omegan;
1517         Rotor* r0 = (Rotor*)_rotors.get(0);
1518         omegarel= r0->getOmegaRel();
1519
1520         float total_torque_of_inertia=0;
1521         float total_torque=0;
1522         for(i=0; i<_rotors.size(); i++) {
1523             Rotor* r = (Rotor*)_rotors.get(i);
1524             omegan=r->getOmegan();
1525             total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1526             //FIXME: this is constant, so this can be done in compile
1527
1528             total_torque+=r->getTorque()*omegan;
1529         }
1530         float max_torque_of_engine=0;
1531         SGPropertyNode * node=fgGetNode("/rotors/gear", true);
1532         if (_engineon)
1533         {
1534             max_torque_of_engine=_max_power_engine*_max_rel_torque;
1535             float df=_target_rel_rpm-omegarel;
1536             df/=_engine_prop_factor;
1537             df = Math::clamp(df, 0, 1);
1538             max_torque_of_engine = df * _max_power_engine*_max_rel_torque;
1539         }
1540         total_torque*=-1;
1541         _ddt_omegarel=0;
1542         float rel_torque_engine=1;
1543         if (total_torque<=0)
1544             rel_torque_engine=0;
1545         else
1546             if (max_torque_of_engine>0)
1547                 rel_torque_engine=1/max_torque_of_engine*total_torque;
1548             else
1549                 rel_torque_engine=0;
1550
1551         //add the rotor brake and the gear fritcion
1552         float dt=0.1f;
1553         if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1554
1555         float rotor_brake_torque;
1556         rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1557         //clamp it to the value you need to stop the rotor
1558         //to avod accelerate the rotor to neagtive rpm:
1559         rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1560             total_torque_of_inertia/dt*omegarel);
1561         max_torque_of_engine-=rotor_brake_torque;
1562
1563         //change the rotation of the rotors
1564         if ((max_torque_of_engine<total_torque) //decreasing rotation
1565             ||((max_torque_of_engine>total_torque)&&(omegarel<_target_rel_rpm))
1566             //increasing rotation due to engine
1567             ||(total_torque<0) ) //increasing rotation due to autorotation
1568         {
1569             _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1570             if(max_torque_of_engine>total_torque)
1571             {
1572                 //check if the acceleration is due to the engine. If yes,
1573                 //the engine self limits the accel.
1574                 float lim1=-total_torque/total_torque_of_inertia;
1575                 //accel. by autorotation
1576
1577                 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1578                 //if the accel by autorotation greater than the max. engine
1579                 //accel, then this is the limit, if not: the engine is the limit
1580                 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1581             }
1582             if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1583             //clamp it to avoid overflow. Should never be reached
1584             if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1585
1586             if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1587             //for debug: negative or no maxpower will result
1588             //in permanet 100% rotation
1589
1590             omegarel+=dt*_ddt_omegarel;
1591
1592             if (omegarel>2.5) omegarel=2.5;
1593             //clamp it to avoid overflow. Should never be reached
1594             if (omegarel<-.5) omegarel=-.5;
1595
1596             r0->setOmegaRelNeu(omegarel);
1597             //calculate the torque, which is needed to accelerate the rotors.
1598             //Add this additional torque to the body
1599             for(j=0; j<_rotors.size(); j++) {
1600                 Rotor* r = (Rotor*)_rotors.get(j);
1601                 for(i=0; i<r->_rotorparts.size(); i++) {
1602                     float torque_scalar=0;
1603                     Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1604                     float torque[3];
1605                     rp->getAccelTorque(_ddt_omegarel,torque);
1606                     Math::add3(torque,torqueOut,torqueOut);
1607                 }
1608             }
1609         }
1610         _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1611     }
1612 }
1613
1614 void Rotorgear::addRotor(Rotor* rotor)
1615 {
1616     _rotors.add(rotor);
1617     _in_use = 1;
1618 }
1619
1620 void Rotorgear::compile()
1621 {
1622     float wgt = 0;
1623     for(int j=0; j<_rotors.size(); j++) {
1624         Rotor* r = (Rotor*)_rotors.get(j);
1625         r->compile();
1626     }
1627 }
1628
1629 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1630 {
1631     float tmp[3];
1632     downwash[0]=downwash[1]=downwash[2]=0;
1633     for(int i=0; i<_rotors.size(); i++) {
1634         Rotor* ro = (Rotor*)_rotors.get(i);
1635         ro->getDownWash(pos,v_heli,tmp);
1636         Math::add3(downwash,tmp,downwash);    //  + downwash
1637     }
1638 }
1639
1640 void Rotorgear::setParameter(char *parametername, float value)
1641 {
1642 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1643         p(max_power_engine,1000)
1644         p(engine_prop_factor,1)
1645         p(yasimdragfactor,1)
1646         p(yasimliftfactor,1)
1647         p(max_power_rotor_brake,1000)
1648         p(rotorgear_friction,1000)
1649         p(engine_accel_limit,0.01)
1650         SG_LOG(SG_INPUT, SG_ALERT,
1651             "internal error in parameter set up for rotorgear: '"
1652             << parametername <<"'" << endl);
1653 #undef p
1654 }
1655 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1656 {
1657     if (j==0)
1658     {
1659         sprintf(text,"/rotors/gear/total-torque");
1660         *f=_total_torque_on_engine;
1661     } else return 0;
1662     return j+1;
1663 }
1664 Rotorgear::Rotorgear()
1665 {
1666     _in_use=0;
1667     _engineon=0;
1668     _rotorbrake=0;
1669     _max_power_rotor_brake=1;
1670     _rotorgear_friction=1;
1671     _max_power_engine=1000*450;
1672     _engine_prop_factor=0.05f;
1673     _yasimdragfactor=1;
1674     _yasimliftfactor=1;
1675     _ddt_omegarel=0;
1676     _engine_accel_limit=0.05f;
1677     _total_torque_on_engine=0;
1678     _target_rel_rpm=1;
1679     _max_rel_torque=1;
1680 }
1681
1682 Rotorgear::~Rotorgear()
1683 {
1684     for(int i=0; i<_rotors.size(); i++)
1685         delete (Rotor*)_rotors.get(i);
1686 }
1687
1688 }; // namespace yasim