Initial commit
This commit is contained in:
torsten committed
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
@@ -0,0 +1,383 @@
|
||||
/**
|
||||
* @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
|
||||
Reference in new issue
Block a user