+ geo_inverse_wgs_84(oldhitchpos, hitchpos, &az1, &az2, &dist);
+
+ _azimuth = az1;
+
+ // and finally store the new values
+ _oldcarthitchPos = carthitchPos;
+ oldhitchpos = hitchpos;
+}
+
+double FGAIBallistic::getElevLoadToHitch() const {
+ // now the angle, positive angles are upwards
+ double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER;
+ double angle = 0;
+ double daltM = hitchpos.getElevationM() - pos.getElevationM();
+
+ if (fabs(distance) < SGLimits<float>::min()) {
+ angle = 0;
+ } else {
+ double sAngle = daltM/distance;
+ sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
+ angle = SGMiscd::rad2deg(asin(sAngle));
+ }
+
+ return angle;
+}
+
+double FGAIBallistic::getBearingLoadToHitch() const {
+ //calculate the bearing and range of the second pos from the first
+ double az1, az2, distance;
+
+ geo_inverse_wgs_84(pos, hitchpos, &az1, &az2, &distance);
+
+ return az1;
+}
+
+double FGAIBallistic::getRelBrgHitchToUser() const {
+ //calculate the relative bearing
+ double az1, az2, distance;
+
+ geo_inverse_wgs_84(hitchpos, userpos, &az1, &az2, &distance);
+
+ double rel_brg = az1 - hdg;
+
+ if (rel_brg > 180)
+ rel_brg -= 360;
+
+ return rel_brg;
+}
+
+double FGAIBallistic::getElevHitchToUser() const {
+
+ //calculate the distance from the user position
+ SGVec3d carthitchPos = getCartHitchPos();
+ SGVec3d cartuserPos = getCartUserPos();
+
+ SGVec3d diff = cartuserPos - carthitchPos;
+
+ double distance = norm(diff);
+ double angle = 0;
+
+ double daltM = userpos.getElevationM() - hitchpos.getElevationM();
+
+ // now the angle, positive angles are upwards
+ if (fabs(distance) < SGLimits<float>::min()) {
+ angle = 0;
+ } else {
+ double sAngle = daltM/distance;
+ sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
+ angle = SGMiscd::rad2deg(asin(sAngle));
+ }
+
+ return angle;
+}
+
+void FGAIBallistic::setTgtOffsets(double dt, double coeff){
+ double c = dt / (coeff + dt);
+
+ _x_offset = (_tgt_x_offset * c) + (_x_offset * (1 - c));
+ _y_offset = (_tgt_y_offset * c) + (_y_offset * (1 - c));
+ _z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
+}
+
+void FGAIBallistic::formateToAC(double dt){
+
+ setTgtOffsets(dt, 25);
+ setHitchPos();
+ setHitchVelocity(dt);
+
+ // elapsed time has a random initialisation so that each
+ // wingman moves differently
+ _elapsed_time += dt;
+
+ // we derive a sine based factor to give us smoothly
+ // varying error between -1 and 1
+ double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
+ double r_angle = 5 * factor;
+ double p_angle = 2.5 * factor;
+ double h_angle = 5 * factor;
+ double h_feet = 3 * factor;
+
+ pos.setLatitudeDeg(hitchpos.getLatitudeDeg());
+ pos.setLongitudeDeg(hitchpos.getLongitudeDeg());
+
+ if (getHtAGL()){
+
+ if(_ht_agl_ft <= 10) {
+ _height = userpos.getElevationFt();
+ } else if (_ht_agl_ft > 10 && _ht_agl_ft <= 150 ) {
+ setHt(userpos.getElevationFt(), dt, 1.0);
+ } else if (_ht_agl_ft > 150 && _ht_agl_ft <= 250) {
+ setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.75);
+ } else
+ setHt(hitchpos.getElevationFt()+ h_feet, dt, 0.5);
+
+ pos.setElevationFt(_height);
+ }
+
+ // these calculations are unreliable at slow speeds
+ if(speed >= 10) {
+ setHdg(_azimuth + h_angle, dt, 0.9);
+ setPch(_elevation + p_angle + _pitch_offset, dt, 0.9);
+
+ if (roll <= 115 && roll >= -115)
+ setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5);
+ else
+ roll = manager->get_user_roll() + r_angle + _roll_offset;
+
+ } else {
+ setHdg(manager->get_user_heading(), dt, 0.9);
+ setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9);
+ setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9);
+ }
+
+ setSpeed(speed);
+}
+// end AIBallistic