src/FDM/YASim/Rotor.cpp

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