--- /dev/null
+#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
+
--- /dev/null
+#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
--- /dev/null
+#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
+
--- /dev/null
+#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
--- /dev/null
+#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
+
--- /dev/null
+#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
--- /dev/null
+#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
+
--- /dev/null
+#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
projects / flightgear.git / commitdiff