Add the missing carrier files.

src/FDM/YASim/FGGround.cpp

@@ -0,0 +1,72 @@
+#include <FDM/flight.hxx>
+
+#include "Glue.hpp"
+#include "Ground.hpp"
+
+#include "FGGround.hpp"
+namespace yasim {
+
+FGGround::FGGround(FGInterface *iface) : _iface(iface)
+{
+    _toff = 0.0;
+}
+
+FGGround::~FGGround()
+{
+}
+
+void FGGround::getGroundPlane(const double pos[3],
+                              double plane[4], float vel[3])
+{
+    // Return values for the callback.
+    double loadCapacity, frictionFactor, agl;
+    double cp[3], dvel[3];
+    int type;
+    _iface->get_agl_m(_toff, pos, cp, plane, dvel,
+                      &type, &loadCapacity, &frictionFactor, &agl);
+
+    // The plane below the actual contact point.
+    plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];
+
+    for(int i=0; i<3; i++) vel[i] = dvel[i];
+}
+
+bool FGGround::caughtWire(const double pos[4][3])
+{
+    return _iface->caught_wire_m(_toff, pos);
+}
+
+bool FGGround::getWire(double end[2][3], float vel[2][3])
+{
+    double dvel[2][3];
+    bool ret = _iface->get_wire_ends_m(_toff, end, dvel);
+    for (int i=0; i<2; ++i)
+      for (int j=0; j<3; ++j)
+        vel[i][j] = dvel[i][j];
+    return ret;
+}
+
+void FGGround::releaseWire(void)
+{
+    _iface->release_wire();
+}
+
+float FGGround::getCatapult(const double pos[3], double end[2][3],
+                            float vel[2][3])
+{
+    double dvel[2][3];
+    float dist = _iface->get_cat_m(_toff, pos, end, dvel);
+    for (int i=0; i<2; ++i)
+      for (int j=0; j<3; ++j)
+        vel[i][j] = dvel[i][j];
+    return dist;
+}
+
+void FGGround::setTimeOffset(double toff)
+{
+    _toff = toff;
+}
+
+
+}; // namespace yasim
+

src/FDM/YASim/FGGround.hpp

@@ -0,0 +1,38 @@
+#ifndef _FGGROUND_HPP
+#define _FGGROUND_HPP
+
+#include "Ground.hpp"
+
+class FGInterface;
+
+namespace yasim {
+
+// The XYZ coordinate system has Z as the earth's axis, the Y axis
+// pointing out the equator at zero longitude, and the X axis pointing
+// out the middle of the western hemisphere.
+class FGGround : public Ground {
+public:
+    FGGround(FGInterface *iface);
+    virtual ~FGGround();
+
+    virtual void getGroundPlane(const double pos[3],
+                                double plane[4], float vel[3]);
+
+    virtual bool caughtWire(const double pos[4][3]);
+
+    virtual bool getWire(double end[2][3], float vel[2][3]);
+
+    virtual void releaseWire(void);
+
+    virtual float getCatapult(const double pos[3],
+                              double end[2][3], float vel[2][3]);
+
+    void setTimeOffset(double toff);
+
+private:
+    FGInterface *_iface;
+    double _toff;
+};
+
+}; // namespace yasim
+#endif // _FGGROUND_HPP

src/FDM/YASim/Ground.cpp

@@ -0,0 +1,52 @@
+#include "Glue.hpp"
+
+#include "Ground.hpp"
+namespace yasim {
+
+Ground::Ground()
+{
+}
+
+Ground::~Ground()
+{
+}
+
+void Ground::getGroundPlane(const double pos[3],
+                            double plane[4], float vel[3])
+{
+    // ground.  Calculate a cartesian coordinate for the ground under
+    // us, find the (geodetic) up vector normal to the ground, then
+    // use that to find the final (radius) term of the plane equation.
+    float up[3];
+    Glue::geodUp((double*)pos, up);
+    int i;
+    for(i=0; i<3; i++) plane[i] = up[i];
+    plane[3] = plane[0]*pos[0] + plane[1]*pos[1] + plane[2]*pos[2];
+
+    vel[0] = 0.0;
+    vel[1] = 0.0;
+    vel[2] = 0.0;
+}
+
+bool Ground::caughtWire(const double pos[4][3])
+{
+    return false;
+}
+
+bool Ground::getWire(double end[2][3], float vel[2][3])
+{
+    return false;
+}
+
+void Ground::releaseWire(void)
+{
+}
+
+float Ground::getCatapult(const double pos[3], double end[2][3],
+                          float vel[2][3])
+{
+    return 1e10;
+}
+
+}; // namespace yasim
+

