153 lines
5.4 KiB
C++
153 lines
5.4 KiB
C++
// Determine time and position of closest approach for two vessels traveling
|
|
// along geodesics. Thanks to
|
|
// Jorge D. Taramona
|
|
// Inspector de Navegación Aérea
|
|
// Dirección General de Aviación Civil del Perú
|
|
// for suggesting this problem. See discussion entry from 2014-08-19:
|
|
// https://sourceforge.net/p/geographiclib/discussion/1026620/thread/33ce09e0/
|
|
|
|
// Compile with
|
|
// g++ -O3 -std=c++11 -o ClosestApproach ClosestApproach.cpp -lGeographic -L/usr/local/lib -Wl,-rpath=/usr/local/lib
|
|
|
|
// Example: Planes leave
|
|
|
|
// (1) Istanbul 42N 29E bearing 51W traveling at 900 km/hr
|
|
// (2) Reyjkavik 64N 22W bearing 154E traveling at 800 km/hr
|
|
|
|
// at the same time; compute time of closest approach. Guess that the time of
|
|
// closest approach is after 1 hr. So run
|
|
|
|
// echo 42 29 -51 900e3 64 -22 154 800e3 1 | ./ClosestApproach
|
|
|
|
// 0 1 2696504 -3.532249e+12 46.74926 19.83775 57.4055 -16.17089
|
|
// 1 2.751696 1083726 6.786464e+10 52.80517 -0.05892146 45.43817 -9.817806
|
|
// 2 2.71894 1082700 -2657490 52.72565 0.3576737 45.66487 -9.90999
|
|
// 3 2.718941 1082700 -0.005293798 52.72565 0.3576574 45.66486 -9.909987
|
|
// 4 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
// 5 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
// 6 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
// 7 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
// 8 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
// 9 2.718941 1082700 0 52.72565 0.3576574 45.66486 -9.909987
|
|
|
|
// At closest approach
|
|
// time = 2.72 hr
|
|
// dist = 1083 km
|
|
// position of first plane = 52.7N 0.3E
|
|
// position of second plane = 45.7N 9.9W
|
|
|
|
// CAVEAT: if the initial guess is bad, this program may return the time at
|
|
// which the distance is a maximum.
|
|
|
|
// Explanation:
|
|
|
|
// Consider f(t) = s12^2 / 2
|
|
// Find zeros of g(t) = f'(t) = s12' * s12 by Newton's method
|
|
// using g'(t) = (s12')^2 + s12'' * s12
|
|
|
|
// Using s12^2 / 2 as the governing function (as opposed, for example, to
|
|
// |s12|) means that in the planar limit f(t) is a quadratic function and g(t)
|
|
// is a linear function. So Newton's method just needs a single iteration.
|
|
|
|
// Let gam{1,2} be angle between s12 and v{1,2}.
|
|
|
|
// s12' = v2 * cos(gam2) - v1 * cos(gam1)
|
|
// g(t) = s12 * (v2 * cos(gam2) - v1 * cos(gam1))
|
|
// s12'' = - v2*sin(gam2) * dgam2/dt + v1*sin(gam1) * dgam1/dt
|
|
// dgam1/dt = (+ M12*v1*sin(gam1) - v2*sin(gam2)) / m12
|
|
// dgam2/dt = (- M21*v2*sin(gam2) + v1*sin(gam1)) / m12
|
|
// s12'' = (M12*v1^2*sin(gam1)^2 + M21*v2^2*sin(gam2)^2
|
|
// - 2*v1*v2*sin(gam1)*sin(gam2)) / m12
|
|
// g'(t) = v1^2 * (cos(gam1)^2 + M12*s12/m12 * sin(gam1)^2)
|
|
// + v2^2 * (cos(gam2)^2 + M21*s12/m12 * sin(gam2)^2)
|
|
// - 2*v1*v2 * (cos(gam1)*cos(gam2) + s12/m12 * sin(gam1)*sin(gam2))
|
|
|
|
// Planar limit (m12 = s12, M12 = M21 = 1):
|
|
// g(t) = (v2-v1) . (r2-r1)
|
|
// g'(t) = v1^2 + v2^2 - 2*v1*v2 * cos(gam1-gam2)
|
|
// = |v1-v2|^2 = const
|
|
|
|
// Special case when v2 = 0 (simple interception), v1 = 1
|
|
|
|
// s12' = - cos(gam1)
|
|
// g(t) = - s12 * cos(gam1)
|
|
// s12'' = sin(gam1) * dgam1/dt
|
|
// dgam1/dt = M12*sin(gam1) / m12
|
|
// s12'' = M12*sin(gam1)^2 / m12
|
|
// g'(t) = (cos(gam1)^2 + M12*s12/m12 * sin(gam1)^2)
|
|
|
|
// Planar limit (m12 = s12, M12 = M21 = 1):
|
|
// g(t) = -v1 . (r2-r1)
|
|
// g'(t) = 1 = const
|
|
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <cmath>
|
|
|
|
#include <GeographicLib/Geodesic.hpp>
|
|
#include <GeographicLib/GeodesicExact.hpp>
|
|
#include <GeographicLib/Utility.hpp>
|
|
|
|
using namespace std;
|
|
using namespace GeographicLib;
|
|
|
|
int main() {
|
|
try {
|
|
typedef Math::real real;
|
|
Utility::set_digits();
|
|
const Geodesic/*Exact*/ g = Geodesic/*Exact*/(Constants::WGS84_a(),
|
|
Constants::WGS84_f());
|
|
real lat1, lon1, azi1, v1;
|
|
real lat2, lon2, azi2, v2;
|
|
real t0;
|
|
cout << setprecision(7);
|
|
while (cin
|
|
>> lat1 >> lon1 >> azi1 >> v1
|
|
>> lat2 >> lon2 >> azi2 >> v2
|
|
>> t0) {
|
|
real t = t0;
|
|
for (int i = 0; i < 10; ++i) {
|
|
// Compute positions at time t
|
|
real lat1a, lon1a, azi1a;
|
|
real lat2a, lon2a, azi2a;
|
|
g.Direct(lat1, lon1, azi1, t*v1, lat1a, lon1a, azi1a);
|
|
g.Direct(lat2, lon2, azi2, t*v2, lat2a, lon2a, azi2a);
|
|
// Compute distance at time t
|
|
real s12, azi1c, azi2c, m12, M12, M21;
|
|
g.Inverse(lat1a, lon1a, lat2a, lon2a,
|
|
s12, azi1c, azi2c, m12, M12, M21);
|
|
real
|
|
r12 = s12 / m12,
|
|
gam1 = (azi1c - azi1a) * Math::degree(),
|
|
gam2 = (azi2c - azi2a) * Math::degree(),
|
|
x1 = v1 * cos(gam1), y1 = v1 * sin(gam1),
|
|
x2 = v2 * cos(gam2), y2 = v2 * sin(gam2);
|
|
// Find g = 0 using Newton's method where
|
|
// g = d( (1/2)*s12^2 ) / dt = s12 * d(s12)/dt
|
|
real g = s12 * (x2 - x1);
|
|
real dg = // dg = dg/dt
|
|
(Math::sq(x1) + M12 * r12 * Math::sq(y1)) +
|
|
(Math::sq(x2) + M21 * r12 * Math::sq(y2)) -
|
|
2 * (x1 * x2 + r12 * y1 * y2);
|
|
cout << i << " "
|
|
<< fixed << setprecision(12) << t << " " << s12 << " "
|
|
<< defaultfloat << setprecision(6) << g << " "
|
|
<< fixed << setprecision(20)
|
|
<< lat1a << " " << lon1a << " "
|
|
<< fixed << setprecision(6)
|
|
<< lat2a << " " << lon2a << "\n";
|
|
t -= g/dg; // Newton iteration
|
|
}
|
|
}
|
|
}
|
|
catch (const exception& e) {
|
|
cerr << "Caught exception: " << e.what() << "\n";
|
|
return 1;
|
|
}
|
|
catch (...) {
|
|
cerr << "Caught unknown exception\n";
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|