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