src/FDM/YASim/Rotor.cpp

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