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