src/FDM/YASim/Hitch.cpp

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