384 lines
14 KiB
C++
Executable File
384 lines
14 KiB
C++
Executable File
/**
|
|
* @RDE_HEADER@
|
|
* LTS - logging facility
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by Norbert Thoden, SLTE, 2023-11-08
|
|
*/
|
|
|
|
#include "trajectory_generator.h"
|
|
#include "trajectory_generator_vulcano.h"
|
|
#include <Plugins/ExerciseManagement/exm_data_plugin_interface.h>
|
|
#include <Plugins/AssetManagement/asm_data_plugin_interface.h>
|
|
|
|
|
|
#include <LtsCoordTrans/lts_coord_trans.h>
|
|
#include <LtsUtilsCore/qpoint3d.h>
|
|
|
|
#ifdef NEVER // for csv file
|
|
# include <QFile>
|
|
# include <QDateTime>
|
|
# include <QTextStream>
|
|
#endif // NEVER
|
|
|
|
#include <cmath>
|
|
|
|
|
|
namespace AreaEffectSimulation
|
|
{
|
|
|
|
static TrajectoryGenerator *s_trajectoryGenerator = nullptr;
|
|
|
|
|
|
TrajectoryGenerator::TrajectoryGenerator()
|
|
: m_logger(getLogger(QStringLiteral("TrajectoryGenerator")))
|
|
{
|
|
}
|
|
|
|
TrajectoryGenerator *TrajectoryGenerator::getInstance(int exerciseId,
|
|
const std::shared_ptr<ExerciseManagement::Interface::Dtc> &exmDtc,
|
|
const std::shared_ptr<AssetManagement::Interface::Dtc> &asmDtc)
|
|
{
|
|
// Be aware: ONE TrajectoryGenerator per process, not per exercise
|
|
// That feature has to be added!
|
|
// In that case, have a look at LtsElevationManager::createInstance() and createInstanceForTrainingAreaId()
|
|
|
|
if (!s_trajectoryGenerator)
|
|
{
|
|
LOGGER_T logger = getLogger(QStringLiteral("TrajectoryGenerator"));
|
|
|
|
s_trajectoryGenerator = new TrajectoryGenerator();
|
|
qAddPostRoutine(&TrajectoryGenerator::cleanup);
|
|
|
|
// assign LtsElevationManager:
|
|
LtsElevation::LTS_ELEVATION_MANAGER_TYPE ltsElevationManager = LtsElevation::LtsElevationManager::createInstance(exerciseId, exmDtc, asmDtc);
|
|
#ifndef LTS_DEBUG
|
|
Q_ASSERT_X(ltsElevationManager, "LtsElevationManager", "createInstance failed");
|
|
#endif // LTS_DEBUG
|
|
|
|
if (ltsElevationManager)
|
|
{
|
|
ltsElevationManager->waitForInitialization();
|
|
DEBUG_LOG(logger) << LOGFUNCTION << ", ltsElevationManager->waitForInitialization() ... DONE, isValid = " << ltsElevationManager->isValid();
|
|
}
|
|
s_trajectoryGenerator->initElevation(ltsElevationManager);
|
|
}
|
|
return s_trajectoryGenerator;
|
|
}
|
|
|
|
TrajectoryGenerator *TrajectoryGenerator::getInstance()
|
|
{
|
|
if (!s_trajectoryGenerator)
|
|
{
|
|
s_trajectoryGenerator = new TrajectoryGenerator();
|
|
qAddPostRoutine(&TrajectoryGenerator::cleanup);
|
|
}
|
|
return s_trajectoryGenerator;
|
|
}
|
|
|
|
void TrajectoryGenerator::cleanup()
|
|
{
|
|
delete s_trajectoryGenerator;
|
|
}
|
|
|
|
void TrajectoryGenerator::initElevation(LtsElevation::LtsElevationManager *ltsElevationManager)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
m_ltsElevationManager = ltsElevationManager;
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
LtsElevation::LtsElevationManager *TrajectoryGenerator::getElevationManager()
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
return m_ltsElevationManager;
|
|
}
|
|
|
|
bool TrajectoryGenerator::hasElevationManager() const
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
return m_ltsElevationManager;
|
|
}
|
|
|
|
|
|
|
|
// Ideal ballistic curve, but with time values:
|
|
bool TrajectoryGenerator::generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
|
|
const LtsUtils::QPoint3D &idealHit_geo,
|
|
double elevation_angle_degree,
|
|
TrajectoryGeoPosition &trajectoryOut)
|
|
{
|
|
return generateTrajectoryBiedziak<std::vector<Core::GeoPosition>>(originatorPosition_geo,
|
|
idealHit_geo,
|
|
elevation_angle_degree,
|
|
trajectoryOut);
|
|
}
|
|
|
|
//
|
|
// Vulcano
|
|
//
|
|
bool TrajectoryGenerator::generateTrajectoryVulcano(const QPoint3D &originatorPosition_geo, const QPoint3D &idealHit_geo,
|
|
double v0, double betaRad,
|
|
double alphaRad, double tapTimeSec,
|
|
TrajectoryGeneratorVulcano::Options options,
|
|
TrajectoryGeoPositionRelTime &trajectoryTimeAwareOut,
|
|
Core::GeoPosition &idealTapAbsoluteGeoOut,
|
|
QString *reasonOut)
|
|
{
|
|
LOGGER_T logger = getLogger(QStringLiteral("TrajectoryGenerator"));
|
|
|
|
// use zone of idealHit:
|
|
INFO_LOG(logger) << " orgPos geo " << originatorPosition_geo;
|
|
INFO_LOG(logger) << " iHPos geo " << idealHit_geo;
|
|
|
|
const LtsCoordTrans::UtmCoordinate &idealHitUtm = LtsCoordTrans::UtmCoordinate::fromGeographic(idealHit_geo.toPointF());
|
|
const LtsCoordTrans::UtmCoordinate &originUtm = LtsCoordTrans::UtmCoordinate::fromGeographic(originatorPosition_geo.toPointF(), idealHitUtm.zone);
|
|
INFO_LOG(logger) << " originUtm " << originUtm.mgrs();
|
|
INFO_LOG(logger) << " idealHitUtm " << idealHitUtm.mgrs();
|
|
|
|
|
|
TRAJECTORY_METRIC trajectoryMetricOut;
|
|
|
|
const QPoint3D origin_metric (originUtm.x, originUtm.y, originatorPosition_geo.z());
|
|
const QPoint3D idealHit_metric(idealHitUtm.x, idealHitUtm.y, idealHit_geo.z());
|
|
|
|
double vGlidingOut = 1;
|
|
double vFallingOut = 1;
|
|
TrajectoryGeneratorVulcano::getSpeedPerPhase(v0, betaRad, vGlidingOut, vFallingOut);
|
|
|
|
QString localReasonOut;
|
|
|
|
QPoint3D idealTapAbsoluteOut;
|
|
|
|
const bool success =
|
|
TrajectoryGeneratorVulcano::trajectoryVulcano(origin_metric, idealHit_metric,
|
|
v0, betaRad,
|
|
vGlidingOut,
|
|
vFallingOut, alphaRad, tapTimeSec,
|
|
options,
|
|
trajectoryMetricOut,
|
|
idealTapAbsoluteOut,
|
|
&localReasonOut);
|
|
|
|
if (!localReasonOut.isEmpty())
|
|
WARNING_LOG(logger) << " trajectoryVulcano failed, reasonOut = '" << localReasonOut << "'";
|
|
|
|
//
|
|
// for documentation, it may be helpful to write the trajectoryMetric to a csv file
|
|
//
|
|
|
|
#ifdef NEVER
|
|
{
|
|
static QString filePath(QStringLiteral("/tmp/trajectory_dump.csv"));
|
|
QFile data(filePath);
|
|
QTextStream output(&data);
|
|
|
|
if (data.open(QFile::WriteOnly | QIODevice::Append |QFile::Truncate))
|
|
{
|
|
output.setRealNumberPrecision(3);
|
|
output.setRealNumberNotation(QTextStream::RealNumberNotation::FixedNotation);
|
|
|
|
output << "TrajectoryVulcano at: " << QDateTime::currentDateTime().toString(QStringLiteral("yyyy.MM.dd hh:mm::ss")) << "\n";
|
|
output << "v0: " << v0 << "\n";
|
|
output << "betaDeg: " << betaRad*LtsCoordTrans::rad2deg() << "\n";
|
|
output << "vGlidingOut: " << vGlidingOut << "\n";
|
|
output << "vFallingOut: " << vFallingOut << "\n";
|
|
output << "alphaDeg: " << alphaRad*LtsCoordTrans::rad2deg() << "\n";
|
|
output << "tapTimeSec: " << tapTimeSec << "\n";
|
|
|
|
const double distance = (origin_metric - idealHit_metric).length();
|
|
output << "distance: " << distance << "\n";
|
|
|
|
|
|
output << "x_rel ; y_rel ; z ; relativeTime" << "\n";
|
|
|
|
if (!trajectoryMetricOut.isEmpty())
|
|
{
|
|
QPoint4D startPoint = trajectoryMetricOut.first();
|
|
|
|
for (const QPoint4D &p4D : qAsConst(trajectoryMetricOut))
|
|
{
|
|
output << p4D.x() - startPoint.x() << ";"
|
|
<< p4D.y() - startPoint.y() << ";"
|
|
<< p4D.z() << ";"
|
|
<< p4D.time() << ";"
|
|
<< "\n";
|
|
} // for
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
#endif // NEVER
|
|
//
|
|
//
|
|
//
|
|
|
|
|
|
|
|
|
|
// convert metric to geo/wgs84
|
|
{
|
|
LtsCoordTrans::UtmCoordinate tapUtm(idealHitUtm.zone, idealTapAbsoluteOut.x(), idealTapAbsoluteOut.y());
|
|
const QPointF &tapGeo = tapUtm.geographic();
|
|
idealTapAbsoluteGeoOut = Core::GeoPosition(tapGeo.x(), tapGeo.y(), idealTapAbsoluteOut.z());
|
|
}
|
|
|
|
|
|
trajectoryTimeAwareOut.clear();
|
|
trajectoryTimeAwareOut.reserve(trajectoryMetricOut.size());
|
|
|
|
for (const QPoint4D& pnt4d : qAsConst(trajectoryMetricOut))
|
|
{
|
|
LtsCoordTrans::UtmCoordinate pntUtm(idealHitUtm.zone, pnt4d.x(), pnt4d.y());
|
|
const QPointF &pntGeo = pntUtm.geographic();
|
|
|
|
// !!! relativeTime is in msec!!!
|
|
trajectoryTimeAwareOut.emplace_back( Core::GeoPositionRelativeTime(pntGeo.x(), pntGeo.y(), pnt4d.z(), pnt4d.time()*1000.0) );
|
|
}
|
|
|
|
|
|
if (reasonOut)
|
|
*reasonOut = localReasonOut;
|
|
|
|
return success;
|
|
}
|
|
|
|
|
|
//
|
|
// real implementation
|
|
//
|
|
|
|
|
|
inline void addItemTo(std::vector<Core::GeoPosition> &cont,
|
|
double x, double y, double z)
|
|
{
|
|
cont.emplace_back(Core::GeoPosition(x, y, z));
|
|
}
|
|
|
|
|
|
template<typename CONT_T>
|
|
bool TrajectoryGenerator::generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
|
|
const LtsUtils::QPoint3D &impactPosition_geo,
|
|
double elevation_angle_degree,
|
|
CONT_T &trajectoryOut)
|
|
{
|
|
// use zone of impact:
|
|
INFO_LOG(m_logger) << " oPosition geo " << originatorPosition_geo.x() << " - " << originatorPosition_geo.y();
|
|
INFO_LOG(m_logger) << " iPosition geo " << impactPosition_geo.x() << " - " << impactPosition_geo.y();
|
|
|
|
const LtsCoordTrans::UtmCoordinate &iPosition = LtsCoordTrans::UtmCoordinate::fromGeographic(impactPosition_geo.toPointF());
|
|
const LtsCoordTrans::UtmCoordinate &oPosition = LtsCoordTrans::UtmCoordinate::fromGeographic(originatorPosition_geo.toPointF(), iPosition.zone);
|
|
|
|
INFO_LOG(m_logger) << " oPosition " << oPosition.mgrs();
|
|
INFO_LOG(m_logger) << " iPosition " << iPosition.mgrs();
|
|
|
|
const double distance = sqrt(pow(iPosition.x - oPosition.x, 2.) + pow(iPosition.y - oPosition.y, 2.));
|
|
|
|
double oHeight = originatorPosition_geo.z();
|
|
double iHeight = impactPosition_geo.z();
|
|
|
|
if (std::isnan(oHeight) && m_ltsElevationManager)
|
|
{
|
|
oHeight = m_ltsElevationManager ? m_ltsElevationManager->elevation(originatorPosition_geo.x(), originatorPosition_geo.y()) : 0;
|
|
if (oHeight < 300)
|
|
oHeight = 100;
|
|
INFO_LOG(m_logger) << " oPosition " << oPosition << " has no zValue provided (NaN), update via ltsElevationManager...";
|
|
}
|
|
if (std::isnan(iHeight) && m_ltsElevationManager)
|
|
{
|
|
iHeight = m_ltsElevationManager ? m_ltsElevationManager->elevation(impactPosition_geo.x(), impactPosition_geo.y()) : 0;
|
|
if (iHeight < 300)
|
|
iHeight = 100;
|
|
INFO_LOG(m_logger) << " iPosition " << iPosition << " has no zValue provided (NaN), update via ltsElevationManager.. ";
|
|
}
|
|
|
|
INFO_LOG(m_logger) << " oPosition " << oPosition << " ; oHeight=" << oHeight;
|
|
INFO_LOG(m_logger) << " iPosition " << iPosition << " ; iHeight=" << iHeight;
|
|
|
|
// z-Values:
|
|
const QVector<double> &zValues = this->trajectoryBiedziak(distance, iHeight - oHeight, elevation_angle_degree);
|
|
|
|
if (zValues.isEmpty())
|
|
return false;
|
|
|
|
trajectoryOut.clear();
|
|
|
|
|
|
const QPointF &oPositionGeo = oPosition.geographic();
|
|
|
|
addItemTo(trajectoryOut,
|
|
oPositionGeo.x(), oPositionGeo.y(), oHeight);
|
|
|
|
const QPointF &step = (QPointF(iPosition.x, iPosition.y) - QPointF(oPosition.x, oPosition.y)) / (zValues.size() - 1);
|
|
QPointF runner;
|
|
for (int i = 1; i < zValues.size(); ++i)
|
|
{
|
|
runner += step;
|
|
const QPointF &p2D = oPosition.geographic(runner);
|
|
addItemTo(trajectoryOut,
|
|
p2D.x(), p2D.y(), oHeight + zValues[i]); // x, y, z
|
|
} // for
|
|
|
|
//// DEBUG_LOG(m_logger) << string4poly(geo);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
QVector<double> TrajectoryGenerator::trajectoryBiedziak(const double &distance,
|
|
const double &heightOffset,
|
|
double elevation_angle_degree,
|
|
int steps)
|
|
{
|
|
const double coefLinear = tan(elevation_angle_degree * LtsCoordTrans::deg2rad());
|
|
const double coefSquare = (heightOffset - distance * coefLinear) / pow(distance, 2.0);
|
|
const double delta = distance / steps;
|
|
|
|
double x = 0.;
|
|
QVector<double> yValues; // later interpreted as z-Values
|
|
yValues.reserve(steps);
|
|
yValues << 0.0;
|
|
for (int i = 0; i < steps; ++i)
|
|
{
|
|
x += delta;
|
|
yValues << (coefLinear * x + coefSquare * pow(x, 2.));
|
|
} // for
|
|
return yValues;
|
|
}
|
|
|
|
const QString TrajectoryGenerator::string4poly(const QPair<QPolygonF, QVector<double> > &pair)
|
|
{
|
|
if (pair.first.isEmpty())
|
|
return (QStringLiteral("Empty Polygon!"));
|
|
int zone = LtsCoordTrans::UtmCoordinate::fromGeographic(pair.first[0]).zone;
|
|
QStringList pointSl;
|
|
pointSl.reserve(pair.first.size());
|
|
for (int i = 0; i < pair.first.size(); ++i)
|
|
pointSl << QString(QStringLiteral("utm=%1 h=%2")).arg(LtsCoordTrans::UtmCoordinate::fromGeographic(pair.first[i], zone).mgrs(), QString::number(pair.second[i], 'f', 1));
|
|
return QStringLiteral("%1 Values (%2)").arg(pair.first.count()).arg(pointSl.join(QStringLiteral(" # ")));
|
|
}
|
|
|
|
|
|
|
|
//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
|