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 #include <iostream>
   9 using namespace std;
  10 namespace yasim {
  11
  12 Launchbar::Launchbar()
  13 {
  14     int i;
  15     for(i=0; i<3; i++)
  16         _launchbar_mount[i] = _holdback_mount[i] = _force[i] = 0;
  17     for(i=0; i<2; i++)
  18         _global_ground[i] = 0;
  19     _global_ground[2] = 1;
  20     _global_ground[3] = -1e5;
  21     _length = 0.0;
  22     _holdback_length = 2.0;
  23     _down_ang = 0.0;
  24     _up_ang = 0.0;
  25     _extension = 0.0;
  26     _frac = 0.0;
  27     _launch_cmd = false;
  28     _pos_on_cat = 0.0;
  29     _state = Unmounted;
  30 }
  31
  32 void Launchbar::setLaunchbarMount(float* position)
  33 {
  34     int i;
  35     for(i=0; i<3; i++) _launchbar_mount[i] = position[i];
  36 }
  37
  38 void Launchbar::setHoldbackMount(float* position)
  39 {
  40     int i;
  41     for(i=0; i<3; i++) _holdback_mount[i] = position[i];
  42 }
  43
  44 void Launchbar::setLength(float length)
  45 {
  46     _length = length;
  47 }
  48
  49 void Launchbar::setDownAngle(float ang)
  50 {
  51     _down_ang = ang;
  52 }
  53
  54 void Launchbar::setUpAngle(float ang)
  55 {
  56     _up_ang = ang;
  57 }
  58
  59 void Launchbar::setExtension(float extension)
  60 {
  61     _extension = extension;
  62 }
  63
  64 void Launchbar::setLaunchCmd(bool cmd)
  65 {
  66     _launch_cmd = cmd;
  67 }
  68
  69 void Launchbar::setGlobalGround(double *global_ground)
  70 {
  71     int i;
  72     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
  73 }
  74
  75 void Launchbar::getLaunchbarMount(float* out)
  76 {
  77     int i;
  78     for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
  79 }
  80
  81 void Launchbar::getHoldbackMount(float* out)
  82 {
  83     int i;
  84     for(i=0; i<3; i++) out[i] = _holdback_mount[i];
  85 }
  86
  87 float Launchbar::getLength(void)
  88 {
  89     return _length;
  90 }
  91
  92 float Launchbar::getDownAngle(void)
  93 {
  94     return _down_ang;
  95 }
  96
  97 float Launchbar::getUpAngle(void)
  98 {
  99     return _up_ang;
 100 }
 101
 102 float Launchbar::getExtension(void)
 103 {
 104     return _extension;
 105 }
 106
 107 void Launchbar::getForce(float* force, float* off)
 108 {
 109     Math::set3(_force, force);
 110     Math::set3(_launchbar_mount, off);
 111 }
 112
 113 float Launchbar::getCompressFraction()
 114 {
 115     return _frac;
 116 }
 117
 118 void Launchbar::getTipPosition(float* out)
 119 {
 120     // The launchbar tip in local coordinates.
 121     float ang = _frac*(_down_ang - _up_ang) + _up_ang;
 122     float pos_tip[3] = { _length*Math::cos(ang), 0.0,-_length*Math::sin(ang) };
 123     Math::add3(_launchbar_mount, pos_tip, out);
 124 }
 125
 126 void Launchbar::getTipGlobalPosition(State* s, double* out)
 127 {
 128     // The launchbar tip in local coordinates.
 129     float pos_tip[3];
 130     getTipPosition(pos_tip);
 131     // The launchbar tip in global coordinates.
 132     s->posLocalToGlobal(pos_tip, out);
 133 }
 134
 135 float Launchbar::getPercentPosOnCat(float* lpos, float off, float lends[2][3])
 136 {
 137     // Compute the forward direction of the cat.
 138     float lforward[3];
 139     Math::sub3(lends[1], lends[0], lforward);
 140     float ltopos[3];
 141     Math::sub3(lpos, lends[0], ltopos);
 142     float fwlen = Math::mag3(lforward);
 143
 144     return (Math::dot3(ltopos, lforward)/fwlen + off)/fwlen;
 145 }
 146
 147 void Launchbar::getPosOnCat(float perc, float* lpos, float* lvel,
 148                             float lends[2][3], float lendvels[2][3])
 149 {
 150     if (perc < 0.0)
 151         perc = 0.0;
 152     if (1.0 < perc)
 153         perc = 1.0;
 154
 155     // Compute the forward direction of the cat.
 156     float lforward[3];
 157     Math::sub3(lends[1], lends[0], lforward);
 158     Math::mul3(perc, lforward, lpos);
 159     Math::add3(lends[0], lpos, lpos);
 160
 161     float tmp[3];
 162     Math::mul3(perc, lendvels[0], lvel);
 163     Math::mul3(1.0-perc, lendvels[1], tmp);
 164     Math::add3(tmp, lvel, lvel);
 165 }
 166
 167 void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot)
 168 {
 169     // Init the return values
 170     int i;
 171     for(i=0; i<3; i++) _force[i] = 0;
 172
 173     if (_state != Unmounted)
 174       _extension = 1;
 175
 176     // Don't bother if it's fully retracted
 177     if(_extension <= 0)
 178         return;
 179
 180     if (_extension < _frac)
 181         _frac = _extension;
 182
 183     // The launchbar tip in global coordinates.
 184     double launchbar_pos[3];
 185     getTipGlobalPosition(s, launchbar_pos);
 186
 187     // If the launchbars tip is less extended than it could be.
 188     if(_frac < _extension) {
 189         // Correct the extension value for no intersection.
 190         // Compute the distance of the mount point from the ground plane.
 191         double a = - _global_ground[3] + launchbar_pos[0]*_global_ground[0]
 192           + launchbar_pos[1]*_global_ground[1]
 193           + launchbar_pos[2]*_global_ground[2];
 194         if(a < _length) {
 195             float ang = Math::asin(a/_length);
 196             _frac = (ang - _up_ang)/(_down_ang - _up_ang);
 197         } else
 198             // FIXME: this will jump
 199             _frac = _extension;
 200     }
 201
 202     // Recompute the launchbar tip.
 203     float llb_mount[3];
 204     getTipPosition(llb_mount);
 205     // The launchbar tip in global coordinates.
 206     s->posLocalToGlobal(llb_mount, launchbar_pos);
 207
 208     double end[2][3]; float vel[2][3];
 209     float dist = g_cb->getCatapult(launchbar_pos, end, vel);
 210     // Work around a problem of flightgear returning totally screwed up
 211     // scenery when switching views.
 212     if (1e3 < dist)
 213         return;
 214
 215     // Compute the positions of the catapult start and endpoints in the
 216     // local coordiante system
 217     float lend[2][3];
 218     s->posGlobalToLocal(end[0], lend[0]);
 219     s->posGlobalToLocal(end[1], lend[1]);
 220
 221     // Transform the velocities of the endpoints to the
 222     // local coordinate sytem.
 223     float lvel[2][3];
 224     s->velGlobalToLocal(vel[0], lvel[0]);
 225     s->velGlobalToLocal(vel[1], lvel[1]);
 226
 227     // Compute the position of the launchbar tip relative to the cat.
 228     float tip_pos_on_cat = getPercentPosOnCat(llb_mount, 0.0, lend);
 229     float llbtip[3], lvlbtip[3];
 230     getPosOnCat(tip_pos_on_cat, llbtip, lvlbtip, lend, lvel);
 231
 232     // Compute the direction from the launchbar mount at the gear
 233     // to the lauchbar mount on the cat.
 234     float llbdir[3];
 235     Math::sub3(llbtip, _launchbar_mount, llbdir);
 236     float lblen = Math::mag3(llbdir);
 237     Math::mul3(1.0/lblen, llbdir, llbdir);
 238
 239     // Check if we are near enough to the cat.
 240     if (_state == Unmounted && dist < 0.5) {
 241         // croase approximation for the velocity of the launchbar.
 242         // Might be sufficient because arresting at the cat makes only
 243         // sense when the aircraft does not rotate much.
 244         float lv_mount[3];
 245         float tmp[3];
 246         float lrot[3], lv[3];
 247         Math::vmul33(s->orient, s->rot, lrot);
 248         Math::vmul33(s->orient, s->v, lv);
 249         body->pointVelocity(llb_mount, lrot, tmp);
 250         Math::sub3(tmp, lvlbtip, lv_mount);
 251         Math::add3(lv, lv_mount, lv_mount);
 252
 253         // We cannot arrest at the cat if we move too fast wrt the cat.
 254         if (0.2 < Math::mag3(lv_mount))
 255             return;
 256
 257         // Compute the position of the holdback mount relative to the cat.
 258         double dd[2][3]; float fd[2][3]; double ghldbkpos[3];
 259         s->posLocalToGlobal(_holdback_mount, ghldbkpos);
 260         float hbdist = g_cb->getCatapult(ghldbkpos, dd, fd);
 261         float offset = -Math::sqrt(_holdback_length*_holdback_length - hbdist*hbdist);
 262         _pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
 263
 264
 265         // We cannot arrest if we are not at the start of the cat.
 266         if (_pos_on_cat < 0.0 || 0.2 < _pos_on_cat)
 267             return;
 268
 269         // Now we are arrested at the cat.
 270         // The force is applied at the next step.
 271         _state = Arrested;
 272         return;
 273     }
 274
 275     // Get the actual distance from the holdback to its mountpoint
 276     // on the cat. If it is longer than the holdback apply a force.
 277     float lhldbk_cmount[3]; float lvhldbk_cmount[3];
 278     getPosOnCat(_pos_on_cat, lhldbk_cmount, lvhldbk_cmount, lend, lvel);
 279     // Compute the direction of holdback.
 280     float lhldbkdir[3];
 281     Math::sub3(lhldbk_cmount, _holdback_mount, lhldbkdir);
 282     float hldbklen = Math::mag3(lhldbkdir);
 283     Math::mul3(1/hldbklen, lhldbkdir, lhldbkdir);
 284
 285     if (_state == Arrested) {
 286         // Now apply a constant tension from the catapult over the launchbar.
 287         Math::mul3(2.0, llbdir, _force);
 288
 289         // If the distance from the holdback mount at the aircraft to the
 290         // holdback mount on the cat is larger than the holdback length itself,
 291         // the holdback applies a force to the gear.
 292         if (_holdback_length < hldbklen) {
 293             // croase approximation for the velocity of the holdback mount
 294             // at the gear.
 295             // Might be sufficient because arresting at the cat makes only
 296             // sense when the aircraft does not rotate much.
 297             float lvhldbk_gmount[3];
 298             float lrot[3], lv[3];
 299             Math::vmul33(s->orient, s->rot, lrot);
 300             Math::vmul33(s->orient, s->v, lv);
 301             body->pointVelocity(_holdback_mount, lrot, lvhldbk_gmount);
 302             Math::add3(lv, lvhldbk_gmount, lvhldbk_gmount);
 303
 304             // The velocity of the holdback mount at the gear wrt the
 305             // holdback mount at the cat.
 306             float lvhldbk[3];
 307             Math::sub3(lvhldbk_gmount, lvhldbk_cmount, lvhldbk);
 308
 309             // The spring force the holdback will apply to the gear
 310             float tmp[3];
 311             Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir, tmp);
 312             Math::add3(tmp, _force, _force);
 313
 314             // The damping force here ...
 315             Math::mul3(2e0, lvhldbk, tmp);
 316             Math::sub3(_force, tmp, _force);
 317         }
 318
 319         if (_launch_cmd)
 320             _state = Launch;
 321     }
 322
 323     if (_state == Launch) {
 324         // Now apply a constant tension from the catapult over the launchbar.
 325         Math::mul3(25.0, llbdir, _force);
 326
 327         if (1.0 < dist)
 328             _state = Unmounted;
 329     }
 330
 331     // Scale by the mass. That keeps the stiffness in reasonable bounds.
 332     float mass = body->getTotalMass();
 333     Math::mul3(mass, _force, _force);
 334 }
 335
 336 }; // namespace yasim
 337