src/FDM/YASim/Rotor.cpp

   1 #include "Math.hpp"
   2 #include "Surface.hpp"
   3 #include "Rotorpart.hpp"
   4 #include "Rotorblade.hpp"
   5 #include "Rotor.hpp"
   6 #include <stdio.h>
   7 //#include <string.h>
   8 namespace yasim {
   9
  10 const float pi=3.14159;
  11
  12 Rotor::Rotor()
  13 {
  14     _alpha0=-.05;
  15     _alpha0factor=1;
  16     _alphamin=-.1;
  17     _alphamax= .1;
  18     _alphaoutput[0][0]=0;
  19     _alphaoutput[1][0]=0;
  20     _alphaoutput[2][0]=0;
  21     _alphaoutput[3][0]=0;
  22     _alphaoutput[4][0]=0;
  23     _alphaoutput[5][0]=0;
  24     _alphaoutput[6][0]=0;
  25     _alphaoutput[7][0]=0;
  26     _base[0] = _base[1] = _base[2] = 0;
  27     _ccw=0;
  28     _delta=1;
  29     _delta3=0;
  30     _diameter =10;
  31     _dynamic=1;
  32     _engineon=0;
  33     _force_at_max_pitch=0;
  34     _force_at_pitch_a=0;
  35     _forward[0]=1;
  36     _forward[1]=_forward[2]=0;
  37     _max_pitch=13./180*pi;
  38     _maxcyclicail=10./180*pi;
  39     _maxcyclicele=10./180*pi;
  40     _maxteeterdamp=0;
  41     _mincyclicail=-10./180*pi;
  42     _mincyclicele=-10./180*pi;
  43     _min_pitch=-.5/180*pi;
  44     _name[0] = 0;
  45     _normal[0] = _normal[1] = 0;
  46     _normal[2] = 1;
  47     _number_of_blades=4;
  48     _omega=_omegan=_omegarel=0;
  49     _pitch_a=0;
  50     _pitch_b=0;
  51     _power_at_pitch_0=0;
  52     _power_at_pitch_b=0;
  53     _rel_blade_center=.7;
  54     _rel_len_hinge=0.01;
  55     _rellenteeterhinge=0.01;
  56     _rotor_rpm=442;
  57     _sim_blades=0;
  58     _teeterdamp=0.00001;
  59     _translift=0.05;
  60     _weight_per_blade=42;
  61
  62
  63
  64 }
  65
  66 Rotor::~Rotor()
  67 {
  68     int i;
  69     for(i=0; i<_rotorparts.size(); i++) {
  70         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
  71         delete r;
  72     }
  73     for(i=0; i<_rotorblades.size(); i++) {
  74         Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
  75         delete r;
  76     }
  77
  78 }
  79
  80 void Rotor::inititeration(float dt)
  81 {
  82    if ((_engineon)&&(_omegarel>=1)) return;
  83    if ((!_engineon)&&(_omegarel<=0)) return;
  84    _omegarel+=dt*1/5.*(_engineon?1:-1); //hier 30
  85    _omegarel=Math::clamp(_omegarel,0,1);
  86    _omega=_omegan*_omegarel;
  87    int i;
  88     for(i=0; i<_rotorparts.size(); i++) {
  89         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
  90         r->setOmega(_omega);
  91     }
  92     for(i=0; i<_rotorblades.size(); i++) {
  93         Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
  94         r->setOmega(_omega);
  95     }
  96 }
  97
  98 int Rotor::getValueforFGSet(int j,char *text,float *f)
  99 {
 100   if (_name[0]==0) return 0;
 101
 102
 103   if (_sim_blades)
 104   {
 105      if (!numRotorblades()) return 0;
 106      if (j==0)
 107      {
 108         sprintf(text,"/rotors/%s/cone", _name);
 109
 110         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
 111             +((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
 112             +((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
 113             +((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
 114            )/4*180/pi;
 115
 116      }
 117      else
 118      if (j==1)
 119      {
 120         sprintf(text,"/rotors/%s/roll", _name);
 121
 122         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
 123             -((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
 124            )/2*180/pi;
 125      }
 126      else
 127      if (j==2)
 128      {
 129         sprintf(text,"/rotors/%s/yaw", _name);
 130
 131         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
 132             -((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
 133            )/2*180/pi;
 134      }
 135      else
 136      if (j==3)
 137      {
 138         sprintf(text,"/rotors/%s/rpm", _name);
 139
 140         *f=_omega/2/pi*60;
 141      }
 142      else
 143      {
 144
 145          int b=(j-4)/3;
 146
 147          if (b>=numRotorblades()) return 0;
 148          int w=j%3;
 149          sprintf(text,"/rotors/%s/blade%i_%s",
 150             _name,b+1,
 151             w==0?"pos":(w==1?"flap":"incidence"));
 152          if (w==0) *f=((Rotorblade*)getRotorblade(b))->getPhi()*180/pi;
 153          else if (w==1) *f=((Rotorblade*) getRotorblade(b))->getrealAlpha()*180/pi;
 154          else *f=((Rotorblade*)getRotorblade(b))->getIncidence()*180/pi;
 155      }
 156      return j+1;
 157   }
 158   else
 159   {
 160      if (4!=numRotorparts()) return 0; //compile first!
 161      if (j==0)
 162      {
 163         sprintf(text,"/rotors/%s/cone", _name);
 164         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 165             +((Rotorpart*)getRotorpart(1))->getrealAlpha()
 166             +((Rotorpart*)getRotorpart(2))->getrealAlpha()
 167             +((Rotorpart*)getRotorpart(3))->getrealAlpha()
 168            )/4*180/pi;
 169      }
 170      else
 171      if (j==1)
 172      {
 173         sprintf(text,"/rotors/%s/roll", _name);
 174         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 175             -((Rotorpart*)getRotorpart(2))->getrealAlpha()
 176            )/2*180/pi*(_ccw?-1:1);
 177      }
 178      else
 179      if (j==2)
 180      {
 181         sprintf(text,"/rotors/%s/yaw", _name);
 182         *f=( ((Rotorpart*)getRotorpart(1))->getrealAlpha()
 183             -((Rotorpart*)getRotorpart(3))->getrealAlpha()
 184            )/2*180/pi;
 185      }
 186      else
 187      if (j==3)
 188      {
 189         sprintf(text,"/rotors/%s/rpm", _name);
 190
 191         *f=_omega/2/pi*60;
 192      }
 193      else
 194      {
 195        int b=(j-4)/3;
 196        if (b>=_number_of_blades) return 0;
 197        int w=j%3;
 198        sprintf(text,"/rotors/%s/blade%i_%s",
 199             _name,b+1,
 200           w==0?"pos":(w==1?"flap":"incidence"));
 201        *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi+360*b/_number_of_blades*(_ccw?1:-1);
 202        if (*f>360) *f-=360;
 203        if (*f<0) *f+=360;
 204        int k,l;
 205        float rk,rl,p;
 206        p=(*f/90);
 207        k=int(p);
 208        l=int(p+1);
 209        rk=l-p;
 210        rl=1-rk;
 211        /*
 212        rl=Math::sqr(Math::sin(rl*pi/2));
 213        rk=Math::sqr(Math::sin(rk*pi/2));
 214        */
 215        if(w==2) {k+=2;l+=2;}
 216        else
 217        if(w==1) {k+=1;l+=1;}
 218        k%=4;
 219        l%=4;
 220        if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi
 221                    +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi;
 222        else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi
 223                        +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi;
 224      }
 225      return j+1;
 226   }
 227
 228 }
 229 void Rotor::setEngineOn(int value)
 230 {
 231   _engineon=value;
 232 }
 233
 234 void Rotor::setAlpha0(float f)
 235 {
 236     _alpha0=f;
 237 }
 238 void Rotor::setAlphamin(float f)
 239 {
 240     _alphamin=f;
 241 }
 242 void Rotor::setAlphamax(float f)
 243 {
 244     _alphamax=f;
 245 }
 246 void Rotor::setAlpha0factor(float f)
 247 {
 248     _alpha0factor=f;
 249 }
 250
 251
 252 int Rotor::numRotorparts()
 253 {
 254     return _rotorparts.size();
 255 }
 256
 257 Rotorpart* Rotor::getRotorpart(int n)
 258 {
 259     return ((Rotorpart*)_rotorparts.get(n));
 260 }
 261 int Rotor::numRotorblades()
 262 {
 263     return _rotorblades.size();
 264 }
 265
 266 Rotorblade* Rotor::getRotorblade(int n)
 267 {
 268     return ((Rotorblade*)_rotorblades.get(n));
 269 }
 270
 271 void Rotor::strncpy(char *dest,const char *src,int maxlen)
 272 {
 273   int n=0;
 274   while(src[n]&&n<(maxlen-1))
 275   {
 276     dest[n]=src[n];
 277     n++;
 278   }
 279   dest[n]=0;
 280 }
 281
 282
 283
 284 void Rotor::setNormal(float* normal)
 285 {
 286     int i;
 287     float invsum,sqrsum=0;
 288     for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
 289
 290     if (sqrsum!=0)
 291       invsum=1/Math::sqrt(sqrsum);
 292     else
 293       invsum=1;
 294     for(i=0; i<3; i++) { _normal[i] = normal[i]*invsum; }
 295 }
 296
 297 void Rotor::setForward(float* forward)
 298 {
 299     int i;
 300     float invsum,sqrsum=0;
 301     for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
 302
 303     if (sqrsum!=0)
 304       invsum=1/Math::sqrt(sqrsum);
 305     else
 306       invsum=1;
 307     for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
 308 }
 309
 310
 311 void Rotor::setForceAtPitchA(float force)
 312 {
 313      _force_at_pitch_a=force;
 314 }
 315 void Rotor::setPowerAtPitch0(float value)
 316 {
 317      _power_at_pitch_0=value;
 318 }
 319 void Rotor::setPowerAtPitchB(float value)
 320 {
 321      _power_at_pitch_b=value;
 322 }
 323 void Rotor::setPitchA(float value)
 324 {
 325      _pitch_a=value/180*pi;
 326 }
 327 void Rotor::setPitchB(float value)
 328 {
 329      _pitch_b=value/180*pi;
 330 }
 331 void Rotor::setBase(float* base)
 332 {
 333     int i;
 334     for(i=0; i<3; i++) _base[i] = base[i];
 335 }
 336
 337
 338 void Rotor::setMaxCyclicail(float value)
 339 {
 340   _maxcyclicail=value/180*pi;
 341 }
 342 void Rotor::setMaxCyclicele(float value)
 343 {
 344   _maxcyclicele=value/180*pi;
 345 }
 346 void Rotor::setMinCyclicail(float value)
 347 {
 348   _mincyclicail=value/180*pi;
 349 }
 350 void Rotor::setMinCyclicele(float value)
 351 {
 352   _mincyclicele=value/180*pi;
 353 }
 354 void Rotor::setMinCollective(float value)
 355 {
 356   _min_pitch=value/180*pi;
 357 }
 358 void Rotor::setMaxCollective(float value)
 359 {
 360   _max_pitch=value/180*pi;
 361 }
 362 void Rotor::setDiameter(float value)
 363 {
 364   _diameter=value;
 365 }
 366 void Rotor::setWeightPerBlade(float value)
 367 {
 368   _weight_per_blade=value;
 369 }
 370 void Rotor::setNumberOfBlades(float value)
 371 {
 372   _number_of_blades=int(value+.5);
 373 }
 374 void Rotor::setRelBladeCenter(float value)
 375 {
 376   _rel_blade_center=value;
 377 }
 378 void Rotor::setDynamic(float value)
 379 {
 380   _dynamic=value;
 381 }
 382 void Rotor::setDelta3(float value)
 383 {
 384   _delta3=value;
 385 }
 386 void Rotor::setDelta(float value)
 387 {
 388   _delta=value;
 389 }
 390 void Rotor::setTranslift(float value)
 391 {
 392   _translift=value;
 393 }
 394 void Rotor::setC2(float value)
 395 {
 396   _c2=value;
 397 }
 398 void Rotor::setStepspersecond(float steps)
 399 {
 400   _stepspersecond=steps;
 401 }
 402 void Rotor::setRPM(float value)
 403 {
 404   _rotor_rpm=value;
 405 }
 406 void Rotor::setRelLenHinge(float value)
 407 {
 408   _rel_len_hinge=value;
 409 }
 410
 411 void Rotor::setAlphaoutput(int i, const char *text)
 412 {
 413   //printf("SetAlphaoutput %i [%s]\n",i,text);
 414   strncpy(_alphaoutput[i],text,255);
 415 }
 416 void Rotor::setName(const char *text)
 417 {
 418   strncpy(_name,text,128);//128: some space needed for settings
 419 }
 420
 421 void Rotor::setCcw(int ccw)
 422 {
 423      _ccw=ccw;
 424 }
 425 void Rotor::setNotorque(int value)
 426 {
 427    _no_torque=value;
 428 }
 429 void Rotor::setSimBlades(int value)
 430 {
 431    _sim_blades=value;
 432 }
 433
 434 void Rotor::setRelLenTeeterHinge(float f)
 435 {
 436    _rellenteeterhinge=f;
 437 }
 438 void Rotor::setTeeterdamp(float f)
 439 {
 440     _teeterdamp=f;
 441 }
 442 void Rotor::setMaxteeterdamp(float f)
 443 {
 444     _maxteeterdamp=f;
 445 }
 446
 447
 448
 449
 450 void Rotor::setCollective(float lval)
 451 {
 452     lval = Math::clamp(lval, -1, 1);
 453     int i;
 454     //printf("col: %5.3f\n",lval);
 455     for(i=0; i<_rotorparts.size(); i++) {
 456         ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
 457
 458     }
 459     float col=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
 460     for(i=0; i<_rotorblades.size(); i++) {
 461         ((Rotorblade*)_rotorblades.get(i))->setCollective(col);
 462
 463     }
 464 }
 465 void Rotor::setCyclicele(float lval,float rval)
 466 {
 467     rval = Math::clamp(rval, -1, 1);
 468     lval = Math::clamp(lval, -1, 1);
 469     float col=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
 470     int i;
 471     for(i=0; i<_rotorblades.size(); i++) {
 472         //((Rotorblade*)_rotorblades.get(i))->setCyclicele(col*Math::sin(((Rotorblade*)_rotorblades.get(i))->getPhi()));
 473         ((Rotorblade*)_rotorblades.get(i))->setCyclicele(col);
 474     }
 475     if (_rotorparts.size()!=4) return;
 476     //printf("                     ele: %5.3f  %5.3f\n",lval,rval);
 477     ((Rotorpart*)_rotorparts.get(1))->setCyclic(lval);
 478     ((Rotorpart*)_rotorparts.get(3))->setCyclic(-lval);
 479 }
 480 void Rotor::setCyclicail(float lval,float rval)
 481 {
 482     lval = Math::clamp(lval, -1, 1);
 483     rval = Math::clamp(rval, -1, 1);
 484     float col=_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail);
 485     int i;
 486     for(i=0; i<_rotorblades.size(); i++) {
 487         ((Rotorblade*)_rotorblades.get(i))->setCyclicail(col);
 488     }
 489     if (_rotorparts.size()!=4) return;
 490     //printf("ail: %5.3f %5.3f\n",lval,rval);
 491     if (_ccw) lval *=-1;
 492     ((Rotorpart*)_rotorparts.get(0))->setCyclic(-lval);
 493     ((Rotorpart*)_rotorparts.get(2))->setCyclic( lval);
 494 }
 495
 496
 497 float Rotor::getGroundEffect(float* posOut)
 498 {
 499     /*
 500     int i;
 501     for(i=0; i<3; i++) posOut[i] = _base[i];
 502     float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
 503     span = 2*(span + Math::abs(_base[2]));
 504     */
 505     return _diameter;
 506 }
 507
 508 void Rotor::compile()
 509 {
 510   // Have we already been compiled?
 511   if(_rotorparts.size() != 0) return;
 512
 513   //rotor is divided into 4 pointlike parts
 514
 515   printf("debug: e %f...%f a%f...%f %f...%f\n",
 516       _mincyclicele,_maxcyclicele,
 517       _mincyclicail,_maxcyclicail,
 518       _min_pitch,_max_pitch);
 519
 520   if(!_sim_blades)
 521   {
 522     _dynamic=_dynamic*(1/                          //inverse of the time
 523              ( (60/_rotor_rpm)/4         //for rotating 90 deg
 524               +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade will pass a given point
 525              ));
 526     float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
 527     directions[0][0]=_forward[0];
 528     directions[0][1]=_forward[1];
 529     directions[0][2]=_forward[2];
 530     int i;
 531     printf("Rotor rotating ccw? %i\n",_ccw);
 532     for (i=1;i<5;i++)
 533
 534     {
 535          if (!_ccw)
 536            Math::cross3(directions[i-1],_normal,directions[i]);
 537          else
 538            Math::cross3(_normal,directions[i-1],directions[i]);
 539          Math::unit3(directions[i],directions[i]);
 540     }
 541     Math::set3(directions[4],directions[0]);
 542     float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;//was pounds -> now kg
 543     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 544     float lentocenter=_diameter*_rel_blade_center*0.5;
 545     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 546     float zentforce=rotorpartmass*speed*speed/lentocenter;
 547     _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
 548     float maxpitchforce=_force_at_max_pitch/4*.453*9.81;//was pounds of force, now N
 549     float torque0=0,torquemax=0;
 550     float omega=_rotor_rpm/60*2*pi;
 551     _omegan=omega;
 552     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 553     //float omega0=omega*Math::sqrt((1-_rel_len_hinge));
 554     //_delta=omega*_delta;
 555     _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*rotorpartmass);
 556
 557     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 558     //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta));
 559     float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
 560     if (!_no_torque)
 561     {
 562        torque0=_power_at_pitch_0/4*1000/omega;
 563        torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch;
 564
 565        if(_ccw)
 566        {
 567          torque0*=-1;
 568          torquemax*=-1;
 569        }
 570
 571     }
 572
 573     printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
 574            "zf: %5.3f mpf: %5.3f\n"
 575            "tq: %5.3f..%5.3f d3:%5.3f\n"
 576            "o/o0: %5.3f phi: %5.3f relamp: %5.3f delta:%5.3f\n"
 577            ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
 578            lentoforceattac,zentforce,maxpitchforce,
 579            torque0,torquemax,_delta3,
 580            omega/omega0,phi*180/pi,relamp,_delta);
 581
 582     Rotorpart* rps[4];
 583     for (i=0;i<4;i++)
 584     {
 585          float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
 586
 587          Math::mul3(lentocenter,directions[i],lpos);
 588          Math::add3(lpos,_base,lpos);
 589          Math::mul3(lentoforceattac,directions[i+1],lforceattac);//i+1: +90deg (gyro)!!!
 590          Math::add3(lforceattac,_base,lforceattac);
 591          Math::mul3(speed,directions[i+1],lspeed);
 592          Math::mul3(1,directions[i+1],dirzentforce);
 593
 594          float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
 595          float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
 596
 597
 598          Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
 599                        lspeed,dirzentforce,zentforce,maxpitchforce, _max_pitch,_min_pitch,mincyclic,maxcyclic,
 600                        _delta3,rotorpartmass,_translift,_rel_len_hinge,lentocenter);
 601          rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
 602          rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
 603          _rotorparts.add(rp);
 604          rp->setTorque(torquemax,torque0);
 605          rp->setRelamp(relamp);
 606
 607
 608     }
 609     for (i=0;i<4;i++)
 610     {
 611
 612       rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]);
 613     }
 614   }
 615   else
 616   {
 617     float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
 618     directions[0][0]=_forward[0];
 619     directions[0][1]=_forward[1];
 620     directions[0][2]=_forward[2];
 621     int i;
 622     printf("Rotor rotating ccw? %i\n",_ccw);
 623     for (i=1;i<5;i++)
 624
 625     {
 626          //if (!_ccw)
 627            Math::cross3(directions[i-1],_normal,directions[i]);
 628          //else
 629          //  Math::cross3(_normal,directions[i-1],directions[i]);
 630          Math::unit3(directions[i],directions[i]);
 631     }
 632     Math::set3(directions[4],directions[0]);
 633     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 634     float lentocenter=_diameter*_rel_blade_center*0.5;
 635     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 636     float zentforce=_weight_per_blade*.453*speed*speed/lentocenter;
 637     _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
 638     float maxpitchforce=_force_at_max_pitch/_number_of_blades*.453*9.81;//was pounds of force, now N
 639     float torque0=0,torquemax=0;
 640     float omega=_rotor_rpm/60*2*pi;
 641     _omegan=omega;
 642     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 643     //float omega0=omega*Math::sqrt(1-_rel_len_hinge);
 644     //_delta=omega*_delta;
 645     _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453);
 646     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 647     float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega));
 648     float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
 649     if (!_no_torque)
 650     {
 651        torque0=_power_at_pitch_0/_number_of_blades*1000/omega;
 652        torquemax=_power_at_pitch_b/_number_of_blades*1000/omega/_pitch_b*_max_pitch;
 653
 654        if(_ccw)
 655        {
 656          torque0*=-1;
 657          torquemax*=-1;
 658        }
 659
 660     }
 661
 662     printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
 663            "zf: %5.3f mpf: %5.3f\n"
 664            "tq: %5.3f..%5.3f d3:%5.3f\n"
 665            "o/o0: %5.3f phi: %5.3f relamp:%5.3f delta:%5.3f\n"
 666            ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
 667            lentoforceattac,zentforce,maxpitchforce,
 668            torque0,torquemax,_delta3,
 669            omega/omega0,float(phi*180/pi),relamp,_delta);
 670
 671     float lspeed[3],dirzentforce[3];
 672
 673     float f=(!_ccw)?1:-1;
 674     //Math::mul3(f*speed,directions[1],lspeed);
 675     Math::mul3(f,directions[1],dirzentforce);
 676     for (i=0;i<_number_of_blades;i++)
 677     {
 678
 679
 680
 681
 682          Rotorblade* rb=newRotorblade(_base,_normal,directions[0],directions[1],
 683                        lentoforceattac,_rel_len_hinge,
 684                        dirzentforce,zentforce,maxpitchforce, _max_pitch,
 685                        _delta3,_weight_per_blade*.453,_translift,2*pi/_number_of_blades*i,
 686                        omega,lentocenter,/*f* */speed);
 687          //rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
 688          //rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
 689          _rotorblades.add(rb);
 690          rb->setTorque(torquemax,torque0);
 691          rb->setDeltaPhi(pi/2.-phi);
 692          rb->setDelta(_delta);
 693
 694          rb->calcFrontRight();
 695
 696     }
 697     /*
 698     for (i=0;i<4;i++)
 699     {
 700
 701       rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]);
 702     }
 703     */
 704
 705   }
 706 }
 707
 708
 709 Rotorblade* Rotor::newRotorblade(float* pos,  float *normal, float *front, float *right,
 710          float lforceattac,float rellenhinge,
 711          float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch,
 712          float delta3,float mass,float translift,float phi,float omega,float len,float speed)
 713 {
 714     Rotorblade *r = new Rotorblade();
 715     r->setPosition(pos);
 716     r->setNormal(normal);
 717     r->setFront(front);
 718     r->setRight(right);
 719     r->setMaxPitchForce(maxpitchforce);
 720     r->setZentipetalForce(zentforce);
 721     r->setAlpha0(_alpha0);
 722     r->setAlphamin(_alphamin);
 723     r->setAlphamax(_alphamax);
 724     r->setAlpha0factor(_alpha0factor);
 725
 726
 727
 728     r->setSpeed(speed);
 729     r->setDirectionofZentipetalforce(dirzentforce);
 730     r->setMaxpitch(maxpitch);
 731     r->setDelta3(delta3);
 732     r->setDynamic(_dynamic);
 733     r->setTranslift(_translift);
 734     r->setC2(_c2);
 735     r->setStepspersecond(_stepspersecond);
 736     r->setWeight(mass);
 737     r->setOmegaN(omega);
 738     r->setPhi(phi);
 739     r->setLforceattac(lforceattac);
 740     r->setLen(len);
 741     r->setLenHinge(rellenhinge);
 742     r->setRelLenTeeterHinge(_rellenteeterhinge);
 743     r->setTeeterdamp(_teeterdamp);
 744     r->setMaxteeterdamp(_maxteeterdamp);
 745
 746     /*
 747     #define a(x) x[0],x[1],x[2]
 748     printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
 749            "       nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
 750            "       dzf(%5.3f %5.3f %5.3f) zf  (%5.3f) mpf (%5.3f)\n"
 751            "       pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
 752            ,a(pos),a(posforceattac),a(normal),
 753           a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
 754           delta3);
 755     #undef a
 756     */
 757     return r;
 758 }
 759
 760 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
 761          float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
 762          float maxpitch, float minpitch, float mincyclic,float maxcyclic,
 763          float delta3,float mass,float translift,float rellenhinge,float len)
 764 {
 765     Rotorpart *r = new Rotorpart();
 766     r->setPosition(pos);
 767     r->setNormal(normal);
 768     r->setMaxPitchForce(maxpitchforce);
 769     r->setZentipetalForce(zentforce);
 770
 771     r->setPositionForceAttac(posforceattac);
 772
 773     r->setSpeed(speed);
 774     r->setDirectionofZentipetalforce(dirzentforce);
 775     r->setMaxpitch(maxpitch);
 776     r->setMinpitch(minpitch);
 777     r->setMaxcyclic(maxcyclic);
 778     r->setMincyclic(mincyclic);
 779     r->setDelta3(delta3);
 780     r->setDynamic(_dynamic);
 781     r->setTranslift(_translift);
 782     r->setC2(_c2);
 783     r->setWeight(mass);
 784     r->setRelLenHinge(rellenhinge);
 785     r->setOmegaN(_omegan);
 786     r->setAlpha0(_alpha0);
 787     r->setAlphamin(_alphamin);
 788     r->setAlphamax(_alphamax);
 789     r->setAlpha0factor(_alpha0factor);
 790     r->setLen(len);
 791
 792
 793
 794     #define a(x) x[0],x[1],x[2]
 795     printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
 796            "       nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
 797            "       dzf(%5.3f %5.3f %5.3f) zf  (%5.3f) mpf (%5.3f)\n"
 798            "       pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
 799            ,a(pos),a(posforceattac),a(normal),
 800           a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
 801           delta3);
 802     #undef a
 803
 804     return r;
 805 }
 806 void Rotor::interp(float* v1, float* v2, float frac, float* out)
 807 {
 808     out[0] = v1[0] + frac*(v2[0]-v1[0]);
 809     out[1] = v1[1] + frac*(v2[1]-v1[1]);
 810     out[2] = v1[2] + frac*(v2[2]-v1[2]);
 811 }
 812
 813 }; // namespace yasim
 814