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