src/FDM/YASim/Ground.hpp

@@ -0,0 +1,25 @@
+#ifndef _GROUND_HPP
+#define _GROUND_HPP
+
+namespace yasim {
+
+class Ground {
+public:
+    Ground();
+    virtual ~Ground();
+
+    virtual void getGroundPlane(const double pos[3],
+                                double plane[4], float vel[3]);
+
+    virtual bool caughtWire(const double pos[4][3]);
+
+    virtual bool getWire(double end[2][3], float vel[2][3]);
+
+    virtual void releaseWire(void);
+
+    virtual float getCatapult(const double pos[3],
+                              double end[2][3], float vel[2][3]);
+};
+
+}; // namespace yasim
+#endif // _GROUND_HPP

src/FDM/YASim/Hook.cpp

@@ -0,0 +1,269 @@
+#include "Math.hpp"
+#include "BodyEnvironment.hpp"
+#include "Ground.hpp"
+#include "RigidBody.hpp"
+
+#include "Hook.hpp"
+namespace yasim {
+
+static const float YASIM_PI2 = 3.14159265358979323846/2;
+
+Hook::Hook()
+{
+    int i;
+    for(i=0; i<3; i++)
+	_pos[i] = _force[i] = 0;
+    for(i=0; i<2; i++)
+	_global_ground[i] = 0;
+    _global_ground[2] = 1;
+    _global_ground[3] = -1e5;
+    _length = 0.0;
+    _down_ang = 0.0;
+    _up_ang = 0.0;
+    _extension = 0.0;
+    _frac = 0.0;
+    _has_wire = false;
+}
+
+void Hook::setPosition(float* position)
+{
+    int i;
+    for(i=0; i<3; i++) _pos[i] = position[i];
+}
+
+void Hook::setLength(float length)
+{
+    _length = length;
+}
+
+void Hook::setDownAngle(float ang)
+{
+    _down_ang = ang;
+}
+
+void Hook::setUpAngle(float ang)
+{
+    _up_ang = ang;
+}
+
+void Hook::setExtension(float extension)
+{
+    _extension = extension;
+}
+
+void Hook::setGlobalGround(double *global_ground)
+{
+    int i;
+    for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
+}
+
+void Hook::getPosition(float* out)
+{
+    int i;
+    for(i=0; i<3; i++) out[i] = _pos[i];
+}
+
+float Hook::getLength(void)
+{
+    return _length;
+}
+
+float Hook::getDownAngle(void)
+{
+    return _down_ang;
+}
+
+float Hook::getUpAngle(void)
+{
+    return _up_ang;
+}
+
+float Hook::getExtension(void)
+{
+    return _extension;
+}
+
+void Hook::getForce(float* force, float* off)
+{
+    Math::set3(_force, force);
+    Math::set3(_pos, off);
+}
+
+float Hook::getCompressFraction()
+{
+    return _frac;
+}
+
+void Hook::getTipPosition(float* out)
+{
+    // The hook tip in local coordinates.
+    float ang = _frac*(_down_ang - _up_ang) + _up_ang;
+    float pos_tip[3] = { _length*Math::cos(ang), 0.0, _length*Math::sin(ang) };
+    Math::sub3(_pos, pos_tip, out);
+}
+
+void Hook::getTipGlobalPosition(State* s, double* out)
+{
+    // The hook tip in local coordinates.
+    float pos_tip[3];
+    getTipPosition(pos_tip);
+    // The hook tip in global coordinates.
+    s->posLocalToGlobal(pos_tip, out);
+}
+
+void Hook::calcForce(Ground* g_cb, RigidBody* body, State* s, float* lv, float* lrot)
+{
+    // Init the return values
+    int i;
+    for(i=0; i<3; i++) _force[i] = 0;
+
+    // Don't bother if it's fully retracted
+    if(_extension <= 0)
+	return;
+
+    // For the first guess, the position fraction is equal to the
+    // extension value.
+    _frac = _extension;
+
+    // The ground plane transformed to the local frame.
+    float ground[4];
+    s->planeGlobalToLocal(_global_ground, ground);
+
+    // The hook tip in local coordinates.
+    float ltip[3];
+    getTipPosition(ltip);
+
+
+    // Correct the extension value for no intersection.
+    
+    // Check if the tip will intersect the ground or not. That is, compute
+    // the distance of the tip to the ground plane.
+    float tipdist = ground[3] - Math::dot3(ltip, ground);
+    if(0 <= tipdist) {
+        _frac = _extension;
+    } else {
+        // Compute the distance of the hooks mount point from the ground plane.
+        float mountdist = ground[3] - Math::dot3(_pos, ground);
+
+        // Compute the distance of the hooks mount point from the ground plane
+        // in the x-z plane. It holds:
+        //  mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
+        // thus
+        float mountdist_xz = _length;
+        if (ground[2] != 0) {
+            float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
+            mountdist_xz = -mountdist*nrm_yz/ground[2];
+        }
+
+        if (mountdist_xz < _length) {
+            float ang = Math::asin(mountdist_xz/_length)
+              + Math::atan2(ground[2], ground[0]) + YASIM_PI2;
+            _frac = (ang - _up_ang)/(_down_ang - _up_ang);
+        } else {
+            _frac = _extension;
+        }
+    }
+
+    double hook_area[4][3];
+    // The hook mount in global coordinates.
+    s->posLocalToGlobal(_pos, hook_area[1]);
+
+    // Recompute the hook tip in global coordinates.
+    getTipGlobalPosition(s, hook_area[0]);
+
+    // The old positions.
+    hook_area[2][0] = _old_mount[0];
+    hook_area[2][1] = _old_mount[1];
+    hook_area[2][2] = _old_mount[2];
+    hook_area[3][0] = _old_tip[0];
+    hook_area[3][1] = _old_tip[1];
+    hook_area[3][2] = _old_tip[2];
+
+
+    // Check if we caught a wire.
+    // Returns true if we caught one.
+    if (!_has_wire && g_cb->caughtWire(hook_area))
+        _has_wire = true;
+
+
+    // save actual position as old position ...
+    _old_mount[0] = hook_area[1][0];
+    _old_mount[1] = hook_area[1][1];
+    _old_mount[2] = hook_area[1][2];
+    _old_tip[0] = hook_area[0][0];
+    _old_tip[1] = hook_area[0][1];
+    _old_tip[2] = hook_area[0][2];
+
+    if (!_has_wire)
+        return;
+
+    // Get the wire endpoints and their velocities wrt the earth.
+    double dpos[2][3];
+    float wire_vel[2][3];
+    g_cb->getWire(dpos, wire_vel);
+
+    // Transform those to the local coordinate system
+    float wire_lpos[2][3];
+    s->posGlobalToLocal(dpos[0], wire_lpos[0]);
+    s->posGlobalToLocal(dpos[1], wire_lpos[1]);
+    s->velGlobalToLocal(wire_vel[0], wire_vel[0]);
+    s->velGlobalToLocal(wire_vel[1], wire_vel[1]);
+
+    // Compute the velocity of the hooks mount point in the local frame.
+    float mount_vel[3];
+    body->pointVelocity(_pos, lrot, mount_vel);
+    Math::add3(lv, mount_vel, mount_vel);
+
+    // The velocity of the hook mount point wrt the earth in
+    // the local frame.
+    float v_wrt_we[2][3];
+    Math::sub3(mount_vel, wire_vel[0], v_wrt_we[0]);
+    Math::sub3(mount_vel, wire_vel[1], v_wrt_we[1]);
+
+    float f[2][3];
+    // The vector from the wire ends to the hook mount point.
+    Math::sub3(_pos, wire_lpos[0], f[0]);
+    Math::sub3(_pos, wire_lpos[1], f[1]);
+
+    // We only need the direction.
+    float mf0 = Math::mag3(f[0]);
+    float mf1 = Math::mag3(f[1]);
+    Math::mul3(1.0/mf0, f[0], f[0]);
+    Math::mul3(1.0/mf1, f[1], f[1]);
+
+    // The velocity of the wire wrt the wire ends at the wire
+    // mount points.
+    float v0 = Math::dot3(v_wrt_we[0], f[0]);
+    float v1 = Math::dot3(v_wrt_we[1], f[1]);
+
+    // We assume that the wire slips through the hook. So the velocity
+    // will be equal at both sides. So take the mean of both.
+    float v = 0.5*(v0+v1);
+
+    // Release wire when we reach zero velocity.
+    if (v <= 0.0) {
+      _has_wire = false;
+      g_cb->releaseWire();
+      return;
+    }
+
+    // The trick is to multiply with the current mass of the aircraft.
+    // That way we control the acceleration and not the force. This is
+    // the implicit calibration of the wires for aircrafts of
+    // different mass.
+    float mass = body->getTotalMass();
+  
+    // The local force is the vector sum of the force on each wire.
+    // The force is computed with some constant tension on the wires
+    // (80000N) plus a velocity dependent component.
+    Math::add3(f[0], f[1], _force);
+    Math::mul3(-mass*( 1.0 + ((mf0+mf1)/70) + 0.2*v ), _force, _force);
+  
+    // Now in the body coordinate system, eliminate the Y coord part
+    // of the hook force. Physically this means that the wire will just
+    // slip through the hook instead of applying a side force.
+    _force[1] = 0.0;
+}
+
+}; // namespace yasim
+

