Initial commit

Lts/Services/AreaEffectSimulation/Trajectory/trajectory_helper.cpp

@@ -0,0 +1,220 @@
+/**
+  * @RDE_HEADER@
+  * Trajectory Helper
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Norbert Thoden, SLTE, 2023-11-08
+  */
+
+#include "trajectory_helper.h"
+#include "../logger.h"
+
+#include <LtsUtilsCore/qpoint3d.h>
+
+#include <QObject>
+#include <QString>
+#include <QDebug>
+
+#include <iostream>
+#include <cmath>
+
+
+namespace AreaEffectSimulation
+{
+
+double TrajectoryHelper::flightDurationSec(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double peakHeightAbs,
+                                           double *betaRadiantOut, double *vOut,
+                                           QString *reasonOut)
+{
+  if ((posStart.z() > peakHeightAbs) || (posEnd.z() > peakHeightAbs))
+  {
+    if (reasonOut)
+    {
+      //: "Höhenfehler"
+      *reasonOut = QObject::tr("HeightElevationError(%1 %2 %3)").arg(posStart.z()).arg(peakHeightAbs).arg(posEnd.z());
+    }
+    return -1.0;
+  }
+
+
+  double z = posEnd.z() - posStart.z();
+  double h = peakHeightAbs - posStart.z();
+
+
+  double x = posEnd.x() - posStart.x();
+  double y = posEnd.y() - posStart.y();
+  double r = sqrt(x * x + y * y);
+  if (r < 0.01)
+  {
+    if (reasonOut)
+    {
+      //: "StartEndeAbstandsfehler"
+      *reasonOut = QObject::tr("StartEndDistanceError(%1)").arg(r);
+    }
+    return 0.0; // No distance -> no duration -> 0.0
+  }
+
+  double beta; // Abschusswinkel / elevation_angle in radiant
+  double v;
+
+  if (fabs(z) < 0.01)
+  {
+    beta = atan(4.0 * h / r);
+    v = sqrt(9.81 * r / sin(2.0 * beta));
+  }
+  else
+  {
+    double b;
+    double p = - 4.0 * h / r;
+    double q = 4.0 * h * z / (r * r);     // ehemals double q = - 4.0 * h * z / (r * r);
+    if (p * p / 4.0 > q)
+      b = - (p / 2.0) + sqrt(p * p / 4.0 - q);
+    else
+      return -1.0;
+    beta = atan(b);
+    double a = b * b / (4.0 * h);
+    double rh = b / a;
+    v = sqrt(9.81 * rh / sin(2.0 * beta));
+  }
+
+  // handle optional values:
+  if (betaRadiantOut)
+    *betaRadiantOut = beta;
+  if (vOut)
+    *vOut = v;
+
+  const double result = r / (v * cos(beta));
+  if ((result < 0) && reasonOut)
+  {
+    //: "FlugzeitFehlerUnbekannt"
+    *reasonOut = QObject::tr("FlightDurationSecErrorUnknown(%1)").arg(result);
+  }
+
+  return result;
+}
+
+double TrajectoryHelper::peakHeightAbs(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double timeSec,
+                                       double *betaRadiantOut, double *vOut,
+                                       QString *reasonOut)
+{
+  double x = posEnd.x() - posStart.x();
+  double y = posEnd.y() - posStart.y();
+  double r = sqrt(x * x + y * y);
+  if (r < 0.01)
+  {
+    if (reasonOut)
+    {
+      //: "StartEndeAbstandsfehler"
+      *reasonOut = QObject::tr("StartEndDistanceError(%1)").arg(r);
+    }
+    return 0.0;
+  }
+
+  double v, rh;
+  double t2 = timeSec * timeSec;
+  double z = posEnd.z() - posStart.z();
+  if (fabs(z) < 0.01)
+  {
+    v = sqrt((9.81 * 9.81 * t2 / 4.0) + (r * r / t2));
+    rh = r;
+  }
+  else
+  {
+    double vx = r / timeSec;
+    double vy = (9.81 * timeSec * timeSec / 2.0 + z) / timeSec;
+    v = sqrt(vx * vx + vy * vy);
+    rh = 2.0 * r * sqrt((v * v - r * r / t2)) / (9.81 * timeSec);
+  }
+
+  double arcSin = 9.81 * rh / (v * v);
+  if (arcSin > 1.0)
+  {
+    if (reasonOut)
+    {
+      //: "45GradSchuss"
+      *reasonOut = QObject::tr("45DegreeShot(%1)").arg(arcSin);
+    }
+    return -1;
+  }
+
+  // beta: Abschusswinkel / elevationsWinkel in radiant
+  double beta;
+
+  if (fabs(9.81 * rh - v * v) < 0.0001)
+  {
+    beta = M_PI_4;
+  }
+  else
+  {
+    if (9.81 * rh > (v * v))
+    {
+      if (reasonOut)
+      {
+        //: "GipfelhöhenFehlerUnbekannt"
+        *reasonOut = QObject::tr("PeakHeightUnknownError(rh=%1,v=%2)").arg(rh).arg(v);
+      }
+      return -1;
+    }
+    beta = asin( 9.81 * rh / (v * v)) / 2.0;
+  }
+
+  // [GUEZE-2472] Inkonsitente Werte bei der Berechnung der Gipfelhöhe (peakHeight)
+  // Es gibt nur eine Flugzeit zu einer maximalen Höhe h bei Vorgabe der Zielentfernung.
+  //  if (beta < M_PI_2)
+  //  {
+  //    if (trajMode == TrajectoryMode::TrajectoryModeHigh)
+  //      beta = M_PI_2 - beta;
+  //  }
+  //  else
+  //  {
+  //    if (trajMode == TrajectoryMode::TrajectoryModeLow)
+  //      beta = M_PI_2 - beta;
+  //  }
+
+  double beta2;
+  if (beta < M_PI_2)
+    beta2 = M_PI_2 - beta;
+  else
+    beta2 = M_PI_2 + beta;
+
+  if (fabs((timeSec * v) * cos(beta) - r) > fabs((timeSec * v) * cos(beta2) - r))
+    beta = beta2;
+
+
+  double h = v * v * sin(beta) * sin(beta) / (2.0 * 9.81);
+  h += posStart.z();
+
+  // handle optional values:
+  if (betaRadiantOut)
+    *betaRadiantOut = beta;
+  if (vOut)
+    *vOut = v;
+
+  return h;
+}
+
+LtsUtils::QPoint3D TrajectoryHelper::realStartPoint(const LtsUtils::QPoint3D &apogaeum, const LtsUtils::QPoint3D &endPos)
+{
+  LtsUtils::QPoint3D realStart = apogaeum - (endPos-apogaeum);
+  realStart.setZ(endPos.z());
+  return realStart;
+}
+
+//const QString TrajectoryGenerator::string4poly(const TrajectoryPoint3D &trajectory)
+//{
+//  if (trajectory.isEmpty())
+//    return (QStringLiteral("Empty Polygon!"));
+
+//  const Point3D &p3D = trajectory.first();
+//  int zone = LtsCoordTrans::UtmCoordinate::fromGeographic(p3D.toPointF()).zone;
+//  QStringList pointSl;
+//  pointSl.reserve(trajectory.size());
+//  for (int i = 0; i < trajectory.size(); ++i)
+//    pointSl << QString(QStringLiteral("utm=%1 h=%2")).arg(LtsCoordTrans::UtmCoordinate::fromGeographic(p3D.toPointF(), zone).mgrs(), QString::number(p3D.z(), 'f', 1));
+//  return QStringLiteral("%1 Values (%2)").arg(trajectory.count()).arg(pointSl.join(QStringLiteral(" # ")));
+//}
+
+
+
+} // namespace AreaEffectSimulation