src/FDM/YASim/Launchbar.cpp

   1 #include "Math.hpp"
   2 #include "BodyEnvironment.hpp"
   3 #include "Ground.hpp"
   4 #include "RigidBody.hpp"
   5
   6 #include "Launchbar.hpp"
   7
   8 using namespace std;
   9 namespace yasim {
  10
  11   static const float YASIM_PI2 = 3.14159265358979323846/2;
  12   static const float YASIM_PI = 3.14159265358979323846;
  13   static const float RAD2DEG = 180/YASIM_PI;
  14
  15 Launchbar::Launchbar()
  16 {
  17     int i;
  18     for(i=0; i<3; i++)
  19       _launchbar_mount[i] = _holdback_mount[i] = _launchbar_force[i]
  20               = _holdback_force[i] = 0;
  21     for(i=0; i<2; i++)
  22         _global_ground[i] = 0;
  23     _global_ground[2] = 1;
  24     _global_ground[3] = -1e5;
  25     _length = 0.0;
  26     _holdback_length = 2.0;
  27     _down_ang = 0.0;
  28     _up_ang = 0.0;
  29     _ang = 0.0;
  30     _extension = 0.0;
  31     _frac = _h_frac =0.0;
  32     _launch_cmd = false;
  33     _pos_on_cat = 0.0;
  34     _state = Unmounted;
  35     _strop = false;
  36 }
  37
  38 void Launchbar::setLaunchbarMount(float* position)
  39 {
  40     int i;
  41     for(i=0; i<3; i++) _launchbar_mount[i] = position[i];
  42 }
  43
  44 void Launchbar::setHoldbackMount(float* position)
  45 {
  46     int i;
  47     for(i=0; i<3; i++) _holdback_mount[i] = position[i];
  48 }
  49
  50 void Launchbar::setLength(float length)
  51 {
  52     _length = length;
  53 }
  54
  55   void Launchbar::setHoldbackLength(float length)
  56   {
  57     _holdback_length = length;
  58   }
  59
  60 void Launchbar::setDownAngle(float ang)
  61 {
  62     _down_ang = ang;
  63 }
  64
  65 void Launchbar::setUpAngle(float ang)
  66 {
  67     _up_ang = ang;
  68 }
  69
  70 void Launchbar::setExtension(float extension)
  71 {
  72     _extension = extension;
  73 }
  74
  75 void Launchbar::setLaunchCmd(bool cmd)
  76 {
  77     _launch_cmd = cmd;
  78 }
  79
  80 void Launchbar::setGlobalGround(double *global_ground)
  81 {
  82     int i;
  83     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
  84 }
  85
  86 void Launchbar::getLaunchbarMount(float* out)
  87 {
  88     int i;
  89     for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
  90 }
  91
  92   float Launchbar::getLaunchbarPos(int i)
  93   {
  94     return _launchbar_mount[i];
  95   }
  96
  97 void Launchbar::getHoldbackMount(float* out)
  98 {
  99     int i;
 100     for(i=0; i<3; i++) out[i] = _holdback_mount[i];
 101 }
 102
 103   float Launchbar::getHoldbackPos(int j)
 104   {
 105     return _holdback_mount[j];
 106   }
 107
 108   float Launchbar::getHoldbackLength(void)
 109   {
 110     return _holdback_length;
 111   }
 112
 113 float Launchbar::getLength(void)
 114 {
 115     return _length;
 116 }
 117
 118 float Launchbar::getDownAngle(void)
 119 {
 120     return _down_ang;
 121 }
 122
 123 float Launchbar::getUpAngle(void)
 124 {
 125     return _up_ang;
 126 }
 127
 128   float Launchbar::getAngle(void)
 129   {
 130     return _ang;
 131   }
 132
 133   float Launchbar::getHoldbackAngle(void)
 134   {
 135     return _h_ang;
 136   }
 137 float Launchbar::getExtension(void)
 138 {
 139     return _extension;
 140 }
 141
 142   void Launchbar::getForce(float* force1, float* off1,
 143     float* force2, float* off2)
 144   {
 145     Math::set3(_launchbar_force, force1);
 146     Math::set3(_launchbar_mount, off1);
 147     Math::set3(_holdback_force, force2);
 148     Math::set3(_holdback_mount, off2);
 149   }
 150
 151   const char* Launchbar::getState(void)
 152   {
 153     switch (_state) {
 154     case Arrested:
 155       return "Engaged";
 156     case Launch:
 157       return "Launching";
 158     case Completed:
 159       return "Completed";
 160     default:
 161       return "Disengaged";
 162     }
 163   }
 164
 165   bool Launchbar::getStrop(void)
 166 {
 167     return _strop;
 168 }
 169
 170 float Launchbar::getCompressFraction()
 171 {
 172     return _frac;
 173 }
 174
 175   float Launchbar::getHoldbackCompressFraction()
 176   {
 177     return _h_frac;
 178   }
 179
 180 void Launchbar::getTipPosition(float* out)
 181 {
 182     // The launchbar tip in local coordinates.
 183
 184     _ang = _frac*(_down_ang - _up_ang ) + _up_ang ;
 185     float ptip[3] = { _length*Math::cos(_ang), 0, -_length*Math::sin(_ang) };
 186     Math::add3(_launchbar_mount, ptip, out);
 187   }
 188
 189   float Launchbar::getTipPos(int i)
 190   {
 191     float pos_tip[3];
 192     getTipPosition(pos_tip);
 193     return pos_tip[i];
 194   }
 195
 196   void Launchbar::getHoldbackTipPosition(float* out)
 197   {
 198     // The holdback tip in local coordinates.
 199     _h_ang = _h_frac*(_down_ang - _up_ang) + _up_ang;
 200     float htip[3] = { -_length*Math::cos(_h_ang), 0, -_length*Math::sin(_h_ang) };
 201     Math::add3(_holdback_mount, htip, out);
 202   }
 203
 204   float Launchbar::getHoldbackTipPos(int i)
 205   {
 206     float pos_tip[3];
 207     getHoldbackTipPosition(pos_tip);
 208     return pos_tip[i];
 209 }
 210
 211
 212 void Launchbar::getTipGlobalPosition(State* s, double* out)
 213 {
 214     // The launchbar tip in local coordinates.
 215     float pos_tip[3];
 216     getTipPosition(pos_tip);
 217     // The launchbar tip in global coordinates.
 218     s->posLocalToGlobal(pos_tip, out);
 219 }
 220
 221 float Launchbar::getPercentPosOnCat(float* lpos, float off, float lends[2][3])
 222 {
 223     // Compute the forward direction of the cat.
 224     float lforward[3];
 225     Math::sub3(lends[1], lends[0], lforward);
 226     float ltopos[3];
 227     Math::sub3(lpos, lends[0], ltopos);
 228     float fwlen = Math::mag3(lforward);
 229
 230     return (Math::dot3(ltopos, lforward)/fwlen + off)/fwlen;
 231 }
 232
 233 void Launchbar::getPosOnCat(float perc, float* lpos, float* lvel,
 234                             float lends[2][3], float lendvels[2][3])
 235 {
 236     if (perc < 0.0)
 237         perc = 0.0;
 238     if (1.0 < perc)
 239         perc = 1.0;
 240
 241     // Compute the forward direction of the cat.
 242     float lforward[3];
 243     Math::sub3(lends[1], lends[0], lforward);
 244     Math::mul3(perc, lforward, lpos);
 245     Math::add3(lends[0], lpos, lpos);
 246
 247     float tmp[3];
 248     Math::mul3(perc, lendvels[0], lvel);
 249     Math::mul3(1.0-perc, lendvels[1], tmp);
 250     Math::add3(tmp, lvel, lvel);
 251 }
 252
 253 void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot)
 254 {
 255     // Init the return values
 256     int i;
 257     for(i=0; i<3; i++) _launchbar_force[i] = 0;
 258     for(i=0; i<3; i++) _holdback_force[i] = 0;
 259
 260     if (_state != Unmounted)
 261       _extension = 1;
 262
 263     // Don't bother if it's fully retracted
 264     if(_extension <= 0)
 265         return;
 266
 267     // For the first guess, the position fraction is equal to the
 268     // extension value.
 269     _frac = _h_frac = _extension;
 270
 271     // The ground plane transformed to the local frame.
 272     float ground[4];
 273     s->planeGlobalToLocal(_global_ground, ground);
 274
 275     // The launchbar tip in local coordinates.
 276     float ltip[3];
 277     getTipPosition(ltip);
 278
 279         // Correct the extension value for no intersection.
 280
 281     // Check if the tip will intersect the ground or not. That is, compute
 282     // the distance of the tip to the ground plane.
 283     float tipdist = ground[3] - Math::dot3(ltip, ground);
 284     if(0 <= tipdist) {
 285       _frac = _extension;
 286     } else {
 287       // Compute the distance of the launchbar mount point from the
 288       // ground plane.
 289       float mountdist = ground[3] - Math::dot3(_launchbar_mount, ground);
 290
 291       // Compute the distance of the launchbar mount point from the
 292       // ground plane in the x-z plane. It holds:
 293       // mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
 294       // thus
 295       float mountdist_xz = _length;
 296       if (ground[2] != 0) {
 297         float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
 298         mountdist_xz = -mountdist*nrm_yz/ground[2];
 299       }
 300
 301       if (mountdist_xz < _length) {
 302         // the launchbar points forward, so we need to change the signs here
 303         float ang = -Math::asin(mountdist_xz/_length)
 304           + Math::atan2(ground[2], ground[0]) + YASIM_PI2;
 305         ang = -ang;
 306             _frac = (ang - _up_ang)/(_down_ang - _up_ang);
 307       } else {
 308             _frac = _extension;
 309     }
 310     }
 311
 312     // Now do it again for the holdback
 313
 314     // The holdback tip in local coordinates.
 315     float htip[3];
 316     getHoldbackTipPosition(htip);
 317
 318     // Check if the tip will intersect the ground or not. That is, compute
 319     // the distance of the tip to the ground plane.
 320     float h_tipdist = ground[3] - Math::dot3(htip, ground);
 321     if (0 <= h_tipdist) {
 322       _h_frac = _extension;
 323     } else {
 324       // Compute the distance of the holdback mount point from the ground
 325       // plane.
 326       float h_mountdist = ground[3] - Math::dot3(_holdback_mount, ground);
 327
 328       // Compute the distance of the holdback mount point from the ground
 329       // plane in the x-z plane. It holds:
 330       //  mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
 331       // thus
 332       float h_mountdist_xz = _holdback_length;
 333       if (ground[2] != 0) {
 334         float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
 335         h_mountdist_xz = -h_mountdist*nrm_yz/ground[2];
 336       }
 337
 338       if (h_mountdist_xz < _holdback_length) {
 339         float h_ang = Math::asin(h_mountdist_xz/_holdback_length)
 340           + Math::atan2(ground[2], ground[0]) + YASIM_PI2;
 341         _h_frac = (h_ang - _up_ang)/(_down_ang - _up_ang);
 342       } else {
 343         _h_frac = _extension;
 344       }
 345     }
 346
 347     float llb_mount[3];
 348     getTipPosition(llb_mount);
 349
 350     // The launchbar tip in global coordinates.
 351     double launchbar_pos[3];
 352     s->posLocalToGlobal(llb_mount, launchbar_pos);
 353
 354     double end[2][3]; float vel[2][3];
 355     float dist = g_cb->getCatapult(launchbar_pos, end, vel);
 356     // Work around a problem of flightgear returning totally screwed up
 357     // scenery when switching views.
 358     if (1e3 < dist)
 359         return;
 360
 361     // Compute the positions of the catapult start and endpoints in the
 362     // local coordinate system
 363     float lend[2][3];
 364     s->posGlobalToLocal(end[0], lend[0]);
 365     s->posGlobalToLocal(end[1], lend[1]);
 366
 367     // Transform the velocities of the endpoints to the
 368     // local coordinate sytem.
 369     float lvel[2][3];
 370     s->velGlobalToLocal(vel[0], lvel[0]);
 371     s->velGlobalToLocal(vel[1], lvel[1]);
 372
 373     // Compute the position of the launchbar tip relative to the cat.
 374     float tip_pos_on_cat = getPercentPosOnCat(llb_mount, 0.0, lend);
 375     float llbtip[3], lvlbtip[3];
 376     getPosOnCat(tip_pos_on_cat, llbtip, lvlbtip, lend, lvel);
 377
 378     // Compute the direction from the launchbar mount at the gear
 379     // to the lauchbar mount on the cat.
 380     float llbdir[3];
 381     Math::sub3(llbtip, _launchbar_mount, llbdir);
 382     float lblen = Math::mag3(llbdir);
 383     Math::mul3(1.0/lblen, llbdir, llbdir);
 384
 385     // Check if we are near enough to the cat.
 386     if (_state == Unmounted && dist < 0.6) {
 387       // coarse approximation for the velocity of the launchbar.
 388         // Might be sufficient because arresting at the cat makes only
 389         // sense when the aircraft does not rotate much.
 390         float lv_mount[3];
 391         float tmp[3];
 392         float lrot[3], lv[3];
 393         Math::vmul33(s->orient, s->rot, lrot);
 394         Math::vmul33(s->orient, s->v, lv);
 395         body->pointVelocity(llb_mount, lrot, tmp);
 396         Math::sub3(tmp, lvlbtip, lv_mount);
 397         Math::add3(lv, lv_mount, lv_mount);
 398
 399         // We cannot arrest at the cat if we move too fast wrt the cat.
 400         if (0.2 < Math::mag3(lv_mount))
 401             return;
 402
 403         // Compute the position of the holdback mount relative to the cat.
 404         double dd[2][3]; float fd[2][3]; double ghldbkpos[3];
 405         s->posLocalToGlobal(_holdback_mount, ghldbkpos);
 406         float hbdist = g_cb->getCatapult(ghldbkpos, dd, fd);
 407
 408       // don't let the calculation go -ve here
 409       if (_holdback_length*_holdback_length - hbdist*hbdist < 0)
 410         return;
 411       float offset = -Math::sqrt(_holdback_length*_holdback_length
 412         - hbdist*hbdist);
 413       _pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
 414
 415         // We cannot arrest if we are not at the start of the cat.
 416       if (_pos_on_cat < 0.0 || 0.4 < _pos_on_cat)
 417             return;
 418
 419         // Now we are arrested at the cat.
 420         // The force is applied at the next step.
 421         _state = Arrested;
 422         return;
 423     }
 424
 425     // Get the actual distance from the holdback to its mountpoint
 426     // on the cat. If it is longer than the holdback apply a force.
 427     float lhldbk_cmount[3]; float lvhldbk_cmount[3];
 428     getPosOnCat(_pos_on_cat, lhldbk_cmount, lvhldbk_cmount, lend, lvel);
 429     // Compute the direction of holdback.
 430     float lhldbkdir[3];
 431     Math::sub3(lhldbk_cmount, _holdback_mount, lhldbkdir);
 432     float hldbklen = Math::mag3(lhldbkdir);
 433     Math::mul3(1/hldbklen, lhldbkdir, lhldbkdir);
 434
 435     if (_state == Arrested) {
 436         // Now apply a constant tension from the catapult over the launchbar.
 437       Math::mul3(2.0, llbdir, _launchbar_force);
 438
 439         // If the distance from the holdback mount at the aircraft to the
 440         // holdback mount on the cat is larger than the holdback length itself,
 441         // the holdback applies a force to the gear.
 442         if (_holdback_length < hldbklen) {
 443         // coarse approximation for the velocity of the holdback mount
 444             // at the gear.
 445             // Might be sufficient because arresting at the cat makes only
 446             // sense when the aircraft does not rotate much.
 447             float lvhldbk_gmount[3];
 448             float lrot[3], lv[3];
 449             Math::vmul33(s->orient, s->rot, lrot);
 450             Math::vmul33(s->orient, s->v, lv);
 451             body->pointVelocity(_holdback_mount, lrot, lvhldbk_gmount);
 452             Math::add3(lv, lvhldbk_gmount, lvhldbk_gmount);
 453
 454             // The velocity of the holdback mount at the gear wrt the
 455             // holdback mount at the cat.
 456             float lvhldbk[3];
 457             Math::sub3(lvhldbk_gmount, lvhldbk_cmount, lvhldbk);
 458
 459             // The spring force the holdback will apply to the gear
 460             float tmp[3];
 461         Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir,
 462           _holdback_force);
 463
 464             // The damping force here ...
 465             Math::mul3(2e0, lvhldbk, tmp);
 466         Math::sub3(_holdback_force, tmp, _holdback_force);
 467         }
 468
 469       if (_launch_cmd) {
 470             _state = Launch;
 471         _strop = false;
 472       }
 473     }
 474
 475     if (_state == Launch) {
 476         // Now apply a constant tension from the catapult over the launchbar.
 477       Math::mul3(25.0, llbdir, _launchbar_force);
 478
 479       if (1.0 < dist) {
 480         _state = Completed;
 481       }
 482     }
 483
 484     if (_state == Completed) {
 485       // Wait until the strop has cleared the deck
 486       // This is a temporary fix until we come up with something better
 487
 488       if (_frac > 0.8) {
 489             _state = Unmounted;
 490         _strop = true;
 491       }
 492     }
 493
 494     // Scale by the mass. That keeps the stiffness in reasonable bounds.
 495     float mass = body->getTotalMass();
 496     Math::mul3(mass, _launchbar_force, _launchbar_force);
 497     Math::mul3(mass, _holdback_force, _holdback_force);
 498 }
 499
 500 }; // namespace yasim
 501