src/FDM/YASim/Hook.hpp

@@ -0,0 +1,64 @@
+#ifndef _HOOK_HPP
+#define _HOOK_HPP
+
+namespace yasim {
+
+class Ground;
+class RigidBody;
+class State;
+
+// A landing hook has the following parameters:
+//
+// position: a point in the aircraft's local coordinate system where the
+//     fully-extended wheel will be found.
+//
+class Hook {
+public:
+    Hook();
+
+    // Externally set values
+    void setPosition(float* position);
+    void setLength(float length);
+    void setDownAngle(float ang);
+    void setUpAngle(float ang);
+    void setExtension(float extension);
+    void setGlobalGround(double *global_ground);
+
+    void getPosition(float* out);
+    float getLength(void);
+    float getDownAngle(void);
+    float getUpAngle(void);
+    float getExtension(void);
+
+    void getTipPosition(float* out);
+    void getTipGlobalPosition(State* s, double* out);
+
+    // Takes a velocity of the aircraft relative to ground, a rotation
+    // vector, and a ground plane (all specified in local coordinates)
+    // and make a force and point of application (i.e. ground contact)
+    // available via getForce().
+    void calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot);
+
+    // Computed values: total force, weight-on-wheels (force normal to
+    // ground) and compression fraction.
+    void getForce(float* force, float* off);
+    float getCompressFraction(void);
+
+private:
+    float _pos[3];
+    float _length;
+    float _down_ang;
+    float _up_ang;
+    float _extension;
+    float _force[3];
+    float _frac;
+    bool _has_wire;
+
+    double _old_mount[3];
+    double _old_tip[3];
+
+    double _global_ground[4];
+};
+
+}; // namespace yasim
+#endif // _HOOK_HPP

