src/FDM/YASim/Hook.cpp

   1 #include "Math.hpp"
   2 #include "BodyEnvironment.hpp"
   3 #include "Ground.hpp"
   4 #include "RigidBody.hpp"
   5
   6 #include "Hook.hpp"
   7 namespace yasim {
   8
   9 static const float YASIM_PI2 = 3.14159265358979323846/2;
  10
  11 Hook::Hook()
  12 {
  13     int i;
  14     for(i=0; i<3; i++)
  15         _pos[i] = _force[i] = 0;
  16     for(i=0; i<2; i++)
  17         _global_ground[i] = 0;
  18     _global_ground[2] = 1;
  19     _global_ground[3] = -1e5;
  20     _length = 0.0;
  21     _down_ang = 0.0;
  22     _up_ang = 0.0;
  23     _extension = 0.0;
  24     _frac = 0.0;
  25     _has_wire = false;
  26 }
  27
  28 void Hook::setPosition(float* position)
  29 {
  30     int i;
  31     for(i=0; i<3; i++) _pos[i] = position[i];
  32 }
  33
  34 void Hook::setLength(float length)
  35 {
  36     _length = length;
  37 }
  38
  39 void Hook::setDownAngle(float ang)
  40 {
  41     _down_ang = ang;
  42 }
  43
  44 void Hook::setUpAngle(float ang)
  45 {
  46     _up_ang = ang;
  47 }
  48
  49 void Hook::setExtension(float extension)
  50 {
  51     _extension = extension;
  52 }
  53
  54 void Hook::setGlobalGround(double *global_ground)
  55 {
  56     int i;
  57     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
  58 }
  59
  60 void Hook::getPosition(float* out)
  61 {
  62     int i;
  63     for(i=0; i<3; i++) out[i] = _pos[i];
  64 }
  65
  66 float Hook::getHookPos(int i)
  67 {
  68     return _pos[i];
  69 }
  70
  71 float Hook::getLength(void)
  72 {
  73     return _length;
  74 }
  75
  76 float Hook::getDownAngle(void)
  77 {
  78     return _down_ang;
  79 }
  80
  81 float Hook::getUpAngle(void)
  82 {
  83     return _up_ang;
  84 }
  85
  86 float Hook::getAngle(void)
  87 {
  88     return _ang;
  89 }
  90
  91 float Hook::getExtension(void)
  92 {
  93     return _extension;
  94 }
  95
  96 void Hook::getForce(float* force, float* off)
  97 {
  98     Math::set3(_force, force);
  99     Math::set3(_pos, off);
 100 }
 101
 102 float Hook::getCompressFraction()
 103 {
 104     return _frac;
 105 }
 106
 107 void Hook::getTipPosition(float* out)
 108 {
 109     // The hook tip in local coordinates.
 110     _ang = _frac*(_down_ang - _up_ang) + _up_ang;
 111     float pos_tip[3] = { _length*Math::cos(_ang), 0, _length*Math::sin(_ang) };
 112     Math::sub3(_pos, pos_tip, out);
 113 }
 114
 115 void Hook::getTipGlobalPosition(State* s, double* out)
 116 {
 117     // The hook tip in local coordinates.
 118     float pos_tip[3];
 119     getTipPosition(pos_tip);
 120     // The hook tip in global coordinates.
 121     s->posLocalToGlobal(pos_tip, out);
 122 }
 123
 124 void Hook::calcForce(Ground* g_cb, RigidBody* body, State* s, float* lv, float* lrot)
 125 {
 126     // Init the return values
 127     int i;
 128     for(i=0; i<3; i++) _force[i] = 0;
 129
 130     // Don't bother if it's fully retracted
 131     if(_extension <= 0)
 132         return;
 133
 134     // For the first guess, the position fraction is equal to the
 135     // extension value.
 136     _frac = _extension;
 137
 138     // The ground plane transformed to the local frame.
 139     float ground[4];
 140     s->planeGlobalToLocal(_global_ground, ground);
 141
 142     // The hook tip in local coordinates.
 143     float ltip[3];
 144     getTipPosition(ltip);
 145
 146
 147     // Correct the extension value for no intersection.
 148
 149     // Check if the tip will intersect the ground or not. That is, compute
 150     // the distance of the tip to the ground plane.
 151     float tipdist = ground[3] - Math::dot3(ltip, ground);
 152     if(0 <= tipdist) {
 153         _frac = _extension;
 154     } else {
 155         // Compute the distance of the hooks mount point from the ground plane.
 156         float mountdist = ground[3] - Math::dot3(_pos, ground);
 157
 158         // Compute the distance of the hooks mount point from the ground plane
 159         // in the x-z plane. It holds:
 160         //  mountdist = mountdist_xz*cos(angle(normal_yz, e_z))
 161         // thus
 162         float mountdist_xz = _length;
 163         if (ground[2] != 0) {
 164             float nrm_yz = Math::sqrt(ground[1]*ground[1]+ground[2]*ground[2]);
 165             mountdist_xz = -mountdist*nrm_yz/ground[2];
 166         }
 167
 168         if (mountdist_xz < _length) {
 169             float ang = Math::asin(mountdist_xz/_length)
 170               + Math::atan2(ground[2], ground[0]) + YASIM_PI2;
 171             _frac = (ang - _up_ang)/(_down_ang - _up_ang);
 172         } else {
 173             _frac = _extension;
 174         }
 175     }
 176
 177     double hook_area[4][3];
 178     // The hook mount in global coordinates.
 179     s->posLocalToGlobal(_pos, hook_area[1]);
 180
 181     // Recompute the hook tip in global coordinates.
 182     getTipGlobalPosition(s, hook_area[0]);
 183
 184     // The old positions.
 185     hook_area[2][0] = _old_mount[0];
 186     hook_area[2][1] = _old_mount[1];
 187     hook_area[2][2] = _old_mount[2];
 188     hook_area[3][0] = _old_tip[0];
 189     hook_area[3][1] = _old_tip[1];
 190     hook_area[3][2] = _old_tip[2];
 191
 192
 193     // Check if we caught a wire.
 194     // Returns true if we caught one.
 195     if (!_has_wire && g_cb->caughtWire(hook_area))
 196         _has_wire = true;
 197
 198
 199     // save actual position as old position ...
 200     _old_mount[0] = hook_area[1][0];
 201     _old_mount[1] = hook_area[1][1];
 202     _old_mount[2] = hook_area[1][2];
 203     _old_tip[0] = hook_area[0][0];
 204     _old_tip[1] = hook_area[0][1];
 205     _old_tip[2] = hook_area[0][2];
 206
 207     if (!_has_wire)
 208         return;
 209
 210     // Get the wire endpoints and their velocities wrt the earth.
 211     double dpos[2][3];
 212     float wire_vel[2][3];
 213     g_cb->getWire(dpos, wire_vel);
 214
 215     // Transform those to the local coordinate system
 216     float wire_lpos[2][3];
 217     s->posGlobalToLocal(dpos[0], wire_lpos[0]);
 218     s->posGlobalToLocal(dpos[1], wire_lpos[1]);
 219     s->velGlobalToLocal(wire_vel[0], wire_vel[0]);
 220     s->velGlobalToLocal(wire_vel[1], wire_vel[1]);
 221
 222     // Compute the velocity of the hooks mount point in the local frame.
 223     float mount_vel[3];
 224     body->pointVelocity(_pos, lrot, mount_vel);
 225     Math::add3(lv, mount_vel, mount_vel);
 226
 227     // The velocity of the hook mount point wrt the earth in
 228     // the local frame.
 229     float v_wrt_we[2][3];
 230     Math::sub3(mount_vel, wire_vel[0], v_wrt_we[0]);
 231     Math::sub3(mount_vel, wire_vel[1], v_wrt_we[1]);
 232
 233     float f[2][3];
 234     // The vector from the wire ends to the hook mount point.
 235     Math::sub3(_pos, wire_lpos[0], f[0]);
 236     Math::sub3(_pos, wire_lpos[1], f[1]);
 237
 238     // We only need the direction.
 239     float mf0 = Math::mag3(f[0]);
 240     float mf1 = Math::mag3(f[1]);
 241     Math::mul3(1.0/mf0, f[0], f[0]);
 242     Math::mul3(1.0/mf1, f[1], f[1]);
 243
 244     // The velocity of the wire wrt the wire ends at the wire
 245     // mount points.
 246     float v0 = Math::dot3(v_wrt_we[0], f[0]);
 247     float v1 = Math::dot3(v_wrt_we[1], f[1]);
 248
 249     // We assume that the wire slips through the hook. So the velocity
 250     // will be equal at both sides. So take the mean of both.
 251     float v = 0.5*(v0+v1);
 252
 253     // Release wire when we reach zero velocity.
 254     if (v <= 0.0) {
 255       _has_wire = false;
 256       g_cb->releaseWire();
 257       return;
 258     }
 259
 260     // The trick is to multiply with the current mass of the aircraft.
 261     // That way we control the acceleration and not the force. This is
 262     // the implicit calibration of the wires for aircrafts of
 263     // different mass.
 264     float mass = body->getTotalMass();
 265
 266     // The local force is the vector sum of the force on each wire.
 267     // The force is computed with some constant tension on the wires
 268     // (80000N) plus a velocity dependent component.
 269     Math::add3(f[0], f[1], _force);
 270     Math::mul3(-mass*( 1.0 + ((mf0+mf1)/70) + 0.2*v ), _force, _force);
 271
 272     // Now in the body coordinate system, eliminate the Y coord part
 273     // of the hook force. Physically this means that the wire will just
 274     // slip through the hook instead of applying a side force.
 275     _force[1] = 0.0;
 276 }
 277
 278 }; // namespace yasim
 279