glide_computer example added

example/glide_computer.cpp

@@ -0,0 +1,561 @@
+// The MIT License (MIT)
+//
+// Copyright (c) 2018 Mateusz Pusz
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <units/physical/si/speed.h>
+#include <units/physical/international/length.h>
+#include <units/math.h>
+#include <units/format.h>
+#include <units/quantity_point.h>
+#include <array>
+#include <iostream>
+
+// An example of a really simplified tactical glide computer
+// Simplifications:
+// - glider 100% clean and with full factory performance (brand new painting)
+// - no influence of the ballast (pilot weight, water, etc) to glider performance
+// - only one point on a glider polar curve
+// - no influence of bank angle (during circling) on a glider performance
+// - no wind
+// - constant thermals strength
+// - thermals exactly where and when we need them ;-)
+// - no airspaces
+// - Earth is flat
+// - ground level changes linearly between airports
+// - no ground obstacles (i.e. mountains) to pass
+// - flight path exactly on a shortest possible line to destination
+
+// horizontal/vertical vector
+namespace {
+
+using namespace units;
+
+enum class direction {
+  horizontal,
+  vertical
+};
+
+template<typename Q, direction D>
+  requires Quantity<Q> || QuantityPoint<Q>
+class vector {
+public:
+  using magnitude_type = Q;
+  static constexpr direction dir = D;
+
+  vector() = default;
+  explicit constexpr vector(Q m): magnitude_(std::move(m)) {}
+
+  // template<Quantity QQ>
+  //   requires QuantityPoint<Q>
+  // explicit constexpr vector(QQ q)
+  //   : magnitude_(std::move(q)) {}
+
+  constexpr const Q& magnitude() const & { return magnitude_; }
+  constexpr Q magnitude() const && { return std::move(magnitude_); }
+
+  template<typename QQ = Q>
+  [[nodiscard]] constexpr vector operator-() const
+    requires requires(QQ q) { -q; }
+    // requires requires { -magnitude(); } // TODO gated by gcc-9 (fixed in gcc-10)
+  {
+    return vector(-magnitude());
+  }
+
+#if __GNUC__ >= 10
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator<=>(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() <=> rhs.magnitude(); }
+  {
+    return lhs.magnitude() <=> rhs.magnitude();
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr bool operator==(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() == rhs.magnitude(); }
+  {
+    return lhs.magnitude() == rhs.magnitude();
+  }
+
+#else
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator==(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() == rhs.magnitude(); }
+  {
+    return lhs.magnitude() == rhs.magnitude();
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator!=(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() != rhs.magnitude(); }
+  {
+    return !(lhs == rhs);
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator<(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() < rhs.magnitude(); }
+  {
+    return lhs.magnitude() < rhs.magnitude();
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator>(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() > rhs.magnitude(); }
+  {
+    return rhs < lhs;
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator<=(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() <= rhs.magnitude(); }
+  {
+    return !(rhs < lhs);
+  }
+
+  template<typename Q2>
+  [[nodiscard]] friend constexpr auto operator>=(const vector& lhs, const vector<Q2, D>& rhs)
+    requires requires { lhs.magnitude() >= rhs.magnitude(); }
+  {
+    return !(lhs < rhs);
+  }
+
+#endif
+
+  // template<class CharT, class Traits>
+    // requires Quantity<Q> // TODO gated by gcc-9 (fixed in gcc-10)
+  template<class CharT, class Traits, typename QQ = Q>
+    requires Quantity<QQ>
+  friend std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os, const vector& v)
+  {
+    return os << v.magnitude();
+  }
+
+private:
+  Q magnitude_{};
+};
+
+template<typename Q1, typename Q2, direction D>
+[[nodiscard]] constexpr auto operator+(const vector<Q1, D>& lhs, const vector<Q2, D>& rhs)
+  requires requires { lhs.magnitude() + rhs.magnitude(); }
+{
+  using ret_type = decltype(lhs.magnitude() + rhs.magnitude());
+  return vector<ret_type, D>(lhs.magnitude() + rhs.magnitude());
+}
+
+template<typename Q1, typename Q2, direction D>
+[[nodiscard]] constexpr auto operator-(const vector<Q1, D>& lhs, const vector<Q2, D>& rhs)
+  requires requires { lhs.magnitude() - rhs.magnitude(); }
+{
+  using ret_type = decltype(lhs.magnitude() - rhs.magnitude());
+  return vector<ret_type, D>(lhs.magnitude() - rhs.magnitude());
+}
+
+// template<typename Q1, typename Q2, direction D>
+// constexpr AUTO operator*(const vector<Q1, D>& lhs, const vector<Q2, D>& rhs)
+//   requires requires { lhs.magnitude() * rhs.magnitude(); }
+// {
+//   using ret_type = decltype(lhs.magnitude() * rhs.magnitude());
+//   return vector<ret_type, D>(lhs.magnitude() * rhs.magnitude());
+// }
+
+template<typename Q1, typename Q2, direction D1, direction D2>
+[[nodiscard]] constexpr double operator/(const vector<Q1, D1>& lhs, const vector<Q2, D2>& rhs)
+  requires equivalent_dim<typename Q1::dimension, typename Q2::dimension> &&
+           requires { lhs.magnitude() / rhs.magnitude(); }
+{
+  return lhs.magnitude() / rhs.magnitude();
+}
+
+template<typename T>
+inline constexpr bool is_vector = false;
+template<typename Q, direction D>
+inline constexpr bool is_vector<vector<Q, D>> = true;
+
+} // namespace
+
+template<Quantity Q, direction D>
+struct fmt::formatter<vector<Q, D>> : formatter<Q> {
+  template <typename FormatContext>
+  auto format(const vector<Q, D>& v, FormatContext& ctx)
+  {
+    return formatter<Q>::format(v.magnitude(), ctx);
+  }
+};
+
+// custom types
+namespace {
+
+using namespace units::physical;
+
+// concepts do not scale :-(
+// template<typename T>
+// concept QPLength = QuantityPoint<T> && Length<typename T::quantity_type>;
+
+using distance = vector<si::length<si::kilometre>, direction::horizontal>;
+using height = vector<si::length<si::metre>, direction::vertical>;
+using altitude = vector<quantity_point<si::dim_length, si::metre>, direction::vertical>;
+
+using velocity = vector<si::speed<si::kilometre_per_hour>, direction::horizontal>;
+using rate_of_climb = vector<si::speed<si::metre_per_second>, direction::vertical>;
+
+using time_point = quantity_point<si::dim_time, si::second>;
+using duration = si::time<si::second>;
+
+template<class CharT, class Traits>
+std::basic_ostream<CharT, Traits>& operator<<(std::basic_ostream<CharT, Traits>& os, const altitude& a)
+{
+  return os << a.magnitude().relative() << " AMSL";
+}
+
+} // namespace
+
+template<>
+struct fmt::formatter<altitude> : formatter<si::length<si::metre>> {
+  template <typename FormatContext>
+  auto format(altitude a, FormatContext& ctx)
+  {
+    formatter<si::length<si::metre>>::format(a.magnitude().relative(), ctx);
+    return format_to(ctx.out(), " AMSL");
+  }
+};
+
+// gliders database
+namespace {
+
+using namespace units::physical::si::literals;
+using namespace units::physical::international::literals;
+
+struct glider {
+  struct polar_point {
+    velocity v;
+    rate_of_climb climb;
+  };
+
+  std::string name;
+  std::array<polar_point, 1> polar;
+};
+
+auto get_gliders()
+{
+  const std::array gliders = {
+    glider{"SZD-30 Pirat", {velocity(83q_km_per_h), rate_of_climb(-0.7389q_m_per_s)}},
+    glider{"SZD-51 Junior", {velocity(80q_km_per_h), rate_of_climb(-0.6349q_m_per_s)}},
+    glider{"SZD-48 Jantar Std 3", {velocity(110q_km_per_h), rate_of_climb(-0.77355q_m_per_s)}},
+    glider{"SZD-56 Diana", {velocity(110q_km_per_h), rate_of_climb(-0.63657q_m_per_s)}}
+  };
+  return gliders;
+}
+
+constexpr double glide_ratio(const glider::polar_point& polar)
+{
+  return polar.v / -polar.climb;
+}
+
+template<std::ranges::forward_range R>
+  requires std::same_as<std::ranges::range_value_t<R>, glider>
+void print(const R& gliders)
+{
+  std::cout << "Gliders:\n";
+  std::cout << "========\n";  
+  for(const auto& g : gliders) {
+    std::cout << "- Name: " << g.name << "\n";
+    std::cout << "- Polar:\n";
+    for(const auto& p : g.polar)
+      fmt::print("  * {:%.4Q %q} @ {:%.1Q %q} -> {:.1f}\n", p.climb, p.v, glide_ratio(g.polar[0]));
+    std::cout << "\n";
+  }
+}
+
+} // namespace
+
+// weather conditions
+namespace {
+
+struct weather {
+  height cloud_base;
+  rate_of_climb thermal_strength;
+};
+
+auto get_weather_conditions()
+{
+  const std::array weather_conditions = {
+    std::pair("Good", weather{height(1900q_m), rate_of_climb(4.3q_m_per_s)}),
+    std::pair("Medium", weather{height(1550q_m), rate_of_climb(2.8q_m_per_s)}),
+    std::pair("Bad", weather{height(850q_m), rate_of_climb(1.8q_m_per_s)})
+  };
+  return weather_conditions;
+}
+
+template<std::ranges::forward_range R>
+  requires std::same_as<std::ranges::range_value_t<R>, std::pair<const char*, weather>>
+void print(const R& conditions)
+{
+  std::cout << "Weather:\n";
+  std::cout << "========\n";  
+  for(const auto& c : conditions) {
+    std::cout << "- Kind:              " << c.first << "\n";
+    const auto& w = c.second;
+    std::cout << "- Cloud base:        " << fmt::format("{:%.0Q %q}", w.cloud_base) << " AGL\n";
+    std::cout << "- Thermals strength: " << fmt::format("{:%.1Q %q}", w.thermal_strength) << "\n";
+    std::cout << "\n";
+  }
+}
+
+} // namespace
+
+// task
+namespace {
+
+struct waypoint {
+  std::string name;
+  altitude alt;
+};
+
+struct task {
+  waypoint start;
+  waypoint finish;
+  distance dist;
+};
+
+void print(const task& t)
+{
+  std::cout << "Task:\n";
+  std::cout << "=====\n";
+  std::cout << "- Start:    " << t.start.name << " (" << fmt::format("{:%.1Q %q}", t.start.alt) << ")\n";
+  std::cout << "- Finish:   " << t.finish.name << " (" << fmt::format("{:%.1Q %q}", t.finish.alt) << ")\n";
+  std::cout << "- Distance: " << fmt::format("{:%.1Q %q}", t.dist) << "\n";
+  std::cout << "\n";
+}
+
+} // namespace
+
+// safety params
+namespace {
+
+struct safety {
+  height min_agl_height;
+};
+
+void print(const safety& s)
+{
+  std::cout << "Safety:\n";
+  std::cout << "=======\n";
+  std::cout << "- Min AGL separation: " << fmt::format("{:%.0Q %q}", s.min_agl_height) << "\n";
+  std::cout << "\n";
+}
+
+} // namespace
+
+// tow parameters
+namespace {
+
+struct aircraft_tow {
+  height height_agl;
+  rate_of_climb performance;
+};
+
+void print(const aircraft_tow& tow)
+{
+  std::cout << "Tow:\n";
+  std::cout << "====\n";
+  std::cout << " - Type:        aircraft\n" ;
+  std::cout << " - Height:      " << fmt::format("{:%.0Q %q}", tow.height_agl) <<"\n";
+  std::cout << " - Performance: " << fmt::format("{:%.1Q %q}", tow.performance) <<"\n";
+  std::cout << "\n";
+}
+
+} // namespace
+
+// tactical flight computer basics
+namespace {
+
+struct position {
+  time_point timestamp;
+  distance dist;
+  altitude alt;
+};
+
+constexpr altitude terrain_level_alt(const task& t, distance pos)
+{
+  const height alt_diff = t.finish.alt - t.start.alt;
+  return t.start.alt + height(alt_diff.magnitude() * (pos / t.dist));
+}
+
+constexpr height agl(altitude glider_alt, altitude terrain_level)
+{
+  return glider_alt - terrain_level;
+}
+
+position takeoff(const task& t)
+{
+  const position new_pos{{}, {}, t.start.alt};
+  return new_pos;
+}
+
+void print(std::string_view phase_name, const position& pos, const position& new_pos)
+{
+  fmt::print("| {:<12} | {:>9%.1Q %q} (Total: {:>9%.1Q %q}) | {:>8%.1Q %q} (Total: {:>8%.1Q %q}) | {:>7%.0Q %q} ({:>6%.0Q %q}) |\n",
+    phase_name,
+    quantity_cast<si::minute>(new_pos.timestamp - pos.timestamp), quantity_cast<si::minute>(new_pos.timestamp.relative()),
+    new_pos.dist - pos.dist, new_pos.dist,
+    new_pos.alt - pos.alt, new_pos.alt);
+}
+
+position tow(const position& pos, const aircraft_tow& at)
+{
+  const duration d = at.height_agl.magnitude() / at.performance.magnitude();
+  const position new_pos{pos.timestamp + d, pos.dist, pos.alt + at.height_agl};
+
+  print("Tow", pos, new_pos);
+  return new_pos;
+}
+
+position circle(const position& pos, const glider& g, const weather& w, const task& t, height& height_to_gain)
+{
+  const height h_agl = agl(pos.alt, terrain_level_alt(t, pos.dist));
+  const height circle_height = std::min(w.cloud_base - h_agl, height_to_gain);
+  const rate_of_climb circling_rate = w.thermal_strength + g.polar[0].climb;
+  const duration d = circle_height.magnitude() / circling_rate.magnitude();
+  const position new_pos{pos.timestamp + d, pos.dist, pos.alt + circle_height};
+
+  height_to_gain = height_to_gain - circle_height; // TODO -=
+
+  print("Circle", pos, new_pos);
+  return new_pos;
+}
+
+// Returns `x` of the intersection of a glide line and a terrain line.
+// y = -x / glide_ratio + pos.alt;
+// y = (finish_alt - ground_alt) / dist_to_finish * x + ground_alt + min_agl_height;
+constexpr distance glide_distance(const position& pos, const glider& g, const task& t, const safety& s, altitude ground_alt)
+{
+  const auto dist_to_finish = t.dist - pos.dist;
+  return distance((ground_alt + s.min_agl_height - pos.alt).magnitude() / ((ground_alt - t.finish.alt) / dist_to_finish - 1 / glide_ratio(g.polar[0])));
+}
+
+position glide(const position& pos, const glider& g, const task& t, const safety& s)
+{
+  const auto ground_alt = terrain_level_alt(t, pos.dist);
+  const auto dist = glide_distance(pos, g, t, s, ground_alt);
+  const auto alt = ground_alt + s.min_agl_height;
+  const auto dist3d = sqrt(pow<2>(dist.magnitude()) + pow<2>((pos.alt - alt).magnitude()));
+  const duration d = dist3d / g.polar[0].v.magnitude();
+  const position new_pos{pos.timestamp + d, pos.dist + dist, terrain_level_alt(t, pos.dist + dist) + s.min_agl_height};
+
+  print("Glide", pos, new_pos);
+  return new_pos;
+}
+
+position final_glide(const position& pos, const glider& g, const task& t)
+{
+  const auto dist = t.dist - pos.dist;
+  const auto dist3d = sqrt(pow<2>(dist.magnitude()) + pow<2>((pos.alt - t.finish.alt).magnitude()));
+  const duration d = dist3d / g.polar[0].v.magnitude();
+  const position new_pos{pos.timestamp + d, pos.dist + dist, t.finish.alt};
+
+  print("Final Glide", pos, new_pos);
+  return new_pos;
+}
+
+void estimate(const glider& g, const weather& w, const task& t, const safety& s, const aircraft_tow& at)
+{
+  // ready to takeoff
+  position pos = takeoff(t);
+
+  // estimate aircraft towing
+  pos = tow(pos, at);
+
+  // estimate the altitude needed to reach the finish line from this place
+  const altitude final_glide_alt = t.finish.alt + height(t.dist.magnitude() / glide_ratio(g.polar[0]));
+
+  // how much height we still need to gain in the thermalls to reach the destination?
+  height height_to_gain = final_glide_alt - pos.alt;
+
+  do {
+    // glide to the next thermall
+    pos = glide(pos, g, t, s);
+
+    // circle in a thermall to gain height
+    pos = circle(pos, g, w, t, height_to_gain);
+  }
+  while(height_to_gain > height(0q_m));
+
+  // final glide
+  pos = final_glide(pos, g, t);
+}
+
+} // namespace
+
+// example
+namespace {
+
+void example()
+{
+  const auto gliders = get_gliders();
+  print(gliders);
+
+  const auto weather_conditions = get_weather_conditions();
+  print(weather_conditions);
+
+  const task t = {
+    waypoint{"EPPR", altitude(quantity_point(16q_ft))},
+    waypoint{"EPGI", altitude(quantity_point(115q_ft))},
+    distance(81.7q_km)
+  };
+  print(t);
+
+  const safety s = {height(300q_m)};
+  print(s);
+
+  const aircraft_tow tow = {height(400q_m), rate_of_climb(1.6q_m_per_s)};
+  print(tow);
+
+  for(const auto& g : gliders) {
+    for(const auto& c : weather_conditions) {
+      std::string txt = "Scenario: Glider = " + g.name + ", Weather = " + c.first;
+      std::cout << txt << "\n";
+      fmt::print("{0:=^{1}}\n\n", "", txt.size());
+      fmt::print("| {:<12} | {:^28} | {:^26} | {:^21} |\n", "Flight phase", "Duration", "Distance", "Height");
+      fmt::print("|{0:-^14}|{0:-^30}|{0:-^28}|{0:-^23}|\n", "");
+
+      estimate(g, c.second, t, s, tow);
+
+      std::cout << "\n\n";
+    }
+  }
+}
+
+} // namespace
+
+int main()
+{
+  try {
+    example();
+  }
+  catch (const std::exception& ex) {
+    std::cerr << "Unhandled std exception caught: " << ex.what() << '\n';
+  }
+  catch (...) {
+    std::cerr << "Unhandled unknown exception caught\n";
+  }
+}