src/FDM/YASim/Launchbar.cpp

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