+bool
+FGJSBsim::get_agl_ft(double t, const double pt[3], double alt_off,
+ double contact[3], double normal[3], double vel[3],
+ double angularVel[3], double *agl)
+{
+ const SGMaterial* material;
+ simgear::BVHNode::Id id;
+ if (!FGInterface::get_agl_ft(t, pt, alt_off, contact, normal, vel,
+ angularVel, material, id))
+ return false;
+ SGGeod geodPt = SGGeod::fromCart(SG_FEET_TO_METER*SGVec3d(pt));
+ SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
+ *agl = dot(hlToEc.rotate(SGVec3d(0, 0, 1)), SGVec3d(contact) - SGVec3d(pt));
+ return true;
+}
+
+inline static double sqr(double x)
+{
+ return x * x;
+}
+
+static double angle_diff(double a, double b)
+{
+ double diff = fabs(a - b);
+ if (diff > 180) diff = 360 - diff;
+
+ return diff;
+}
+
+static void check_hook_solution(const FGColumnVector3& ground_normal_body, double E, double hook_length, double sin_fi_guess, double cos_fi_guess, double* sin_fis, double* cos_fis, double* fis, int* points)
+{
+ FGColumnVector3 tip(-hook_length * cos_fi_guess, 0, hook_length * sin_fi_guess);
+ double dist = DotProduct(tip, ground_normal_body);
+ if (fabs(dist + E) < 0.0001) {
+ sin_fis[*points] = sin_fi_guess;
+ cos_fis[*points] = cos_fi_guess;
+ fis[*points] = atan2(sin_fi_guess, cos_fi_guess) * SG_RADIANS_TO_DEGREES;
+ (*points)++;
+ }
+}
+
+
+static void check_hook_solution(const FGColumnVector3& ground_normal_body, double E, double hook_length, double sin_fi_guess, double* sin_fis, double* cos_fis, double* fis, int* points)
+{
+ if (sin_fi_guess >= -1 && sin_fi_guess <= 1) {
+ double cos_fi_guess = sqrt(1 - sqr(sin_fi_guess));
+ check_hook_solution(ground_normal_body, E, hook_length, sin_fi_guess, cos_fi_guess, sin_fis, cos_fis, fis, points);
+ if (fabs(cos_fi_guess) > SG_EPSILON) {
+ check_hook_solution(ground_normal_body, E, hook_length, sin_fi_guess, -cos_fi_guess, sin_fis, cos_fis, fis, points);
+ }
+ }
+}
+
+void FGJSBsim::update_external_forces(double t_off)
+{
+ const FGMatrix33& Tb2l = Propagate->GetTb2l();
+ const FGMatrix33& Tl2b = Propagate->GetTl2b();
+ const FGLocation& Location = Propagate->GetLocation();
+ const FGMatrix33& Tec2l = Location.GetTec2l();
+
+ double hook_area[4][3];
+
+ FGColumnVector3 hook_root_body = MassBalance->StructuralToBody(hook_root_struct);
+ FGColumnVector3 hook_root = Location.LocalToLocation(Tb2l * hook_root_body);
+ hook_area[1][0] = hook_root(1);
+ hook_area[1][1] = hook_root(2);
+ hook_area[1][2] = hook_root(3);
+
+ hook_length = fgGetDouble("/fdm/jsbsim/systems/hook/tailhook-length-ft", 6.75);
+ double fi_min = fgGetDouble("/fdm/jsbsim/systems/hook/tailhook-pos-min-deg", -18);
+ double fi_max = fgGetDouble("/fdm/jsbsim/systems/hook/tailhook-pos-max-deg", 30);
+ double fi = fgGetDouble("/fdm/jsbsim/systems/hook/tailhook-pos-norm") * (fi_max - fi_min) + fi_min;
+ double cos_fi = cos(fi * SG_DEGREES_TO_RADIANS);
+ double sin_fi = sin(fi * SG_DEGREES_TO_RADIANS);
+
+ FGColumnVector3 hook_tip_body = hook_root_body;
+ hook_tip_body(1) -= hook_length * cos_fi;
+ hook_tip_body(3) += hook_length * sin_fi;
+
+ double contact[3];
+ double ground_normal[3];
+ double ground_vel[3];
+ double ground_angular_vel[3];
+ double root_agl_ft;
+
+ if (!got_wire) {
+ bool got = get_agl_ft(t_off, hook_area[1], 0, contact, ground_normal,
+ ground_vel, ground_angular_vel, &root_agl_ft);
+ if (got && root_agl_ft > 0 && root_agl_ft < hook_length) {
+ FGColumnVector3 ground_normal_body = Tl2b * (Tec2l * FGColumnVector3(ground_normal[0], ground_normal[1], ground_normal[2]));
+ FGColumnVector3 contact_body = Tl2b * Location.LocationToLocal(FGColumnVector3(contact[0], contact[1], contact[2]));
+ double D = -DotProduct(contact_body, ground_normal_body);
+
+ // check hook tip agl against same ground plane
+ double hook_tip_agl_ft = DotProduct(hook_tip_body, ground_normal_body) + D;
+ if (hook_tip_agl_ft < 0) {
+
+ // hook tip: hx - l cos, hy, hz + l sin
+ // on ground: - n0 l cos + n2 l sin + E = 0
+
+ double E = D + DotProduct(hook_root_body, ground_normal_body);
+
+ // substitue x = sin fi, cos fi = sqrt(1 - x * x)
+ // and rearrange to get a quadratic with coeffs:
+ double a = sqr(hook_length) * (sqr(ground_normal_body(1)) + sqr(ground_normal_body(3)));
+ double b = 2 * E * ground_normal_body(3) * hook_length;
+ double c = sqr(E) - sqr(ground_normal_body(1) * hook_length);
+
+ double disc = sqr(b) - 4 * a * c;
+ if (disc >= 0) {
+ double delta = sqrt(disc) / (2 * a);
+
+ // allow 4 solutions for safety, should never happen
+ double sin_fis[4];
+ double cos_fis[4];
+ double fis[4];
+ int points = 0;
+
+ double sin_fi_guess = -b / (2 * a) - delta;
+ check_hook_solution(ground_normal_body, E, hook_length, sin_fi_guess, sin_fis, cos_fis, fis, &points);
+ check_hook_solution(ground_normal_body, E, hook_length, sin_fi_guess + 2 * delta, sin_fis, cos_fis, fis, &points);
+
+ if (points == 2) {
+ double diff1 = angle_diff(fi, fis[0]);
+ double diff2 = angle_diff(fi, fis[1]);
+ int point = diff1 < diff2 ? 0 : 1;
+ fi = fis[point];
+ sin_fi = sin_fis[point];
+ cos_fi = cos_fis[point];
+ hook_tip_body(1) = hook_root_body(1) - hook_length * cos_fi;
+ hook_tip_body(3) = hook_root_body(3) + hook_length * sin_fi;
+ }
+ }
+ }
+ }
+ } else {
+ FGColumnVector3 hook_root_vel = Propagate->GetVel() + (Tb2l * (Propagate->GetPQR() * hook_root_body));
+ double wire_ends_ec[2][3];
+ double wire_vel_ec[2][3];
+ get_wire_ends_ft(t_off, wire_ends_ec, wire_vel_ec);
+ FGColumnVector3 wire_vel_1 = Tec2l * FGColumnVector3(wire_vel_ec[0][0], wire_vel_ec[0][1], wire_vel_ec[0][2]);
+ FGColumnVector3 wire_vel_2 = Tec2l * FGColumnVector3(wire_vel_ec[1][0], wire_vel_ec[1][1], wire_vel_ec[1][2]);
+ FGColumnVector3 rel_vel = hook_root_vel - (wire_vel_1 + wire_vel_2) / 2;
+ if (rel_vel.Magnitude() < 3) {
+ got_wire = false;
+ release_wire();
+ fgSetDouble("/fdm/jsbsim/external_reactions/hook/magnitude", 0.0);