src/FDM/YASim/Rotor.cpp

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