src/FDM/YASim/Launchbar.cpp

@@ -0,0 +1,337 @@
+#include "Math.hpp"
+#include "BodyEnvironment.hpp"
+#include "Ground.hpp"
+#include "RigidBody.hpp"
+
+#include "Launchbar.hpp"
+
+#include <iostream>
+using namespace std;
+namespace yasim {
+
+Launchbar::Launchbar()
+{
+    int i;
+    for(i=0; i<3; i++)
+	_launchbar_mount[i] = _holdback_mount[i] = _force[i] = 0;
+    for(i=0; i<2; i++)
+	_global_ground[i] = 0;
+    _global_ground[2] = 1;
+    _global_ground[3] = -1e5;
+    _length = 0.0;
+    _holdback_length = 2.0;
+    _down_ang = 0.0;
+    _up_ang = 0.0;
+    _extension = 0.0;
+    _frac = 0.0;
+    _launch_cmd = false;
+    _pos_on_cat = 0.0;
+    _state = Unmounted;
+}
+
+void Launchbar::setLaunchbarMount(float* position)
+{
+    int i;
+    for(i=0; i<3; i++) _launchbar_mount[i] = position[i];
+}
+
+void Launchbar::setHoldbackMount(float* position)
+{
+    int i;
+    for(i=0; i<3; i++) _holdback_mount[i] = position[i];
+}
+
+void Launchbar::setLength(float length)
+{
+    _length = length;
+}
+
+void Launchbar::setDownAngle(float ang)
+{
+    _down_ang = ang;
+}
+
+void Launchbar::setUpAngle(float ang)
+{
+    _up_ang = ang;
+}
+
+void Launchbar::setExtension(float extension)
+{
+    _extension = extension;
+}
+
+void Launchbar::setLaunchCmd(bool cmd)
+{
+    _launch_cmd = cmd;
+}
+
+void Launchbar::setGlobalGround(double *global_ground)
+{
+    int i;
+    for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
+}
+
+void Launchbar::getLaunchbarMount(float* out)
+{
+    int i;
+    for(i=0; i<3; i++) out[i] = _launchbar_mount[i];
+}
+
+void Launchbar::getHoldbackMount(float* out)
+{
+    int i;
+    for(i=0; i<3; i++) out[i] = _holdback_mount[i];
+}
+
+float Launchbar::getLength(void)
+{
+    return _length;
+}
+
+float Launchbar::getDownAngle(void)
+{
+    return _down_ang;
+}
+
+float Launchbar::getUpAngle(void)
+{
+    return _up_ang;
+}
+
+float Launchbar::getExtension(void)
+{
+    return _extension;
+}
+
+void Launchbar::getForce(float* force, float* off)
+{
+    Math::set3(_force, force);
+    Math::set3(_launchbar_mount, off);
+}
+
+float Launchbar::getCompressFraction()
+{
+    return _frac;
+}
+
+void Launchbar::getTipPosition(float* out)
+{
+    // The launchbar tip in local coordinates.
+    float ang = _frac*(_down_ang - _up_ang) + _up_ang;
+    float pos_tip[3] = { _length*Math::cos(ang), 0.0,-_length*Math::sin(ang) };
+    Math::add3(_launchbar_mount, pos_tip, out);
+}
+
+void Launchbar::getTipGlobalPosition(State* s, double* out)
+{
+    // The launchbar tip in local coordinates.
+    float pos_tip[3];
+    getTipPosition(pos_tip);
+    // The launchbar tip in global coordinates.
+    s->posLocalToGlobal(pos_tip, out);
+}
+
+float Launchbar::getPercentPosOnCat(float* lpos, float off, float lends[2][3])
+{
+    // Compute the forward direction of the cat.
+    float lforward[3];
+    Math::sub3(lends[1], lends[0], lforward);
+    float ltopos[3];
+    Math::sub3(lpos, lends[0], ltopos);
+    float fwlen = Math::mag3(lforward);
+    
+    return (Math::dot3(ltopos, lforward)/fwlen + off)/fwlen;
+}
+
+void Launchbar::getPosOnCat(float perc, float* lpos, float* lvel,
+                            float lends[2][3], float lendvels[2][3])
+{
+    if (perc < 0.0)
+        perc = 0.0;
+    if (1.0 < perc)
+        perc = 1.0;
+
+    // Compute the forward direction of the cat.
+    float lforward[3];
+    Math::sub3(lends[1], lends[0], lforward);
+    Math::mul3(perc, lforward, lpos);
+    Math::add3(lends[0], lpos, lpos);
+    
+    float tmp[3];
+    Math::mul3(perc, lendvels[0], lvel);
+    Math::mul3(1.0-perc, lendvels[1], tmp);
+    Math::add3(tmp, lvel, lvel);
+}
+
+void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot)
+{
+    // Init the return values
+    int i;
+    for(i=0; i<3; i++) _force[i] = 0;
+
+    if (_state != Unmounted)
+      _extension = 1;
+
+    // Don't bother if it's fully retracted
+    if(_extension <= 0)
+	return;
+
+    if (_extension < _frac)
+        _frac = _extension;
+
+    // The launchbar tip in global coordinates.
+    double launchbar_pos[3];
+    getTipGlobalPosition(s, launchbar_pos);
+
+    // If the launchbars tip is less extended than it could be.
+    if(_frac < _extension) {
+        // Correct the extension value for no intersection.
+        // Compute the distance of the mount point from the ground plane.
+        double a = - _global_ground[3] + launchbar_pos[0]*_global_ground[0]
+          + launchbar_pos[1]*_global_ground[1]
+          + launchbar_pos[2]*_global_ground[2];
+        if(a < _length) {
+            float ang = Math::asin(a/_length);
+            _frac = (ang - _up_ang)/(_down_ang - _up_ang);
+        } else
+            // FIXME: this will jump 
+            _frac = _extension;
+    }
+
+    // Recompute the launchbar tip.
+    float llb_mount[3];
+    getTipPosition(llb_mount);
+    // The launchbar tip in global coordinates.
+    s->posLocalToGlobal(llb_mount, launchbar_pos);
+
+    double end[2][3]; float vel[2][3];
+    float dist = g_cb->getCatapult(launchbar_pos, end, vel);
+    // Work around a problem of flightgear returning totally screwed up
+    // scenery when switching views.
+    if (1e3 < dist)
+        return;
+
+    // Compute the positions of the catapult start and endpoints in the
+    // local coordiante system
+    float lend[2][3];
+    s->posGlobalToLocal(end[0], lend[0]);
+    s->posGlobalToLocal(end[1], lend[1]);
+    
+    // Transform the velocities of the endpoints to the
+    // local coordinate sytem.
+    float lvel[2][3];
+    s->velGlobalToLocal(vel[0], lvel[0]);
+    s->velGlobalToLocal(vel[1], lvel[1]);
+
+    // Compute the position of the launchbar tip relative to the cat.
+    float tip_pos_on_cat = getPercentPosOnCat(llb_mount, 0.0, lend);
+    float llbtip[3], lvlbtip[3];
+    getPosOnCat(tip_pos_on_cat, llbtip, lvlbtip, lend, lvel);
+
+    // Compute the direction from the launchbar mount at the gear
+    // to the lauchbar mount on the cat.
+    float llbdir[3];
+    Math::sub3(llbtip, _launchbar_mount, llbdir);
+    float lblen = Math::mag3(llbdir);
+    Math::mul3(1.0/lblen, llbdir, llbdir);
+
+    // Check if we are near enough to the cat.
+    if (_state == Unmounted && dist < 0.5) {
+        // croase approximation for the velocity of the launchbar.
+        // Might be sufficient because arresting at the cat makes only
+        // sense when the aircraft does not rotate much.
+        float lv_mount[3];
+        float tmp[3];
+        float lrot[3], lv[3];
+        Math::vmul33(s->orient, s->rot, lrot);
+        Math::vmul33(s->orient, s->v, lv);
+        body->pointVelocity(llb_mount, lrot, tmp);
+        Math::sub3(tmp, lvlbtip, lv_mount);
+        Math::add3(lv, lv_mount, lv_mount);
+
+        // We cannot arrest at the cat if we move too fast wrt the cat.
+        if (0.2 < Math::mag3(lv_mount))
+            return;
+
+        // Compute the position of the holdback mount relative to the cat.
+        double dd[2][3]; float fd[2][3]; double ghldbkpos[3];
+        s->posLocalToGlobal(_holdback_mount, ghldbkpos);
+        float hbdist = g_cb->getCatapult(ghldbkpos, dd, fd);
+        float offset = -Math::sqrt(_holdback_length*_holdback_length - hbdist*hbdist);
+        _pos_on_cat = getPercentPosOnCat(_holdback_mount, offset, lend);
+
+
+        // We cannot arrest if we are not at the start of the cat.
+        if (_pos_on_cat < 0.0 || 0.2 < _pos_on_cat)
+            return;
+
+        // Now we are arrested at the cat.
+        // The force is applied at the next step.
+        _state = Arrested;
+        return;
+    }
+
+    // Get the actual distance from the holdback to its mountpoint
+    // on the cat. If it is longer than the holdback apply a force.
+    float lhldbk_cmount[3]; float lvhldbk_cmount[3];
+    getPosOnCat(_pos_on_cat, lhldbk_cmount, lvhldbk_cmount, lend, lvel);
+    // Compute the direction of holdback.
+    float lhldbkdir[3];
+    Math::sub3(lhldbk_cmount, _holdback_mount, lhldbkdir);
+    float hldbklen = Math::mag3(lhldbkdir);
+    Math::mul3(1/hldbklen, lhldbkdir, lhldbkdir);
+    
+    if (_state == Arrested) {
+        // Now apply a constant tension from the catapult over the launchbar.
+        Math::mul3(2.0, llbdir, _force);
+
+        // If the distance from the holdback mount at the aircraft to the
+        // holdback mount on the cat is larger than the holdback length itself,
+        // the holdback applies a force to the gear.
+        if (_holdback_length < hldbklen) {
+            // croase approximation for the velocity of the holdback mount
+            // at the gear.
+            // Might be sufficient because arresting at the cat makes only
+            // sense when the aircraft does not rotate much.
+            float lvhldbk_gmount[3];
+            float lrot[3], lv[3];
+            Math::vmul33(s->orient, s->rot, lrot);
+            Math::vmul33(s->orient, s->v, lv);
+            body->pointVelocity(_holdback_mount, lrot, lvhldbk_gmount);
+            Math::add3(lv, lvhldbk_gmount, lvhldbk_gmount);
+
+            // The velocity of the holdback mount at the gear wrt the
+            // holdback mount at the cat.
+            float lvhldbk[3];
+            Math::sub3(lvhldbk_gmount, lvhldbk_cmount, lvhldbk);
+
+            // The spring force the holdback will apply to the gear
+            float tmp[3];
+            Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir, tmp);
+            Math::add3(tmp, _force, _force);
+
+            // The damping force here ...
+            Math::mul3(2e0, lvhldbk, tmp);
+            Math::sub3(_force, tmp, _force);
+        }
+
+        if (_launch_cmd)
+            _state = Launch;
+    }
+
+    if (_state == Launch) {
+        // Now apply a constant tension from the catapult over the launchbar.
+        Math::mul3(25.0, llbdir, _force);
+
+        if (1.0 < dist)
+            _state = Unmounted;
+    }
+
+    // Scale by the mass. That keeps the stiffness in reasonable bounds.
+    float mass = body->getTotalMass();
+    Math::mul3(mass, _force, _force);
+}
+
+}; // namespace yasim
+

