src/FDM/YASim/Hitch.cpp

   1 #include "Math.hpp"
   2 #include "BodyEnvironment.hpp"
   3 #include "RigidBody.hpp"
   4 #include <Main/fg_props.hxx>
   5 #include <string.h>
   6
   7
   8
   9 #include "Hitch.hpp"
  10 namespace yasim {
  11 Hitch::Hitch(const char *name)
  12 {
  13     int i;
  14     strncpy(_name,name,128);
  15     _name[127]=0;
  16     for(i=0; i<3; i++)
  17         _pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0;
  18     for(i=0; i<2; i++)
  19         _global_ground[i] = 0;
  20     _global_ground[2] = 1;
  21     _global_ground[3] = -1e5;
  22     _forceMagnitude=0;
  23     _open=true;
  24     _oldOpen=_open;
  25     _towLength=60;
  26     _towElasticConstant=1e5;
  27     _towBrakeForce=100000;
  28     _towWeightPerM=1;
  29     _winchMaxSpeed=40;
  30     _winchRelSpeed=0;
  31     _winchInitialTowLength=1000;
  32     _winchPower=100000;
  33     _winchMaxForce=10000;
  34     _winchActualForce=0;
  35     _winchMaxTowLength=1000;
  36     _winchMinTowLength=0;
  37     _dist=0;
  38     _towEndIsConnectedToProperty=false;
  39     _towEndNode=0;
  40     _nodeIsMultiplayer=false;
  41     _nodeIsAiAircraft=false;
  42     _forceIsCalculatedByMaster=false;
  43     _nodeID=0;
  44     //_ai_MP_callsign=0;
  45     _height_above_ground=0;
  46     _winch_height_above_ground=0;
  47     _loPosFrac=0;
  48     _lowest_tow_height=0;
  49     _state=new State;
  50     _displayed_len_lower_dist_message=false;
  51     _last_wish=true;
  52     _isSlave=false;
  53     _mpAutoConnectPeriod=0;
  54     _timeToNextAutoConnectTry=0;
  55     _timeToNextReConnectTry=10;
  56     _speed_in_tow_direction=0;
  57     _mp_time_lag=1;
  58     _mp_last_reported_dist=0;
  59     _mp_last_reported_v=0;
  60     _mp_is_slave=false;
  61     _mp_open_last_state=false;
  62     _timeLagCorrectedDist=0;
  63
  64     //tie the properties
  65     char text[128];
  66     sprintf(text,"/sim/hitches/%s", _name);
  67     SGPropertyNode * node = fgGetNode(text, true);
  68     node->tie("tow/length",SGRawValuePointer<float>(&_towLength));
  69     node->tie("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
  70     node->tie("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
  71     node->tie("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
  72     node->tie("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
  73     node->tie("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
  74     node->tie("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
  75     node->tie("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
  76     node->tie("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
  77     node->tie("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
  78     node->tie("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
  79     node->tie("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
  80     node->tie("winch/max-power",SGRawValuePointer<float>(&_winchPower));
  81     node->tie("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
  82     node->tie("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
  83     node->tie("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
  84     node->tie("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
  85     node->tie("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
  86     node->tie("force",SGRawValuePointer<float>(&_forceMagnitude));
  87     node->tie("open",SGRawValuePointer<bool>(&_open));
  88     node->tie("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
  89     node->tie("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
  90     node->tie("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
  91     node->tie("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
  92     node->tie("tow/dist",SGRawValuePointer<float>(&_dist));
  93     node->tie("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
  94     node->tie("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
  95     node->tie("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
  96     node->tie("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
  97     node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
  98     node->tie("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
  99     node->tie("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
 100     node->tie("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
 101     node->tie("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
 102     node->tie("is-slave",SGRawValuePointer<bool>(&_isSlave));
 103     node->tie("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
 104     node->tie("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
 105     node->tie("mp-time-lag",SGRawValuePointer<float>(&_mp_time_lag));
 106     node->setStringValue("tow/node","");
 107     node->setStringValue("tow/connected-to-ai-or-mp-callsign");
 108     node->setBoolValue("broken",false);
 109 }
 110
 111 Hitch::~Hitch()
 112 {
 113     char text[128];
 114     sprintf(text,"/sim/hitches/%s", _name);
 115     SGPropertyNode * node = fgGetNode(text, true);
 116     node->untie("tow/length");
 117     node->untie("tow/elastic-constant");
 118     node->untie("tow/weight-per-m-kg-m");
 119     node->untie("tow/brake-force");
 120     node->untie("winch/max-speed-m-s");
 121     node->untie("winch/rel-speed");
 122     node->untie("winch/initial-tow-length-m");
 123     node->untie("winch/min-tow-length-m");
 124     node->untie("winch/max-tow-length-m");
 125     node->untie("winch/global-pos-x");
 126     node->untie("winch/global-pos-y");
 127     node->untie("winch/global-pos-z");
 128     node->untie("winch/max-power");
 129     node->untie("winch/max-force");
 130     node->untie("winch/actual-force");
 131     node->untie("tow/end-force-x");
 132     node->untie("tow/end-force-y");
 133     node->untie("tow/end-force-z");
 134     node->untie("force");
 135     node->untie("open");
 136     node->untie("force-is-calculated-by-other");
 137     node->untie("local-pos-x");
 138     node->untie("local-pos-y");
 139     node->untie("local-pos-z");
 140     node->untie("tow/dist");
 141     node->untie("tow/dist-time-lag-corrected");
 142     node->untie("tow/connected-to-property-node");
 143     node->untie("tow/connected-to-mp-node");
 144     node->untie("tow/connected-to-ai-node");
 145     node->untie("tow/connected-to-ai-or-mp-id");
 146     node->untie("debug/hitch-height-above-ground");
 147     node->untie("debug/tow-end-height-above-ground");
 148     node->untie("debug/tow-rel-lo-pos");
 149     node->untie("debug/tow-lowest-pos-height");
 150     node->untie("is-slave");
 151     node->untie("speed-in-tow-direction");
 152     node->untie("mp-auto-connect-period");
 153     node->untie("mp-time-lag");
 154
 155     delete _state;
 156 }
 157
 158 void Hitch::setPosition(float* position)
 159 {
 160     int i;
 161     for(i=0; i<3; i++) _pos[i] = position[i];
 162 }
 163
 164 void Hitch::setTowLength(float length)
 165 {
 166     _towLength = length;
 167 }
 168
 169 void Hitch::setOpen(bool isOpen)
 170 {
 171    //test if we already processed this before
 172     //without this test a binded property could
 173     //try to close the Hitch every run
 174     //it will close, if we are near the end
 175     //e.g. if we are flying over the parked
 176     //tow-aircraft....
 177     if (isOpen==_last_wish)
 178         return;
 179     _last_wish=isOpen;
 180     _open=isOpen;
 181 }
 182
 183 void Hitch::setTowElasticConstant(float sc)
 184 {
 185     _towElasticConstant=sc;
 186 }
 187
 188 void Hitch::setTowBreakForce(float bf)
 189 {
 190     _towBrakeForce=bf;
 191 }
 192
 193 void Hitch::setWinchMaxForce(float f)
 194 {
 195     _winchMaxForce=f;
 196 }
 197
 198 void Hitch::setTowWeightPerM(float rw)
 199 {
 200     _towWeightPerM=rw;
 201 }
 202
 203 void Hitch::setWinchMaxSpeed(float mws)
 204 {
 205     _winchMaxSpeed=mws;
 206 }
 207
 208 void Hitch::setWinchRelSpeed(float rws)
 209 {
 210     _winchRelSpeed=rws;
 211 }
 212
 213 void Hitch::setWinchPosition(double *winchPosition)//in global coordinates!
 214 {
 215     for (int i=0; i<3;i++)
 216         _winchPos[i]=winchPosition[i];
 217 }
 218
 219 void Hitch::setMpAutoConnectPeriod(float dt)
 220 {
 221     _mpAutoConnectPeriod=dt;
 222 }
 223
 224 void Hitch::setForceIsCalculatedByOther(bool b)
 225 {
 226     _forceIsCalculatedByMaster=b;
 227 }
 228
 229 const char *Hitch::getConnectedPropertyNode() const
 230 {
 231     if (_towEndNode)
 232         return _towEndNode->getDisplayName();
 233     else
 234         return 0;
 235 }
 236
 237 void Hitch::setConnectedPropertyNode(const char *nodename)
 238 {
 239     _towEndNode=fgGetNode(nodename,false);
 240 }
 241
 242 void Hitch::setWinchPositionAuto(bool doit)
 243 {
 244     static bool lastState=false;
 245     if(!_state)
 246         return;
 247     if (!doit)
 248     {
 249         lastState=false;
 250         return;
 251     }
 252     if(lastState)
 253         return;
 254     lastState=true;
 255     float lWinchPos[3];
 256     // The ground plane transformed to the local frame.
 257     float ground[4];
 258     _state->planeGlobalToLocal(_global_ground, ground);
 259
 260     float help[3];
 261     //find a normalized vector pointing forward parallel to the ground
 262     help[0]=0;
 263     help[1]=1;
 264     help[2]=0;
 265     Math::cross3(ground,help,help);
 266     //multiplay by initial tow length;
 267     //reduced by 1m to be able to close the
 268     //hitch either if the glider slips backwards a bit
 269     Math::mul3((_winchInitialTowLength-1.),help,help);
 270     //add to the actual pos
 271     Math::add3(_pos,help,lWinchPos);
 272     //put it onto the ground plane
 273     Math::mul3(ground[3],ground,help);
 274     Math::add3(lWinchPos,help,lWinchPos);
 275
 276     _state->posLocalToGlobal(lWinchPos,_winchPos);
 277     _towLength=_winchInitialTowLength;
 278     SG_LOG(SG_GENERAL, SG_ALERT,"Set the winch pos to "<<_winchPos[0]<<","<<_winchPos[1]<<","<<_winchPos[2]<<endl);
 279     _open=false;
 280
 281     char text[512];
 282     sprintf(text,"/sim/hitches/%s", _name);
 283     SGPropertyNode * node = fgGetNode(text, true);
 284     node->setBoolValue("broken",false);
 285
 286     //set the dist value (if not, the hitch would open in the next calcforce run
 287     //float delta[3];
 288     //Math::sub3(lWinchPos,_pos,delta);
 289     //_dist=Math::mag3(delta);
 290     _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
 291                                   //this position is transferred to the MP-Aircraft.
 292                                   //With this trick, both player in aerotow get the same length
 293
 294 }
 295
 296 void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect)
 297 {
 298     static bool lastState=false;
 299     if(!_state)
 300         return;
 301     if (!doit)
 302     {
 303         lastState=false;
 304         return;
 305     }
 306     if(lastState)
 307         return;
 308     lastState=true;
 309     double gpos[3];
 310     _state->posLocalToGlobal(_pos,gpos);
 311     double bestdist=_towLength*_towLength;//squared!
 312     _towEndIsConnectedToProperty=false;
 313     SGPropertyNode * ainode = fgGetNode("/ai/models",false);
 314     if(!ainode) return;
 315     char myCallsign[256]="***********";
 316     if (running_as_autoconnect)
 317     {
 318         //get own callsign
 319         SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false);
 320         if (cs)
 321         {
 322             strncpy(myCallsign,cs->getStringValue(),256);
 323             myCallsign[255]=0;
 324         }
 325         //reset tow length for search radius. Lentgh will be later copied from master
 326         _towLength=_winchInitialTowLength;
 327     }
 328     bool found=false;
 329     vector <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
 330     for (int i=0;i<ainode->nChildren();i++)
 331     {
 332         SGPropertyNode * n=ainode->getChild(i);
 333         _nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0;
 334         _nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0;
 335         if (!(_nodeIsMultiplayer || _nodeIsAiAircraft))
 336             continue;
 337         if (running_as_autoconnect)
 338         {
 339             if (!_nodeIsMultiplayer)
 340                 continue;
 341             if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue;
 342             if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connected-to-ai-or-mp-callsign"),255)!=0)
 343                 continue;
 344         }
 345         double pos[3];
 346         pos[0]=n->getDoubleValue("position/global-x",0);
 347         pos[1]=n->getDoubleValue("position/global-y",0);
 348         pos[2]=n->getDoubleValue("position/global-z",0);
 349         double dist=0;
 350         for (int j=0;j<3;j++)
 351             dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]);
 352         if (dist<bestdist)
 353         {
 354             bestdist=dist;
 355             _towEndNode=n;
 356             _towEndIsConnectedToProperty=true;
 357             char text[512];
 358             sprintf(text,"/sim/hitches/%s", _name);
 359             SGPropertyNode * node = fgGetNode(text, true);
 360             //node->setStringValue("tow/node",n->getPath());
 361             node->setStringValue("tow/node",n->getDisplayName());
 362             _nodeID=n->getIntValue("id",0);
 363             node->setStringValue("tow/connected-to-ai-or-mp-callsign",n->getStringValue("callsign"));
 364             _open=false;
 365             found = true;
 366         }
 367     }
 368     if (found)
 369     {
 370         char text[512];
 371         sprintf(text,"/sim/hitches/%s", _name);
 372         SGPropertyNode * node = fgGetNode(text, true);
 373         //if (!running_as_autoconnect)
 374         SG_LOG(SG_GENERAL, SG_ALERT,"Found aircraft for aerotow: "
 375                 <<node->getStringValue("tow/connected-to-ai-or-mp-callsign")
 376                 <<", distance "<<Math::sqrt(bestdist)<<"m at "
 377                 <<node->getStringValue("tow/node")<<endl);
 378         if (running_as_autoconnect)
 379             _isSlave=true;
 380         //set the dist value to some value below the tow lentgh (if not, the hitch
 381         //would open in the next calc force run
 382         _dist=_towLength*0.5;
 383         _mp_open_last_state=true;
 384     }
 385     else
 386         if (!running_as_autoconnect)
 387             SG_LOG(SG_GENERAL, SG_ALERT,"Found no aircraft for aerotow!"<<endl);
 388 }
 389
 390 void Hitch::setWinchInitialTowLength(float length)
 391 {
 392     _winchInitialTowLength=length;
 393 }
 394
 395 void Hitch::setWinchPower(float power)
 396 {
 397     _winchPower=power;
 398 }
 399
 400 void Hitch::setWinchMaxTowLength(float length)
 401 {
 402     _winchMaxTowLength=length;
 403 }
 404
 405 void Hitch::setWinchMinTowLength(float length)
 406 {
 407     _winchMinTowLength=length;
 408 }
 409
 410 void Hitch::setGlobalGround(double *global_ground, float *global_vel)
 411 {
 412     int i;
 413     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 414     for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
 415 }
 416
 417 void Hitch::getPosition(float* out)
 418 {
 419     int i;
 420     for(i=0; i<3; i++) out[i] = _pos[i];
 421 }
 422
 423 float Hitch::getTowLength(void)
 424 {
 425     return _towLength;
 426 }
 427
 428 void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
 429 {
 430     float lWinchPos[3],delta[3],deltaN[3];
 431     *_state=*s;
 432     s->posGlobalToLocal(_winchPos,lWinchPos);
 433     Math::sub3(lWinchPos,_pos,delta);
 434     //_dist=Math::mag3(delta);
 435     _dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
 436                                   //this position is transferred to the MP-Aircraft.
 437                                   //With this trick, both player in aerotow get the same length
 438     Math::unit3(delta,deltaN);
 439     float lvel[3];
 440     s->velGlobalToLocal(s->v,lvel);
 441     _speed_in_tow_direction=Math::dot3(lvel,deltaN);
 442     if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
 443     {
 444         float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction);
 445         _timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
 446         if(_forceIsCalculatedByMaster && !_open)
 447         {
 448             s->velGlobalToLocal(_mp_force,_force);
 449             return;
 450         }
 451     }
 452     else
 453         _timeLagCorrectedDist=_dist;
 454     if (_open)
 455     {
 456         _force[0]=_force[1]=_force[2]=0;
 457         return;
 458     }
 459     if(_dist>_towLength)
 460         if(_towLength>1e-3)
 461             _forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant;
 462         else
 463             _forceMagnitude=2*_towBrakeForce;
 464     else
 465         _forceMagnitude=0;
 466     if(_forceMagnitude>=_towBrakeForce)
 467     {
 468         _forceMagnitude=0;
 469         _open=true;
 470         char text[128];
 471         sprintf(text,"/sim/hitches/%s", _name);
 472         SGPropertyNode * node = fgGetNode(text, true);
 473         node->setBoolValue("broken",true);
 474         _force[0]=_force[1]=_force[2]=0;
 475         _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
 476         _reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0;
 477         return;
 478     }
 479     Math::mul3(_forceMagnitude,deltaN,_force);
 480     Math::mul3(-1.,_force,_towEndForce);
 481     _winchActualForce=_forceMagnitude; //missing: gravity on this end and friction
 482     //Add the gravitiy of the rope.
 483     //calculate some numbers:
 484     float grav_force=_towWeightPerM*_towLength*9.81;
 485     //the length of the gravity-expanded row:
 486     float leng=_towLength+grav_force*_towLength/_towElasticConstant;
 487     // The ground plane transformed to the local frame.
 488     float ground[4];
 489     s->planeGlobalToLocal(_global_ground, ground);
 490
 491     // The velocity of the contact patch transformed to local coordinates.
 492     //float glvel[3];
 493     //s->velGlobalToLocal(_global_vel, glvel);
 494
 495     _height_above_ground = ground[3] - Math::dot3(_pos, ground);
 496
 497     //the same for the winch-pos (the pos of the tow end)
 498     _winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground);
 499
 500     //the frac of the grav force acting on _pos:
 501     float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng);
 502     grav_frac=Math::clamp(grav_frac,0,1);
 503     float grav_frac_tow_end=1-grav_frac;
 504     //reduce grav_frac, if the tow has ground contact.
 505     if (_height_above_ground<leng) //if not, the tow can not be on ground
 506     {
 507         float fa[3],fb[3],fg[3];
 508         //the grav force an the hitch position:
 509         Math::mul3(-grav_frac*grav_force,ground,fg);
 510         //the total force on hitch postion:
 511         Math::add3(fg,_force,fa);
 512         //the grav force an the tow end position:
 513         Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
 514         //the total force on tow end postion:
 515         //note: sub: _force on tow-end is negative of force on hitch postion
 516         Math::sub3(fg,_force,fb);
 517         float fa_=Math::mag3(fa);
 518         float fb_=Math::mag3(fb);
 519         float stretchedTowLen;
 520         stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
 521         //the relative position of the lowest postion of the tow:
 522         if ((fa_+fb_)>1e-3)
 523             _loPosFrac=fa_/(fa_+fb_);
 524         else
 525             _loPosFrac=0.5;
 526         //dist to tow-end parallel to ground
 527         float ground_dist;
 528         float help[3];
 529         //Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center....
 530         Math::cross3(lWinchPos,ground,help);
 531         ground_dist=Math::mag3(help);
 532         //height of lowest tow pos (relative to _pos)
 533         _lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist));
 534         if (_height_above_ground<_lowest_tow_height)
 535         {
 536             if (_height_above_ground>1e-3)
 537                 grav_frac*=_height_above_ground/_lowest_tow_height;
 538             else
 539                 grav_frac=0;
 540         }
 541         if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground))
 542         {
 543             if (_winch_height_above_ground>1e-3)
 544                 grav_frac_tow_end*=_winch_height_above_ground/
 545                     (_lowest_tow_height-_height_above_ground+_winch_height_above_ground);
 546             else
 547                 grav_frac_tow_end=0;
 548         }
 549     }
 550     else _lowest_tow_height=_loPosFrac=-1; //for debug output
 551     float grav_force_v[3];
 552     Math::mul3(grav_frac*grav_force,ground,grav_force_v);
 553     Math::add3(grav_force_v,_force,_force);
 554     _forceMagnitude=Math::mag3(_force);
 555     //the same for the tow end:
 556     Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
 557     Math::add3(grav_force_v,_towEndForce,_towEndForce);
 558     s->velLocalToGlobal(_towEndForce,_towEndForce);
 559
 560     if(_forceMagnitude>=_towBrakeForce)
 561     {
 562         _forceMagnitude=0;
 563         _open=true;
 564         char text[128];
 565         sprintf(text,"/sim/hitches/%s", _name);
 566         SGPropertyNode * node = fgGetNode(text, true);
 567         node->setBoolValue("broken",true);
 568         _force[0]=_force[1]=_force[2]=0;
 569         _towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
 570     }
 571
 572
 573 }
 574
 575 // Computed values: total force
 576 void Hitch::getForce(float* force, float* off)
 577 {
 578     Math::set3(_force, force);
 579     Math::set3(_pos, off);
 580 }
 581
 582 void Hitch::integrate (float dt)
 583 {
 584     //check if hitch has opened or closed, if yes: message
 585     if (_open !=_oldOpen)
 586     {
 587         if (_oldOpen)
 588         {
 589             if (_dist>_towLength*1.00001)
 590             {
 591                 SG_LOG(SG_GENERAL, SG_ALERT,"Could not lock Hinch (tow length is insufficient) on hitch '"<<_name<<"' !"<<endl);
 592                 _open=true;
 593                 return;
 594             }
 595             char text[512];
 596             sprintf(text,"/sim/hitches/%s", _name);
 597             SGPropertyNode * node = fgGetNode(text, true);
 598             node->setBoolValue("broken",false);
 599         }
 600         SG_LOG(SG_GENERAL, SG_ALERT,(_open?"Opened hitch (or broken tow) '":"Locked hitch '")<<_name<<"' !"<<endl);
 601         _oldOpen=_open;
 602     }
 603
 604     //check, if tow end should be searched in all MP-aircrafts
 605     if(_open && _mpAutoConnectPeriod)
 606     {
 607         _isSlave=false;
 608         _timeToNextAutoConnectTry-=dt;
 609         if ((_timeToNextAutoConnectTry>_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0))
 610         {
 611             _timeToNextAutoConnectTry=_mpAutoConnectPeriod;
 612             //search for MP-Aircraft, which is towed with us
 613             findBestAIObject(true,true);
 614         }
 615     }
 616     //check, if tow end can be modified by property, if yes: update
 617     if(_towEndIsConnectedToProperty)
 618     {
 619         char text[128];
 620         sprintf(text,"/sim/hitches/%s", _name);
 621         SGPropertyNode * node = fgGetNode(text, true);
 622         if (node)
 623         {
 624             //_towEndNode=fgGetNode(node->getStringValue("tow/node"), false);
 625             char towNode[256];
 626             strncpy(towNode,node->getStringValue("tow/node"),256);
 627             towNode[255]=0;
 628             _towEndNode=fgGetNode("ai/models")->getNode(towNode, false);
 629             //AI and multiplayer objects seem to change node?
 630             //Check if we have the right one by callsign
 631             if (_nodeIsMultiplayer || _nodeIsAiAircraft)
 632             {
 633                 char MPcallsign[256]="";
 634                 const char *MPc;
 635                 MPc=node->getStringValue("tow/connected-to-ai-or-mp-callsign");
 636                 if (MPc)
 637                 {
 638                     strncpy(MPcallsign,MPc,256);
 639                     MPcallsign[255]=0;
 640                 }
 641                 if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode)
 642                 {
 643                     _timeToNextReConnectTry-=dt;
 644                     if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10))
 645                     {
 646                         _timeToNextReConnectTry=10;
 647                         SGPropertyNode * ainode = fgGetNode("/ai/models",false);
 648                         if(ainode)
 649                         {
 650                             for (int i=0;i<ainode->nChildren();i++)
 651                             {
 652                                 SGPropertyNode * n=ainode->getChild(i);
 653                                 if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8))
 654                                     if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found
 655                                     {
 656                                         _towEndNode=n;
 657                                         //node->setStringValue("tow/node",n->getPath());
 658                                         node->setStringValue("tow/node",n->getDisplayName());
 659                                     }
 660                             }
 661                         }
 662                     }
 663                 }
 664             }
 665             if(_towEndNode)
 666             {
 667                 _winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]);
 668                 _winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]);
 669                 _winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]);
 670                 _mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0);
 671                 _mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0);
 672                 _mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0);
 673                 _mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist");
 674                 _mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction");
 675                 _mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0);
 676                 _mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0);
 677                 _mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0);
 678
 679                 if(_isSlave)
 680                 {
 681 #define gf(a,b) a=_towEndNode->getFloatValue(b,a)
 682 #define gb(a,b) a=_towEndNode->getBoolValue(b,a)
 683                     gf(_towLength,"sim/hitches/aerotow/tow/length");
 684                     gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant");
 685                     gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m");
 686                     gf(_towBrakeForce,"sim/hitches/aerotow/brake-force");
 687                     gb(_open,"sim/hitches/aerotow/open");
 688                     gb(_mp_is_slave,"sim/hitches/aerotow/is-slave");
 689 #undef gf
 690 #undef gb
 691                     if (_mp_is_slave) _isSlave=false; //someone should be master
 692                 }
 693                 else
 694                 {
 695                     //check if other has opened hitch, but is neccessary, that it was closed before
 696                     bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state);
 697                     if (mp_open != _mp_open_last_state) //state has changed
 698                     {
 699                         _mp_open_last_state=mp_open; //store that value
 700                         if(!_open)
 701                         {
 702                             if(mp_open)
 703                             {
 704                                 _open=true;
 705                                 char text[512];
 706                                 sprintf(text,"/sim/hitches/%s", _name);
 707                                 SGPropertyNode * node = fgGetNode(text, true);
 708                                 node->getStringValue("tow/node","");
 709                                 SG_LOG(SG_GENERAL, SG_ALERT,"'"<<node->getStringValue("tow/node","")<<"' has opened hitch!"<<endl);
 710                             }
 711                         }
 712                     }
 713                 }
 714                 //try to calculate the time lag
 715                 if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
 716                 {
 717                     _mp_last_reported_dist=_mp_dist;
 718                     _mp_last_reported_v=_mp_v;
 719                     float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
 720                     float abs_v=Math::abs(total_v);
 721                     if (abs_v>0.1)
 722                     {
 723                         float actual_time_lag_guess=(_mp_dist-_dist)/total_v;
 724                         //check, if it sounds ok
 725                         if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5))
 726                         {
 727                             float frac=abs_v*0.01;
 728                             if (frac>0.05) frac=0.05;
 729                             // if we are slow, the guess of the lag can be rather wrong. as faster we are
 730                             // the better the guess. Therefore frac is proportiona to the speed. Clamp it
 731                             // at 5m/s
 732                             _mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess;
 733                         }
 734                     }
 735                 }
 736             }
 737         }
 738     }
 739     //set the _reported_tow_end_force (smoothed)
 740     //smooth it a bit and store it
 741     float sc=10.*dt; //100ms
 742     float tmp[3];
 743     Math::mul3(sc,_towEndForce,tmp);
 744     Math::mul3(1.-sc,_reportTowEndForce,_reportTowEndForce);
 745     Math::add3(tmp,_reportTowEndForce,_reportTowEndForce);
 746
 747     if (_open) return;
 748     if (_winchRelSpeed==0) return;
 749     float factor=1,offset=0;
 750     if (_winchActualForce>_winchMaxForce)
 751         offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20;
 752     if (_winchRelSpeed>0.01) // to avoit div by zero
 753     {
 754         float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed);
 755         if (_winchActualForce>maxForcePowerLimit)
 756             factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit;
 757     }
 758     _towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed;
 759     if (_towLength<=_winchMinTowLength)
 760     {
 761         if (_winchRelSpeed<0)
 762             _winchRelSpeed=0;
 763         _towLength=_winchMinTowLength;
 764         return;
 765     }
 766     if (_towLength>=_winchMaxTowLength)
 767     {
 768         if (_winchRelSpeed<0)
 769             _winchRelSpeed=0;
 770         _towLength=_winchMaxTowLength;
 771         return;
 772     }
 773 }
 774
 775
 776
 777
 778 }; // namespace yasim