diff --git a/src/FDM/YASim/FGGround.cpp b/src/FDM/YASim/FGGround.cpp new file mode 100644 index 000000000..6a82151df --- /dev/null +++ b/src/FDM/YASim/FGGround.cpp @@ -0,0 +1,72 @@ +#include + +#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 + diff --git a/src/FDM/YASim/FGGround.hpp b/src/FDM/YASim/FGGround.hpp new file mode 100644 index 000000000..3567c0217 --- /dev/null +++ b/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 diff --git a/src/FDM/YASim/Ground.cpp b/src/FDM/YASim/Ground.cpp new file mode 100644 index 000000000..3fc299d7f --- /dev/null +++ b/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 + diff --git a/src/FDM/YASim/Ground.hpp b/src/FDM/YASim/Ground.hpp new file mode 100644 index 000000000..eec8f997d --- /dev/null +++ b/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 diff --git a/src/FDM/YASim/Hook.cpp b/src/FDM/YASim/Hook.cpp new file mode 100644 index 000000000..6a0368d6b --- /dev/null +++ b/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 + diff --git a/src/FDM/YASim/Hook.hpp b/src/FDM/YASim/Hook.hpp new file mode 100644 index 000000000..591798722 --- /dev/null +++ b/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 diff --git a/src/FDM/YASim/Launchbar.cpp b/src/FDM/YASim/Launchbar.cpp new file mode 100644 index 000000000..34a438b61 --- /dev/null +++ b/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 +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 + diff --git a/src/FDM/YASim/Launchbar.hpp b/src/FDM/YASim/Launchbar.hpp new file mode 100644 index 000000000..dcdcb8a71 --- /dev/null +++ b/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