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