2 #include "BodyEnvironment.hpp"
4 #include "RigidBody.hpp"
5 #include "Launchbar.hpp"
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;
13 Launchbar::Launchbar()
17 _launchbar_mount[i] = _holdback_mount[i] = _launchbar_force[i]
18 = _holdback_force[i] = 0;
20 _global_ground[i] = 0;
21 _global_ground[2] = 1;
22 _global_ground[3] = -1e5;
24 _holdback_length = 2.0;
37 void Launchbar::setLaunchbarMount(float* position)
40 for(i=0; i<3; i++) _launchbar_mount[i] = position[i];
43 void Launchbar::setHoldbackMount(float* position)
46 for(i=0; i<3; i++) _holdback_mount[i] = position[i];
49 void Launchbar::setLength(float length)
54 void Launchbar::setHoldbackLength(float length)
56 _holdback_length = length;
59 void Launchbar::setDownAngle(float ang)
64 void Launchbar::setUpAngle(float ang)
69 void Launchbar::setExtension(float extension)
71 _extension = extension;
74 void Launchbar::setLaunchCmd(bool cmd)
79 void Launchbar::setAcceleration(float acceleration)
81 _acceleration = acceleration;
84 void Launchbar::setGlobalGround(double *global_ground)
87 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
90 void Launchbar::getLaunchbarMount(float* out)
93 for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
96 float Launchbar::getLaunchbarPos(int i)
98 return _launchbar_mount[i];
101 void Launchbar::getHoldbackMount(float* out)
104 for(i=0; i<3; i++) out[i] = _holdback_mount[i];
107 float Launchbar::getHoldbackPos(int j)
109 return _holdback_mount[j];
112 float Launchbar::getHoldbackLength(void)
114 return _holdback_length;
117 float Launchbar::getLength(void)
122 float Launchbar::getDownAngle(void)
127 float Launchbar::getUpAngle(void)
132 float Launchbar::getAngle(void)
137 float Launchbar::getHoldbackAngle(void)
141 float Launchbar::getExtension(void)
146 void Launchbar::getForce(float* force1, float* off1,
147 float* force2, float* off2)
149 Math::set3(_launchbar_force, force1);
150 Math::set3(_launchbar_mount, off1);
151 Math::set3(_holdback_force, force2);
152 Math::set3(_holdback_mount, off2);
155 const char* Launchbar::getState(void)
169 bool Launchbar::getStrop(void)
174 float Launchbar::getCompressFraction()
179 float Launchbar::getHoldbackCompressFraction()
184 void Launchbar::getTipPosition(float* out)
186 // The launchbar tip in local coordinates.
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);
193 float Launchbar::getTipPos(int i)
196 getTipPosition(pos_tip);
200 void Launchbar::getHoldbackTipPosition(float* out)
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);
208 float Launchbar::getHoldbackTipPos(int i)
211 getHoldbackTipPosition(pos_tip);
216 void Launchbar::getTipGlobalPosition(State* s, double* out)
218 // The launchbar tip in local coordinates.
220 getTipPosition(pos_tip);
221 // The launchbar tip in global coordinates.
222 s->posLocalToGlobal(pos_tip, out);
225 float Launchbar::getPercentPosOnCat(float* lpos, float off, float lends[2][3])
227 // Compute the forward direction of the cat.
229 Math::sub3(lends[1], lends[0], lforward);
231 Math::sub3(lpos, lends[0], ltopos);
232 float fwlen = Math::mag3(lforward);
234 return (Math::dot3(ltopos, lforward)/fwlen + off)/fwlen;
237 void Launchbar::getPosOnCat(float perc, float* lpos, float* lvel,
238 float lends[2][3], float lendvels[2][3])
245 // Compute the forward direction of the cat.
247 Math::sub3(lends[1], lends[0], lforward);
248 Math::mul3(perc, lforward, lpos);
249 Math::add3(lends[0], lpos, lpos);
252 Math::mul3(perc, lendvels[0], lvel);
253 Math::mul3(1.0f-perc, lendvels[1], tmp);
254 Math::add3(tmp, lvel, lvel);
257 void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot)
259 // Init the return values
261 for(i=0; i<3; i++) _launchbar_force[i] = 0;
262 for(i=0; i<3; i++) _holdback_force[i] = 0;
264 if (_state != Unmounted)
267 // Don't bother if it's fully retracted
271 // For the first guess, the position fraction is equal to the
273 _frac = _h_frac = _extension;
275 // The ground plane transformed to the local frame.
277 s->planeGlobalToLocal(_global_ground, ground);
279 // The launchbar tip in local coordinates.
281 getTipPosition(ltip);
283 // Correct the extension value for no intersection.
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);
291 // Compute the distance of the launchbar mount point from the
293 float mountdist = ground[3] - Math::dot3(_launchbar_mount, ground);
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))
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];
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;
310 _frac = (ang - _up_ang)/(_down_ang - _up_ang);
316 // Now do it again for the holdback
318 // The holdback tip in local coordinates.
320 getHoldbackTipPosition(htip);
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;
328 // Compute the distance of the holdback mount point from the ground
330 float h_mountdist = ground[3] - Math::dot3(_holdback_mount, ground);
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))
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];
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);
347 _h_frac = _extension;
352 getTipPosition(llb_mount);
354 // The launchbar tip in global coordinates.
355 double launchbar_pos[3];
356 s->posLocalToGlobal(llb_mount, launchbar_pos);
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.
365 // Compute the positions of the catapult start and endpoints in the
366 // local coordinate system
368 s->posGlobalToLocal(end[0], lend[0]);
369 s->posGlobalToLocal(end[1], lend[1]);
371 // Transform the velocities of the endpoints to the
372 // local coordinate sytem.
374 s->velGlobalToLocal(vel[0], lvel[0]);
375 s->velGlobalToLocal(vel[1], lvel[1]);
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);
382 // Compute the direction from the launchbar mount at the gear
383 // to the lauchbar mount on the cat.
385 Math::sub3(llbtip, _launchbar_mount, llbdir);
386 float lblen = Math::mag3(llbdir);
387 Math::mul3(1.0f/lblen, llbdir, llbdir);
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.
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);
403 // We cannot arrest at the cat if we move too fast wrt the cat.
404 if (0.2 < Math::mag3(lv_mount))
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);
412 // don't let the calculation go -ve here
413 if (_holdback_length*_holdback_length - hbdist*hbdist < 0)
415 float offset = -Math::sqrt(_holdback_length*_holdback_length
417 _pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
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)
423 // Now we are arrested at the cat.
424 // The force is applied at the next step.
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.
435 Math::sub3(lhldbk_cmount, _holdback_mount, lhldbkdir);
436 float hldbklen = Math::mag3(lhldbkdir);
437 Math::mul3(1/hldbklen, lhldbkdir, lhldbkdir);
439 if (_state == Arrested) {
440 // Now apply a constant tension from the catapult over the launchbar.
441 Math::mul3(2.0, llbdir, _launchbar_force);
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
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);
458 // The velocity of the holdback mount at the gear wrt the
459 // holdback mount at the cat.
461 Math::sub3(lvhldbk_gmount, lvhldbk_cmount, lvhldbk);
463 // The spring force the holdback will apply to the gear
465 Math::mul3(10.f*(hldbklen - _holdback_length), lhldbkdir,
468 // The damping force here ...
469 Math::mul3(2e0, lvhldbk, tmp);
470 Math::sub3(_holdback_force, tmp, _holdback_force);
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);
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
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);
506 }; // namespace yasim