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