diff --git a/src/FDM/YASim/Hook.cpp b/src/FDM/YASim/Hook.cpp index 6a0368d6b..092404883 100644 --- a/src/FDM/YASim/Hook.cpp +++ b/src/FDM/YASim/Hook.cpp @@ -63,6 +63,11 @@ void Hook::getPosition(float* out) for(i=0; i<3; i++) out[i] = _pos[i]; } +float Hook::getHookPos(int i) +{ + return _pos[i]; +} + float Hook::getLength(void) { return _length; @@ -78,6 +83,11 @@ float Hook::getUpAngle(void) return _up_ang; } +float Hook::getAngle(void) +{ + return _ang; +} + float Hook::getExtension(void) { return _extension; @@ -97,8 +107,8 @@ float Hook::getCompressFraction() 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) }; + _ang = _frac*(_down_ang - _up_ang) + _up_ang; + float pos_tip[3] = { _length*Math::cos(_ang), 0, _length*Math::sin(_ang) }; Math::sub3(_pos, pos_tip, out); } diff --git a/src/FDM/YASim/Hook.hpp b/src/FDM/YASim/Hook.hpp index 028a86680..c44d45786 100644 --- a/src/FDM/YASim/Hook.hpp +++ b/src/FDM/YASim/Hook.hpp @@ -44,11 +44,15 @@ public: void getForce(float* force, float* off); float getCompressFraction(void); + float Hook::getAngle(void); + float Hook::getHookPos(int i); + private: float _pos[3]; float _length; float _down_ang; float _up_ang; + float _ang; float _extension; float _force[3]; float _frac; diff --git a/src/FDM/YASim/Launchbar.cpp b/src/FDM/YASim/Launchbar.cpp index 34a438b61..ebe599999 100644 --- a/src/FDM/YASim/Launchbar.cpp +++ b/src/FDM/YASim/Launchbar.cpp @@ -5,15 +5,19 @@ #include "Launchbar.hpp" -#include using namespace std; namespace yasim { + static const float YASIM_PI2 = 3.14159265358979323846/2; + static const float YASIM_PI = 3.14159265358979323846; + static const float RAD2DEG = 180/YASIM_PI; + Launchbar::Launchbar() { int i; for(i=0; i<3; i++) - _launchbar_mount[i] = _holdback_mount[i] = _force[i] = 0; + _launchbar_mount[i] = _holdback_mount[i] = _launchbar_force[i] + = _holdback_force[i] = 0; for(i=0; i<2; i++) _global_ground[i] = 0; _global_ground[2] = 1; @@ -22,11 +26,13 @@ Launchbar::Launchbar() _holdback_length = 2.0; _down_ang = 0.0; _up_ang = 0.0; + _ang = 0.0; _extension = 0.0; - _frac = 0.0; + _frac = _h_frac =0.0; _launch_cmd = false; _pos_on_cat = 0.0; _state = Unmounted; + _strop = false; } void Launchbar::setLaunchbarMount(float* position) @@ -46,6 +52,11 @@ void Launchbar::setLength(float length) _length = length; } + void Launchbar::setHoldbackLength(float length) + { + _holdback_length = length; + } + void Launchbar::setDownAngle(float ang) { _down_ang = ang; @@ -78,12 +89,27 @@ void Launchbar::getLaunchbarMount(float* out) for(i=0; i<3; i++) out[i] = _launchbar_mount[i]; } + float Launchbar::getLaunchbarPos(int i) + { + return _launchbar_mount[i]; + } + void Launchbar::getHoldbackMount(float* out) { int i; for(i=0; i<3; i++) out[i] = _holdback_mount[i]; } + float Launchbar::getHoldbackPos(int j) + { + return _holdback_mount[j]; + } + + float Launchbar::getHoldbackLength(void) + { + return _holdback_length; + } + float Launchbar::getLength(void) { return _length; @@ -99,15 +125,46 @@ float Launchbar::getUpAngle(void) return _up_ang; } + float Launchbar::getAngle(void) + { + return _ang; + } + + float Launchbar::getHoldbackAngle(void) + { + return _h_ang; + } float Launchbar::getExtension(void) { return _extension; } -void Launchbar::getForce(float* force, float* off) + void Launchbar::getForce(float* force1, float* off1, + float* force2, float* off2) + { + Math::set3(_launchbar_force, force1); + Math::set3(_launchbar_mount, off1); + Math::set3(_holdback_force, force2); + Math::set3(_holdback_mount, off2); + } + + const char* Launchbar::getState(void) + { + switch (_state) { + case Arrested: + return "Engaged"; + case Launch: + return "Launching"; + case Completed: + return "Completed"; + default: + return "Disengaged"; + } + } + + bool Launchbar::getStrop(void) { - Math::set3(_force, force); - Math::set3(_launchbar_mount, off); + return _strop; } float Launchbar::getCompressFraction() @@ -115,14 +172,43 @@ float Launchbar::getCompressFraction() return _frac; } + float Launchbar::getHoldbackCompressFraction() + { + return _h_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); + + _ang = _frac*(_down_ang - _up_ang ) + _up_ang ; + float ptip[3] = { _length*Math::cos(_ang), 0, -_length*Math::sin(_ang) }; + Math::add3(_launchbar_mount, ptip, out); + } + + float Launchbar::getTipPos(int i) + { + float pos_tip[3]; + getTipPosition(pos_tip); + return pos_tip[i]; + } + + void Launchbar::getHoldbackTipPosition(float* out) + { + // The holdback tip in local coordinates. + _h_ang = _h_frac*(_down_ang - _up_ang) + _up_ang; + float htip[3] = { -_length*Math::cos(_h_ang), 0, -_length*Math::sin(_h_ang) }; + Math::add3(_holdback_mount, htip, out); + } + + float Launchbar::getHoldbackTipPos(int i) + { + float pos_tip[3]; + getHoldbackTipPosition(pos_tip); + return pos_tip[i]; } + void Launchbar::getTipGlobalPosition(State* s, double* out) { // The launchbar tip in local coordinates. @@ -168,7 +254,8 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl { // Init the return values int i; - for(i=0; i<3; i++) _force[i] = 0; + for(i=0; i<3; i++) _launchbar_force[i] = 0; + for(i=0; i<3; i++) _holdback_force[i] = 0; if (_state != Unmounted) _extension = 1; @@ -177,32 +264,91 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl if(_extension <= 0) return; - if (_extension < _frac) - _frac = _extension; + // For the first guess, the position fraction is equal to the + // extension value. + _frac = _h_frac = _extension; + + // The ground plane transformed to the local frame. + float ground[4]; + s->planeGlobalToLocal(_global_ground, ground); + + // The launchbar 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 launchbar mount point from the + // ground plane. + float mountdist = ground[3] - Math::dot3(_launchbar_mount, ground); + + // Compute the distance of the launchbar 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) { + // the launchbar points forward, so we need to change the signs here + float ang = -Math::asin(mountdist_xz/_length) + + Math::atan2(ground[2], ground[0]) + YASIM_PI2; + ang = -ang; + _frac = (ang - _up_ang)/(_down_ang - _up_ang); + } else { + _frac = _extension; + } + } + + // Now do it again for the holdback + + // The holdback tip in local coordinates. + float htip[3]; + getHoldbackTipPosition(htip); + + // Check if the tip will intersect the ground or not. That is, compute + // the distance of the tip to the ground plane. + float h_tipdist = ground[3] - Math::dot3(htip, ground); + if (0 <= h_tipdist) { + _h_frac = _extension; + } else { + // Compute the distance of the holdback mount point from the ground + // plane. + float h_mountdist = ground[3] - Math::dot3(_holdback_mount, ground); + + // Compute the distance of the holdback mount point from the ground + // plane in the x-z plane. It holds: + // mountdist = mountdist_xz*cos(angle(normal_yz, e_z)) + // thus + float h_mountdist_xz = _holdback_length; + if (ground[2] != 0) { + float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]); + h_mountdist_xz = -h_mountdist*nrm_yz/ground[2]; + } + + if (h_mountdist_xz < _holdback_length) { + float h_ang = Math::asin(h_mountdist_xz/_holdback_length) + + Math::atan2(ground[2], ground[0]) + YASIM_PI2; + _h_frac = (h_ang - _up_ang)/(_down_ang - _up_ang); + } else { + _h_frac = _extension; + } + } + + float llb_mount[3]; + getTipPosition(llb_mount); // 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]; @@ -213,7 +359,7 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl return; // Compute the positions of the catapult start and endpoints in the - // local coordiante system + // local coordinate system float lend[2][3]; s->posGlobalToLocal(end[0], lend[0]); s->posGlobalToLocal(end[1], lend[1]); @@ -237,8 +383,8 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl 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. + if (_state == Unmounted && dist < 0.6) { + // coarse 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]; @@ -258,12 +404,16 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl 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); + // don't let the calculation go -ve here + if (_holdback_length*_holdback_length - hbdist*hbdist < 0) + return; + 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) + if (_pos_on_cat < 0.0 || 0.4 < _pos_on_cat) return; // Now we are arrested at the cat. @@ -284,13 +434,13 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl if (_state == Arrested) { // Now apply a constant tension from the catapult over the launchbar. - Math::mul3(2.0, llbdir, _force); + Math::mul3(2.0, llbdir, _launchbar_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 + // coarse 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. @@ -308,29 +458,43 @@ void Launchbar::calcForce(Ground *g_cb, RigidBody* body, State* s, float* lv, fl // 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); + Math::mul3(1e1*(hldbklen - _holdback_length), lhldbkdir, + _holdback_force); // The damping force here ... Math::mul3(2e0, lvhldbk, tmp); - Math::sub3(_force, tmp, _force); + Math::sub3(_holdback_force, tmp, _holdback_force); } - if (_launch_cmd) + if (_launch_cmd) { _state = Launch; + _strop = false; + } } if (_state == Launch) { // Now apply a constant tension from the catapult over the launchbar. - Math::mul3(25.0, llbdir, _force); + Math::mul3(25.0, llbdir, _launchbar_force); - if (1.0 < dist) + if (1.0 < dist) { + _state = Completed; + } + } + + if (_state == Completed) { + // Wait until the strop has cleared the deck + // This is a temporary fix until we come up with something better + + if (_frac > 0.8) { _state = Unmounted; + _strop = true; + } } // Scale by the mass. That keeps the stiffness in reasonable bounds. float mass = body->getTotalMass(); - Math::mul3(mass, _force, _force); + Math::mul3(mass, _launchbar_force, _launchbar_force); + Math::mul3(mass, _holdback_force, _holdback_force); } }; // namespace yasim diff --git a/src/FDM/YASim/Launchbar.hpp b/src/FDM/YASim/Launchbar.hpp index 06b7bfab7..86733d882 100644 --- a/src/FDM/YASim/Launchbar.hpp +++ b/src/FDM/YASim/Launchbar.hpp @@ -14,7 +14,7 @@ struct State; // class Launchbar { public: - enum LBState { Arrested, Launch, Unmounted }; + enum LBState { Arrested, Launch, Unmounted, Completed }; Launchbar(); @@ -22,6 +22,7 @@ public: void setLaunchbarMount(float* position); void setHoldbackMount(float* position); void setLength(float length); + void setHoldbackLength(float length); void setDownAngle(float ang); void setUpAngle(float ang); void setExtension(float extension); @@ -30,12 +31,18 @@ public: void getLaunchbarMount(float* out); void getHoldbackMount(float* out); + const char* getState(void); float getLength(void); + float getHoldbackLength(void); float getDownAngle(void); float getUpAngle(void); float getExtension(void); + bool getStrop(void); void getTipPosition(float* out); + void getHoldbackTipPosition(float* out); + float getTipPos(int i); + float getHoldbackTipPos(int i); void getTipGlobalPosition(State* s, double* out); float getPercentPosOnCat(float* lpos, float off, float lends[2][3]); @@ -46,12 +53,18 @@ public: // 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); + 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); + void getForce(float* force1, float* off1, float* force2, float* off2); float getCompressFraction(void); + float getHoldbackCompressFraction(void); + float getAngle(void); + float getHoldbackAngle(void); + float getLaunchbarPos(int i); + float getHoldbackPos(int j); private: float _launchbar_mount[3]; @@ -60,11 +73,16 @@ private: float _holdback_length; float _down_ang; float _up_ang; + float _ang; + float _h_ang; float _extension; - float _force[3]; + float _launchbar_force[3]; + float _holdback_force[3]; float _frac; + float _h_frac; float _pos_on_cat; bool _launch_cmd; + bool _strop; double _global_ground[4]; LBState _state; }; diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 643b88fe1..ece48823d 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -480,9 +480,10 @@ void Model::calcForces(State* s) if(_launchbar) { float v[3], rot[3], glvel[3], ground[3]; _launchbar->calcForce(_ground_cb, &_body, s, lv, lrot); - float force[3], contact[3]; - _launchbar->getForce(force, contact); - _body.addForce(contact, force); + float forcelb[3], contactlb[3], forcehb[3], contacthb[3]; + _launchbar->getForce(forcelb, contactlb, forcehb, contacthb); + _body.addForce(contactlb, forcelb); + _body.addForce(contacthb, forcehb); } } diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 021931f1b..02c54174d 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -480,5 +480,9 @@ void YASim::copyFromYASim() if(l) { SGPropertyNode * node = fgGetNode("gear/launchbar", 0, true); node->setFloatValue("position-norm", l->getCompressFraction()); + node->setFloatValue("holdback-position-norm", l->getHoldbackCompressFraction()); + node->setStringValue("state", l->getState()); + node->setBoolValue("strop", l->getStrop()); } + }