src/marnav/geo/cpa.cpp

   1 #include <marnav/geo/cpa.hpp>
   2 #include <marnav/math/vector.hpp>
   3 #include <cmath>
   4
   5 namespace marnav::geo
   6 {
   7
   8 /// @brief Computes the CPA (closest point of approach)
   9 /// and TCPA (time to closest point of approach).
  10 ///
  11 /// @param[in] vessel1 Telemetry about vessel 1.
  12 /// @param[in] vessel2 Telemetry about vessel 2.
  13 /// @return Position of the two vessels at the closest approach
  14 ///   and the time until the closest approach is reached. Also included
  15 ///   in the result, does a CPA exist or not. If both vessels
  16 ///   go in the same direction and are parallel to eachother,
  17 ///   no CPA exists at all.
  18 ///   - Position of vessel 1 at closest point of approach
  19 ///   - Position of vessel 2 at closest point of approach
  20 ///   - TCPA
  21 ///   - CPA exists (true) or not (false)
  22 ///
  23 /// @note This function calculates the CPA/TCPA on a plane (not sphere, not geoid).
  24 ///       This is precise enough for vessels in intresting vincinity. The farther away
  25 ///       the vessels are, the less precise the calculation gets.
  26 ///
  27 /// Formulae:
  28 ///
  29 /// Position of vessels and in function of time: @f{eqnarray*}{
  30 /// V_1(t) &=& V_1(0) + t \cdot \vec{u} \\
  31 /// V_2(t) &=& V_2(0) + t \cdot \vec{v}
  32 /// @f}
  33 /// with polar coordinates @f{eqnarray*}{
  34 /// \vec{u} = (sog_1, \phi cog_1) \\
  35 /// \vec{v} = (sog_2, \phi cog_2)
  36 /// @f}
  37 ///
  38 /// - @f${sog}@f$ : speed over ground
  39 /// - @f${cog}@f$ : course over ground in degrees
  40 ///
  41 /// The distance between the two vessels as function of time: @f[
  42 /// \vec{d}_t = V_1(t) - V_2(t)
  43 /// @f]
  44 /// with @f{eqnarray*}{
  45 /// \vec{d}_0 &=& V_1(0) - V_2(0) \\
  46 /// \vec{d}_t &=& V_1(t) - V_2(t) = V_1(0) + t \cdot \vec{u} - V_2(0) + t \cdot \vec{v} \\
  47 ///           &=& V_1(0) - V_2(0) + t \cdot \left( \vec{u} - \vec{v} \right) \\
  48 ///           &=& \vec{d}_0 + t \cdot \left( \vec{u} - \vec{v} \right)
  49 /// @f}
  50 /// Closest point of approach is reached if the distance @f$\vec{d}_t@f$ is minimal:
  51 /// @f{eqnarray*}{
  52 /// || \vec{d}_t ||^2 &=& \vec{d}_t \cdot \vec{d}_t \\
  53 ///                   &=& \left( \vec{d}_0 + t \cdot \left( \vec{u} - \vec{v} \right) \right)^2 \\
  54 ///                   &=& {\vec{d}_0}^2 + 2\cdot\vec{d}_0\cdot t\cdot\left(\vec{u}-\vec{v}\right)
  55 ///                       + t^2\cdot\left(\vec{u}-\vec{v}\right)^2
  56 /// @f}
  57 /// The minimum is reached, when the first derivation becomes zero. The derivation is:
  58 /// @f{eqnarray*}{
  59 /// \frac{\partial}{\partial t}|| \vec{d}_t ||^2 &=&
  60 /// 2\cdot\vec{d}_0\cdot\left(\vec{u}-\vec{v}\right)
  61 ///     + 2\cdot t\cdot\left(\vec{u}-\vec{v}\right)^2
  62 /// @f}
  63 /// Solving for @f$t@f$:
  64 /// @f{eqnarray*}{
  65 /// 0 &=& 2\cdot\vec{d}_0\cdot\left(\vec{u}-\vec{v}\right)
  66 ///       + 2\cdot t\cdot\left(\vec{u}-\vec{v}\right)^2 \\
  67 /// -2\cdot\vec{d}_0\cdot\left(\vec{u}-\vec{v}\right) &=& 2\cdot t\cdot\left(\vec{u}-\vec{v}\right)^2 \\
  68 /// \frac{-2\cdot\vec{d}_0\cdot\left(\vec{u}-\vec{v}\right)}{2\cdot\left(\vec{u}-\vec{v}\right)^2} &=& t \\
  69 /// t_{CPA} &=& \frac{-\vec{d}_0\cdot\left(\vec{u}-\vec{v}\right)}{\left(\vec{u}-\vec{v}\right)^2}
  70 /// @f}
  71 /// If the denominator @f$\left(\vec{u}-\vec{v}\right)^2@f$ is zero, there will never be a CPA.
  72 /// If @f$t_{CPA}@f$ is less than zero, it means CPA was reached in the past.
  73 ///
  74 /// The location of the vessels at closest approach:
  75 /// @f{eqnarray*}{
  76 /// V_1(t_{CPA}) &=& V_1(0) + t_{CPA} \cdot \vec{u} \\
  77 /// V_2(t_{CPA}) &=& V_2(0) + t_{CPA} \cdot \vec{v}
  78 /// @f}
  79 ///
  80 /// Example:
  81 /// @code
  82 /// const vessel vessel1 = {{0.0, 1.0}, 1.0, 90.0}; // being west, going east with 1kn
  83 /// const vessel vessel2 = {{-1.0, 0.0}, 1.0, 0.0}; // being south, going north with 1k
  84 ///
  85 /// position p1;
  86 /// position p2;
  87 /// std::chrono::seconds t_cpa;
  88 /// bool cpa_exists;
  89 /// std::tie(p1, p2, t_cpa, cpa_exists) = cpa(vessel1, vessel2);
  90 ///
  91 /// if (cpa_exists) {
  92 ///     const double d = abs(distance_sphere(p1, p2));
  93 ///     if ((d < 3.0 * 1852.0) || (t_cpa < std::chrono::minutes{10})) {
  94 ///         // .. warning!?
  95 ///     }
  96 /// }
  97 /// @endcode
  98 ///
  99 std::tuple<position, position, std::chrono::seconds, bool> cpa(
 100         const vessel & vessel1, const vessel & vessel2)
 101 {
 102         using math::vec2;
 103
 104         const vec2 v1_0{-vessel1.pos.lon(), vessel1.pos.lat()};
 105         const vec2 v2_0{-vessel2.pos.lon(), vessel2.pos.lat()};
 106
 107         const vec2 u = vec2::make_from_polar(vessel1.sog, 90.0 - vessel1.cog);
 108         const vec2 v = vec2::make_from_polar(vessel2.sog, 90.0 - vessel2.cog);
 109
 110         const vec2 d0 = v1_0 - v2_0;
 111         const vec2 t = u - v;
 112         const double den = t * t;
 113         if (std::abs(den) < 1e-7)
 114                 return std::make_tuple<position, position, std::chrono::seconds>(
 115                         position{0.0, 0.0}, position{0.0, 0.0}, std::chrono::seconds{0}, false);
 116
 117         const double t_cpa = (-1.0 * (d0 * t)) / den;
 118
 119         const vec2 v1_t = v1_0 + t_cpa * u;
 120         const vec2 v2_t = v2_0 + t_cpa * v;
 121
 122         // we need to convert the time, because the units were nautical miles and knots.
 123         // there are 60nm per degree and 1kn is 1nm per hour or 3600 seconds.
 124         const std::chrono::duration<double> t_cpa_seconds{t_cpa * 60 * 3600};
 125
 126         return std::make_tuple<position, position, std::chrono::seconds>(
 127                 position{v1_t[1], -v1_t[0]}, position{v2_t[1], -v2_t[0]},
 128                 std::chrono::duration_cast<std::chrono::seconds>(t_cpa_seconds), true);
 129 }
 130 }