src/FDM/YASim/Launchbar.hpp

@@ -0,0 +1,73 @@
+#ifndef _LAUNCHBAR_HPP
+#define _LAUNCHBAR_HPP
+
+namespace yasim {
+
+class Ground;
+class RigidBody;
+class State;
+
+// A launchbar has the following parameters:
+//
+// position: a point in the aircraft's local coordinate system where the
+//     fully-extended wheel will be found.
+//
+class Launchbar {
+public:
+    enum LBState { Arrested, Launch, Unmounted };
+
+    Launchbar();
+
+    // Externally set values
+    void setLaunchbarMount(float* position);
+    void setHoldbackMount(float* position);
+    void setLength(float length);
+    void setDownAngle(float ang);
+    void setUpAngle(float ang);
+    void setExtension(float extension);
+    void setLaunchCmd(bool cmd);
+    void setGlobalGround(double *global_ground);
+
+    void getLaunchbarMount(float* out);
+    void getHoldbackMount(float* out);
+    float getLength(void);
+    float getDownAngle(void);
+    float getUpAngle(void);
+    float getExtension(void);
+
+    void getTipPosition(float* out);
+    void getTipGlobalPosition(State* s, double* out);
+
+    float getPercentPosOnCat(float* lpos, float off, float lends[2][3]);
+    void getPosOnCat(float perc, float* lpos, float* lvel,
+                     float lends[2][3], float lendvels[2][3]);
+
+    // Takes a velocity of the aircraft relative to ground, a rotation
+    // vector, and a ground plane (all specified in local coordinates)
+    // and make a force and point of application (i.e. ground contact)
+    // available via getForce().
+    void calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, float* lrot);
+
+    // Computed values: total force, weight-on-wheels (force normal to
+    // ground) and compression fraction.
+    void getForce(float* force, float* off);
+    float getCompressFraction(void);
+
+private:
+    float _launchbar_mount[3];
+    float _holdback_mount[3];
+    float _length;
+    float _holdback_length;
+    float _down_ang;
+    float _up_ang;
+    float _extension;
+    float _force[3];
+    float _frac;
+    float _pos_on_cat;
+    bool _launch_cmd;
+    double _global_ground[4];
+    LBState _state;
+};
+
+}; // namespace yasim
+#endif // _LAUNCHBAR_HPP