Initial commit
This commit is contained in:
torsten committed
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
@@ -0,0 +1,38 @@
|
||||
#
|
||||
# @RDE_HEADER@
|
||||
# $Id$
|
||||
# $HeadURL$
|
||||
# $Author$
|
||||
#
|
||||
|
||||
NGL_ADD_LIBRARY2(WS SV HLA Trajectory SHARED
|
||||
trajectory_generator.cpp
|
||||
trajectory_generator.h
|
||||
trajectory_helper.cpp
|
||||
trajectory_helper.h
|
||||
trajectory_generator_vulcano.cpp
|
||||
trajectory_generator_vulcano.h
|
||||
trajectory_point.h
|
||||
trajectory_transform.cpp
|
||||
trajectory_transform.h
|
||||
../logger.cpp
|
||||
../logger.h
|
||||
)
|
||||
NGL_TARGET_INCLUDE_DIRECTORIES(Trajectory
|
||||
INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../
|
||||
)
|
||||
NGL_TARGET_LINK_LIBRARIES(Trajectory
|
||||
PRIVATE
|
||||
NGLLogger
|
||||
AsmDataPluginInterface
|
||||
ExmDataPluginInterface
|
||||
PUBLIC
|
||||
LtsElevation
|
||||
LtsUtilsCore
|
||||
LtsCoordTrans
|
||||
Qt5::Core
|
||||
Qt5::Gui
|
||||
)
|
||||
|
||||
ADD_SUBDIRECTORY(UnitTest_Trajectory)
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
#
|
||||
# @RDE_HEADER@
|
||||
# $Id$
|
||||
# $HeadURL$
|
||||
# $Author$
|
||||
#
|
||||
|
||||
SET(LinkedSources
|
||||
log4cxxTrajectoryUnitTest.props
|
||||
)
|
||||
|
||||
SET(UnitTestSources
|
||||
trajectory_unit_test.cpp
|
||||
trajectory_unit_test.h
|
||||
)
|
||||
|
||||
IF(BUILD_TESTING)
|
||||
ADD_EXECUTABLE(aesUnitTestTrajectory ${LinkedSources} ${UnitTestSources} )
|
||||
|
||||
TARGET_LINK_LIBRARIES(aesUnitTestTrajectory
|
||||
NGLLogger
|
||||
nglcoreapplication
|
||||
svcAesCommon
|
||||
Qt5::Core
|
||||
Qt5::Gui
|
||||
Qt5::Test)
|
||||
|
||||
ADD_TEST(NAME aesUnitTestTrajectory COMMAND $<TARGET_FILE:aesUnitTestTrajectory>)
|
||||
|
||||
SET(LOG4CXX_PROPS_FILE log4cxxTrajectoryUnitTest.props)
|
||||
ADD_CUSTOM_COMMAND(TARGET aesUnitTestTrajectory POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND}
|
||||
ARGS -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/${LOG4CXX_PROPS_FILE} $<TARGET_FILE_DIR:aesUnitTestTrajectory>/${LOG4CXX_PROPS_FILE}
|
||||
)
|
||||
|
||||
ENDIF()
|
||||
Executable
+129
@@ -0,0 +1,129 @@
|
||||
log4j.reset=true
|
||||
log4j.rootLogger=INFO, file, stdout
|
||||
|
||||
# SituationMap
|
||||
###########################################################
|
||||
#log4j.logger.SituationMap.SitMapUnitTest=DEBUG
|
||||
#log4j.logger.SituationMap.SitMap=WARN
|
||||
|
||||
log4j.logger.SituationMap.LoggingTest=ERROR
|
||||
log4j.logger.SituationMap.LoggingTest.Chan1=INFO
|
||||
log4j.logger.SituationMap.LoggingTest.Chan2=ERROR
|
||||
|
||||
#log4j.logger.SituationMap.MapImpl=WARN
|
||||
#log4j.logger.SituationMap.LayerFactoryImpl=INFO
|
||||
#log4j.logger.SituationMap.CoordTranslationFunctorImpl=INFO
|
||||
#log4j.logger.SituationMap.GridImpl=WARN
|
||||
#log4j.logger.SituationMap.ObjectFactory=WARN
|
||||
#log4j.logger.SituationMap.IdGenerator=TRACE
|
||||
#log4j.logger.SituationMap.ObjectContainer=TRACE
|
||||
#log4j.logger.SituationMap.ObjectContainerCTC=TRACE
|
||||
#log4j.logger.SituationMap.ViewshedImpl=INFO
|
||||
#log4j.logger.SituationMap.ViewshedBase=INFO
|
||||
#log4j.logger.SituationMap.SymbolDictionary=WARN
|
||||
#log4j.logger.SituationMap.SymbolDictionaryImpl=WARN
|
||||
#log4j.logger.SituationMap.Sidc2525Delta=WARN
|
||||
|
||||
#log4j.logger.SituationMap.AbstractMapObjectImpl=INFO
|
||||
#log4j.logger.SituationMap.AbstractEffectSimulationImpl=INFO
|
||||
#log4j.logger.SituationMap.OriginTranslated=INFO
|
||||
#log4j.logger.SituationMap.PositionsTranslated=INFO
|
||||
#log4j.logger.SituationMap.AbstractMultiPointMapObjectImpl=INFO
|
||||
#log4j.logger.SituationMap.AbstractSinglePointMapObjectImpl=INFO
|
||||
#log4j.logger.SituationMap.BasicPathObjectImpl=INFO
|
||||
#log4j.logger.SituationMap.IndicationContainer=INFO
|
||||
|
||||
#log4j.logger.SituationMap.AreaFireImpl=INFO
|
||||
#log4j.logger.SituationMap.BiologicalAgentImpl=INFO
|
||||
#log4j.logger.SituationMap.BuildingOutlineImpl=INFO
|
||||
#log4j.logger.SituationMap.ChemicalAgentImpl=INFO
|
||||
#log4j.logger.SituationMap.DangerAreaImpl=INFO
|
||||
#log4j.logger.SituationMap.FunctionalAreaImpl=INFO
|
||||
#log4j.logger.SituationMap.HitLineImpl=INFO
|
||||
#log4j.logger.SituationMap.MineImpl=INFO
|
||||
#log4j.logger.SituationMap.NuclearAgentImpl=INFO
|
||||
#log4j.logger.SituationMap.ParticipantImpl=INFO
|
||||
#log4j.logger.SituationMap.SingleFireImpl=INFO
|
||||
|
||||
#log4j.logger.SituationMap.AbstractGraphicalObject=WARN
|
||||
#log4j.logger.SituationMap.GraphicalObjectEllipse=WARN
|
||||
#log4j.logger.SituationMap.GraphicalObjectPath=WARN
|
||||
#log4j.logger.SituationMap.GraphicalObjectMultiPath=WARN
|
||||
#log4j.logger.SituationMap.GraphicalObjectPictureMarker=WARN
|
||||
#log4j.logger.SituationMap.GraphicalObjectText=WARN
|
||||
#log4j.logger.SituationMap.TacticalAppearance=INFO
|
||||
#log4j.logger.SituationMap.MessageBase=INFO
|
||||
|
||||
|
||||
# Appnd_File
|
||||
###########################################################
|
||||
log4j.appender.file=org.apache.log4j.RollingFileAppender
|
||||
log4j.appender.file.MaxFileSize=1MB
|
||||
log4j.appender.file.MaxBackupIndex=9
|
||||
log4j.appender.file.file=${LOG4QT_LOGDIR}/${LOG4QT_USER}_SituationMap.log
|
||||
log4j.appender.file.layout=org.apache.log4j.PatternLayout
|
||||
log4j.appender.file.layout.ConversionPattern=%d %-5p %c |#| %m (%.50F:%L) %n
|
||||
|
||||
|
||||
# Appnd_Console
|
||||
###########################################################
|
||||
log4j.appender.stdout=org.apache.log4j.ConsoleAppender
|
||||
log4j.appender.stdout.layout=org.apache.log4j.PatternLayout
|
||||
log4j.appender.stdout.layout.ConversionPattern= %d %-5p %c{1} |#| %m (%-.30F:%L) %n
|
||||
|
||||
#log4j.appender.stdout.layout.ConversionPattern= %d %-5p %-30c |#| %m (%-.30F:%L) %n
|
||||
|
||||
|
||||
# Appnd_Syslog
|
||||
###########################################################
|
||||
log4j.appender.syslog=org.apache.log4j.SyslogAppender
|
||||
log4j.appender.syslog.Threshold=ERROR
|
||||
log4j.appender.syslog.SysLogHost=localhost
|
||||
log4j.appender.syslog.Facility=LOCAL7
|
||||
log4j.appender.syslog.layout=org.apache.log4j.PatternLayout
|
||||
log4j.appender.syslog.layout.ConversionPattern= %-4r %-5p %c %x - %m
|
||||
log4j.appender.syslog.LevelRangeFilter.AcceptOnMatch=true
|
||||
log4j.appender.syslog.LevelRangeFilter.LevelMin=INFO
|
||||
log4j.appender.syslog.LevelRangeFilter.LevelMax=FATAL
|
||||
|
||||
|
||||
###########################################################
|
||||
###########################################################
|
||||
###########################################################
|
||||
# 1) Available Levels in LOG4CXX Version 0.10.0:
|
||||
# TRACE,
|
||||
# DEBUG,
|
||||
# INFO,
|
||||
# WARN,
|
||||
# ERROR
|
||||
# FATAL
|
||||
#
|
||||
# d = date/time
|
||||
# p = priority (Level)
|
||||
# c = category/channel
|
||||
# m = message
|
||||
# F = file
|
||||
# L = line
|
||||
# n = platform dependent line separator character or characters.
|
||||
# ....
|
||||
#
|
||||
# Examples for console:
|
||||
# a)
|
||||
# %d %p %c #%m# (%.50F:%L)%n
|
||||
# date/time
|
||||
# priority
|
||||
# category/channel
|
||||
# #message#
|
||||
# (Maximal 50Character des Filename von rechts:Line)
|
||||
# line separator
|
||||
#
|
||||
# b)
|
||||
# %d (%.15F:%L) %-5p %c |#| %m%n
|
||||
# date/time
|
||||
# (Maximal 15Character des Filename von rechts:Line)
|
||||
# priority, Right pad with spaces if the priority name is less than 5 characters long.
|
||||
# category/channel
|
||||
# message
|
||||
# line separator
|
||||
#
|
||||
|
||||
Executable
+1009
File diff suppressed because it is too large.
Load diff
+83
@@ -0,0 +1,83 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* Unittest
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QCoreApplication>
|
||||
#include <QtTest/QTest>
|
||||
#include <NGLLogger/INGLLogger.h>
|
||||
|
||||
using LOGGER_T = NGL::INGLLogger&;
|
||||
|
||||
|
||||
class UnitTestTrajectory : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
UnitTestTrajectory();
|
||||
|
||||
|
||||
private Q_SLOTS:
|
||||
|
||||
// void init();
|
||||
|
||||
|
||||
void test_copy_long_vector();
|
||||
|
||||
void test_duration();
|
||||
void test_calcDuration();
|
||||
void test_calcDuration2();
|
||||
|
||||
void test_calcPeak();
|
||||
void test_calcPeak2();
|
||||
|
||||
void test_calcPeak3_continuity();
|
||||
|
||||
void test_calcTimeToPeakAndBack();
|
||||
|
||||
void test_calcDuration_realistic40km();
|
||||
|
||||
void test_calcDuration_differentDistances();
|
||||
|
||||
void test_calcDuration_downToZero();
|
||||
|
||||
// for context AirSupport:
|
||||
void test_realStartPoint_data();
|
||||
void test_realStartPoint();
|
||||
|
||||
// generateTrajectoryBiedziak
|
||||
void test_generateTrajectoryBiedziak();
|
||||
|
||||
|
||||
// Vulcano:
|
||||
void test_vulcano_beta_Abschusswinkel();
|
||||
void test_vulcano_apogaeumCosmos();
|
||||
|
||||
void test_vulcano_apogaeum_data();
|
||||
void test_vulcano_apogaeum();
|
||||
|
||||
void test_vulcano_apogaeumStrich_data();
|
||||
void test_vulcano_apogaeumStrich();
|
||||
|
||||
void test_vulcano_tap_data();
|
||||
void test_vulcano_tap();
|
||||
|
||||
void test_vulcano_ballisticCurve_data();
|
||||
void test_vulcano_ballisticCurve();
|
||||
|
||||
void test_vulcano_trajectory_data();
|
||||
void test_vulcano_trajectory();
|
||||
|
||||
// void cleanup();
|
||||
|
||||
private:
|
||||
|
||||
LOGGER_T m_logger;
|
||||
|
||||
};
|
||||
@@ -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
|
||||
@@ -0,0 +1,115 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* TrajectoryGenerator
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Norbert Thoden, SLTE, 2023-11-08
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../logger.h"
|
||||
#include "trajectory_generator_vulcano.h"
|
||||
#include <Core/core.hpp> // Lts/DDS/Core/core.hpp for Core::GeoPosition
|
||||
#include <LtsElevation/lts_elevation_manager.h>
|
||||
#include <LtsUtilsCore/qpoint3d.h>
|
||||
#include <Trajectory/trajectory_export.h>
|
||||
|
||||
#include <QObject>
|
||||
#include <QPair>
|
||||
#include <QPolygonF>
|
||||
|
||||
#include <vector>
|
||||
|
||||
// forward
|
||||
namespace ExerciseManagement
|
||||
{
|
||||
namespace Interface
|
||||
{
|
||||
class Dtc;
|
||||
}
|
||||
} // namespace ExerciseManagement
|
||||
|
||||
namespace AssetManagement
|
||||
{
|
||||
namespace Interface
|
||||
{
|
||||
class Dtc;
|
||||
}
|
||||
} // namespace AssetManagement
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
// version 1
|
||||
using TrajectoryGeoPosition = std::vector<Core::GeoPosition>;
|
||||
// version 2
|
||||
using TrajectoryGeoPositionRelTime = std::vector<Core::GeoPositionRelativeTime>;
|
||||
|
||||
using QPoint3D = LtsUtils::QPoint3D;
|
||||
|
||||
//
|
||||
// class TrajectoryGenerator
|
||||
//
|
||||
class TRAJECTORY_EXPORT TrajectoryGenerator : public QObject
|
||||
{
|
||||
public:
|
||||
/*QT_DEPRECATED*/ static TrajectoryGenerator *getInstance(int exerciseId,
|
||||
const std::shared_ptr<ExerciseManagement::Interface::Dtc> &exmDtc,
|
||||
const std::shared_ptr<AssetManagement::Interface::Dtc> &asmDtc);
|
||||
|
||||
static TrajectoryGenerator *getInstance();
|
||||
|
||||
static void cleanup();
|
||||
|
||||
void initElevation(LtsElevation::LtsElevationManager *ltsElevationManager);
|
||||
LtsElevation::LtsElevationManager *getElevationManager();
|
||||
bool hasElevationManager() const;
|
||||
|
||||
~TrajectoryGenerator() = default;
|
||||
|
||||
/*! \name Ideal trajectory using Core::GeoPosition at return trajectory ... */
|
||||
//@{
|
||||
bool generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
|
||||
const LtsUtils::QPoint3D &idealHit_geo,
|
||||
double elevation_angle_degree, // aka beta
|
||||
TrajectoryGeoPosition &trajectoryOut);
|
||||
//@}
|
||||
|
||||
static bool generateTrajectoryVulcano(const QPoint3D &originatorPosition_geo,
|
||||
const QPoint3D &idealHit_geo,
|
||||
double v0,
|
||||
double betaRad, // Abschussgeschwindigkeit, -winkel ( betaRad ~ elevation_angle_degree
|
||||
double alphaRad,
|
||||
double tapTimeSec, // Fallwinkel, -geschwindigkeit, -zeit
|
||||
TrajectoryGeneratorVulcano::Options options,
|
||||
TrajectoryGeoPositionRelTime &trajectoryTimeAwareOut,
|
||||
Core::GeoPosition &idealTapAbsoluteGeoOut,
|
||||
QString *reasonOut);
|
||||
|
||||
protected:
|
||||
/*! \name Ideal trajectory */
|
||||
template <typename CONT_T>
|
||||
bool generateTrajectoryBiedziak(const LtsUtils::QPoint3D &originatorPosition_geo,
|
||||
const LtsUtils::QPoint3D &impactPosition_geo,
|
||||
double elevation_angle_degree,
|
||||
CONT_T &trajectoryOut);
|
||||
|
||||
protected:
|
||||
explicit TrajectoryGenerator();
|
||||
|
||||
public:
|
||||
// For UnitTest:
|
||||
friend class UnitTestTrajectoryHelper;
|
||||
|
||||
// calculate the z-Values of the trajectory:
|
||||
static QVector<double> trajectoryBiedziak(const double &distance, const double &heightOffset, double elevation_angle_degree, int steps = 72);
|
||||
|
||||
static const QString string4poly(const QPair<QPolygonF, QVector<double>> &pair);
|
||||
|
||||
LtsElevation::LtsElevationManager *m_ltsElevationManager{nullptr};
|
||||
LOGGER_T m_logger;
|
||||
};
|
||||
|
||||
} // namespace AreaEffectSimulation
|
||||
+676
@@ -0,0 +1,676 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* LTS - logging facility
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Norbert Thoden, SLTE, 2023-11-08
|
||||
*/
|
||||
|
||||
#include "trajectory_generator_vulcano.h"
|
||||
|
||||
#include <QObject>
|
||||
#include <QPointF>
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
//#define G 9.80665
|
||||
#define G 9.81
|
||||
|
||||
inline void addItemLocal(TRAJECTORY_METRIC &trajectoryRef, const QPoint3D &point, double time = 0.0)
|
||||
{
|
||||
trajectoryRef.push_back(QPoint4D(point.x(), point.y(), point.z(), time));
|
||||
}
|
||||
|
||||
|
||||
LOGGER_T getLocalLogger()
|
||||
{
|
||||
return getLogger(QStringLiteral("TrajectoryGeneratorVulcano"));
|
||||
}
|
||||
|
||||
|
||||
bool TrajectoryGeneratorVulcano::trajectoryVulcano(const QPoint3D &origin, const QPoint3D &idealHit,
|
||||
double v0, double betaRad,
|
||||
double vGliding,
|
||||
double vFalling, double alphaRad, double tapTimeSec,
|
||||
TrajectoryGeneratorVulcano::Options options,
|
||||
TRAJECTORY_METRIC &trajectoryOut,
|
||||
QPoint3D &idealTapAbsoluteOut,
|
||||
QString *reasonOut)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION
|
||||
<< ", origin " << origin << ", idealHit " << idealHit
|
||||
<< ", v0=" << v0 << ", betaDeg=" << betaRad*LtsCoordTrans::rad2deg()
|
||||
<< ", vGliding=" << vGliding
|
||||
<< ", vFalling=" << vFalling << ", alphaDeg=" << alphaRad*LtsCoordTrans::rad2deg() << ", tapTimeSec=" << tapTimeSec
|
||||
<< ", options " << options;
|
||||
|
||||
idealTapAbsoluteOut = QPoint3D();
|
||||
|
||||
// OptRasterTapHit
|
||||
//
|
||||
// very simple check:
|
||||
//
|
||||
const QVector2D dist = QVector2D(idealHit.toPointF() - origin.toPointF());
|
||||
|
||||
if (dist.length() < 100)
|
||||
{
|
||||
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", dist is very small (100m)";
|
||||
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("DistanceError(%1 > %2)").arg(dist.length()).arg(100);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (v0 < vFalling)
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", v0 ist less than vFalling";
|
||||
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("SpeedError(v0=%1 ; vF %2)").arg(v0).arg(vFalling);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// line between origin and idealHit:
|
||||
//
|
||||
const QVector2D &distHitAndOrigin = QVector2D(idealHit.toPointF() - origin.toPointF());
|
||||
const double length = distHitAndOrigin.length();
|
||||
|
||||
const double factorX = distHitAndOrigin.x() / length;
|
||||
const double factorY = distHitAndOrigin.y() / length;
|
||||
|
||||
|
||||
//
|
||||
// Apo
|
||||
//
|
||||
const QPointF &apo2D = XY::calculateApogaeum(v0, betaRad);
|
||||
|
||||
// translate from XY to XYZ
|
||||
QPoint3D apoAbs(apo2D.x() * factorX, // .x() !!
|
||||
apo2D.x() * factorY, // .x() !!
|
||||
apo2D.y());
|
||||
apoAbs += origin;
|
||||
|
||||
// check origin -> apo -> idealHit
|
||||
if (!checkSimple(origin, apoAbs, idealHit, reasonOut))
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", checkOriginApoHit failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
//
|
||||
// TAP
|
||||
//
|
||||
// vFalling is maybe slightly smaller than v0;
|
||||
// tapTime is typically 8 sec
|
||||
const QPointF &tapRelToImpact = XY::calculateTapRelToIdealHit(vFalling, alphaRad, tapTimeSec);
|
||||
|
||||
// translate from XY to XYZ
|
||||
QPoint3D tapAbs(tapRelToImpact.x() * factorX, // .x() !!
|
||||
tapRelToImpact.x() * factorY, // .x() !!
|
||||
tapRelToImpact.y());
|
||||
tapAbs += idealHit;
|
||||
|
||||
// set out value:
|
||||
idealTapAbsoluteOut = tapAbs;
|
||||
|
||||
// check origin -> apo -> TAP - >idealHit
|
||||
if (!checkSimple(origin, apoAbs, tapAbs, idealHit, reasonOut))
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << " checkOriginApoTapHit failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// raster phase 1 (ballistic curve)
|
||||
//
|
||||
// try to generate a value each second:
|
||||
//
|
||||
double x_distancePhase1 = apo2D.x(); // maybe move to Apo' in future (intersection between ballisitic curve an line to TAP)
|
||||
|
||||
// does not work: const double x_distancePhase1Strich = XY::calculateApogaeumStrichEval(origin.toPointF(), idealHit.toPointF(), v0, betaRad, tapAbs.toPointF());
|
||||
// so create temoporary originTemp, idealHitTemp: // ignoring the zValue
|
||||
// {
|
||||
// const QPointF originTemp;
|
||||
// const QPointF idealHitTemp(dist.length(), 0);
|
||||
// const QPointF tapTemp = idealHitTemp + tapRelToImpact;
|
||||
|
||||
// double x_distancePhase1Strich = XY::calculateApogaeumStrichEval(originTemp, idealHitTemp, v0, betaRad, tapTemp);
|
||||
|
||||
// ERROR_LOG(logger) << " x_distancePhase1a = " << x_distancePhase1;
|
||||
// ERROR_LOG(logger) << " x_distancePhase1b = " << x_distancePhase1Strich;
|
||||
|
||||
// if (!std::isnan(x_distancePhase1Strich) && (x_distancePhase1Strich > x_distancePhase1))
|
||||
// x_distancePhase1 = x_distancePhase1Strich;
|
||||
|
||||
// ERROR_LOG(logger) << " apo2D.x() = " << apo2D.x()
|
||||
// << ", x_distancePhase1Strich = " << x_distancePhase1Strich
|
||||
// << ", finally use: x_distancePhase1 = " << x_distancePhase1;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
const double secPerIntervall = 1;
|
||||
double x_delta_per_step_out = -1;
|
||||
double secPerIntervall_out = -1;
|
||||
const QVector<QPointF> &phase1_bal = XY::generateBallisticCurveUntil(v0, betaRad, x_distancePhase1, secPerIntervall,
|
||||
&x_delta_per_step_out, &secPerIntervall_out);
|
||||
|
||||
|
||||
|
||||
trajectoryOut.clear();
|
||||
trajectoryOut.reserve(phase1_bal.size()+5);
|
||||
|
||||
int cnt = 0;
|
||||
for (const QPointF &xyVertex : qAsConst(phase1_bal))
|
||||
{
|
||||
// translate from XY to XYZ
|
||||
const QPoint3D p3d_offset(xyVertex.x() * factorX, // xyVertex.x()
|
||||
xyVertex.x() * factorY, // xyVertex.x()
|
||||
xyVertex.y());
|
||||
// is relative to origin (gun)
|
||||
addItemLocal(trajectoryOut, p3d_offset+origin, (secPerIntervall_out*cnt));
|
||||
++cnt;
|
||||
} // for
|
||||
|
||||
|
||||
// ToDo: Check!!!
|
||||
|
||||
// const double distPhase1 = apo2D.x();
|
||||
// const double timeSpanForPhase1Sec = distPhase1 / v0 * cos(betaRad); // / xy-Anteil der Abschussgeschwindigkeit
|
||||
const double timeSpanForPhase1Sec = trajectoryOut.last().time();
|
||||
double timeSpanForPhase2Sec = -1;
|
||||
double timeSpanForPhase3Sec = -1;
|
||||
|
||||
INFO_LOG(logger) << "PHASE 1: generateBallisticCurveUntil: "
|
||||
<< ", phase1_bal.size() = " << phase1_bal.size()
|
||||
<< ", x_delta_per_step_out = " << x_delta_per_step_out
|
||||
<< ", secPerIntervall_out = " << secPerIntervall_out
|
||||
<< ", v0 * cos(betaRad) = " << v0 * cos(betaRad)
|
||||
<< ", timeSpanForPhase1Sec = " << timeSpanForPhase1Sec;
|
||||
|
||||
|
||||
//
|
||||
// add gliding phase / tap
|
||||
//
|
||||
|
||||
double timeOffset = 0;
|
||||
|
||||
const double distPhase2 = (apoAbs - tapAbs).length();
|
||||
timeSpanForPhase2Sec = distPhase2 / vGliding;
|
||||
INFO_LOG(logger) << " continue with PHASE 2"
|
||||
<< ", distPhase2 = " << distPhase2
|
||||
<< ", timeSpanForPhase2Sec = " << timeSpanForPhase2Sec;
|
||||
|
||||
// continue rastering?
|
||||
if (options.testFlag(Option::OptRasterApoTap))
|
||||
{
|
||||
// which kind of rastering?
|
||||
if (options.testFlag(Option::OptRasterMetrical))
|
||||
{
|
||||
// dist from APO (or APO') to TAP: ( apoAbs , tapAbs )
|
||||
double realDelta_xy_out = -1;
|
||||
rasterLineByDistance(apoAbs, tapAbs, x_delta_per_step_out, trajectoryOut, &realDelta_xy_out);
|
||||
DEBUG_LOG(logger) << " rastering was done for line ApoTap: realDelta_xy_out " << realDelta_xy_out;
|
||||
}
|
||||
else
|
||||
{
|
||||
// we can use x_delta_per_step_out and or secPerIntervall_out
|
||||
double realDeltaTime_out = -1;
|
||||
rasterLineByTime(apoAbs, tapAbs,
|
||||
timeSpanForPhase1Sec, vGliding, 1,
|
||||
trajectoryOut, &realDeltaTime_out);
|
||||
|
||||
DEBUG_LOG(logger) << " rastering was done for line ApoTap: "
|
||||
<< ", distPhase2 =" << distPhase2
|
||||
<< ", timeSpanForPhase2Sec =" << timeSpanForPhase2Sec
|
||||
<< ", realDeltaTime_out " << realDeltaTime_out;
|
||||
}
|
||||
} else {
|
||||
// TabAbs has to be added in any case!!!
|
||||
addItemLocal(trajectoryOut, tapAbs, timeSpanForPhase1Sec + timeSpanForPhase2Sec);
|
||||
}
|
||||
|
||||
// prepare timeOffset for next phase:
|
||||
const QPoint4D &pnt4DRef = trajectoryOut.back();
|
||||
timeOffset = pnt4DRef.time();
|
||||
|
||||
//
|
||||
// add falling phase / idealHit
|
||||
//
|
||||
if (options.testFlag(Option::OptIncludeIdealHit))
|
||||
{
|
||||
const double distPhase3 = (idealHit - tapAbs).length();
|
||||
timeSpanForPhase3Sec = distPhase3 / vFalling;
|
||||
INFO_LOG(logger) << " continue with PHASE 3"
|
||||
<< ", distPhase3 = " << distPhase3
|
||||
<< ", timeSpanForPhase3Sec = " << timeSpanForPhase3Sec;
|
||||
|
||||
if (options.testFlag(Option::OptRasterTapHit))
|
||||
{
|
||||
// which kind of rastering?
|
||||
if (options.testFlag(Option::OptRasterMetrical))
|
||||
{
|
||||
// we can use x_delta_per_step_out and or secPerIntervall_out
|
||||
// dist from TAP to hit: ( tapAbs , idealHit )
|
||||
double realDelta_xy_out = -1;
|
||||
rasterLineByDistance(tapAbs, idealHit, x_delta_per_step_out, trajectoryOut, &realDelta_xy_out);
|
||||
DEBUG_LOG(logger) << " rastering was done for line TapHit: realDelta_xy_out " << realDelta_xy_out;
|
||||
} else {
|
||||
double realDeltaTime_out = -1;
|
||||
rasterLineByTime(tapAbs, idealHit,
|
||||
timeOffset, vFalling, 1,
|
||||
trajectoryOut, &realDeltaTime_out);
|
||||
|
||||
DEBUG_LOG(logger) << " rastering was done for line TapHit: realDeltaTime_out " << realDeltaTime_out
|
||||
<< ", distPhase3 = " << distPhase3
|
||||
<< ", vFalling = " << vFalling
|
||||
<< ", timeSpanForPhase3Sec = " << timeSpanForPhase3Sec;
|
||||
}
|
||||
} else {
|
||||
// simply add single point:
|
||||
addItemLocal(trajectoryOut, idealHit, timeSpanForPhase1Sec + timeSpanForPhase2Sec + timeSpanForPhase3Sec);
|
||||
}
|
||||
}
|
||||
|
||||
const double timeSpanComplete = timeSpanForPhase1Sec + timeSpanForPhase2Sec + timeSpanForPhase3Sec;
|
||||
const double diffTime = trajectoryOut.last().time() - timeSpanComplete;
|
||||
INFO_LOG(logger) << " rastering was done"
|
||||
<< ", timeSpanForPhase1Sec = " << timeSpanForPhase1Sec
|
||||
<< ", timeSpanForPhase2Sec = " << timeSpanForPhase2Sec
|
||||
<< ", timeSpanForPhase3Sec = " << timeSpanForPhase3Sec
|
||||
<< ", timeSpanComplete = " << timeSpanComplete
|
||||
<< " ### diffTime related to .last() = " << diffTime;
|
||||
|
||||
Q_ASSERT(diffTime < 1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TrajectoryGeneratorVulcano::checkSimple(const QPoint3D &origin,
|
||||
const QPoint3D &apoAbs,
|
||||
const QPoint3D &idealHit,
|
||||
QString *reasonOut)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
DEBUG_LOG(logger) << ", origin = " << origin
|
||||
<< ", apoAbs = " << apoAbs
|
||||
<< ", idealHit = " << idealHit;
|
||||
|
||||
const QVector2D hitDist = QVector2D(idealHit.toPointF() - origin.toPointF());
|
||||
const QVector2D apoDist = QVector2D(apoAbs.toPointF() - origin.toPointF());
|
||||
|
||||
// apo dist must be less than the half of (idealHit-origin)
|
||||
if (apoDist.length() > (hitDist.length() / 2.0))
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", apo dist must be less than the half of (idealHit-origin)"
|
||||
<< ", origin = " << origin
|
||||
<< ", apoAbs = " << apoAbs
|
||||
<< ", idealHit = " << idealHit
|
||||
<< ", apoDist.length() = " << apoDist.length()
|
||||
<< ", hitDist.length()/2 = " << hitDist.length()/2;
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("DistanceError(apogaeumDistance=%1 = impactDistance/2=%2)").arg(apoDist.length()).arg((hitDist.length() / 2.0));
|
||||
return false;
|
||||
}
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION << ", apo dist OKAY :-) the half of (idealHit-origin)"
|
||||
<< ", origin = " << origin
|
||||
<< ", apoAbs = " << apoAbs
|
||||
<< ", idealHit = " << idealHit
|
||||
<< ", apoDist.length() = " << apoDist.length()
|
||||
<< ", hitDist.length()/2 = " << hitDist.length()/2;
|
||||
|
||||
if (apoAbs.z() < idealHit.z())
|
||||
{
|
||||
ERROR_LOG(logger) << LOGFUNCTION << ", apoAbs.z() is less than idealHit.z()"
|
||||
<< "( apoAbs=" << apoAbs
|
||||
<< "; idealHit=" << idealHit << ")";
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("HeigthError(apogaeum.z=%1 >= impact.z=%2)").arg(apoAbs.z()).arg(idealHit.z());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TrajectoryGeneratorVulcano::checkSimple(const QPoint3D &origin,
|
||||
const QPoint3D &apoAbs,
|
||||
const QPoint3D &tabAbs,
|
||||
const QPoint3D &idealHit, QString *reasonOut)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION
|
||||
<< ", origin = " << origin
|
||||
<< ", apoAbs = " << apoAbs
|
||||
<< ", tabAbs = " << tabAbs
|
||||
<< ", idealHit = " << idealHit;
|
||||
|
||||
// apo dist must be less than the half of (idealHit-origin)
|
||||
if (apoAbs.z() < tabAbs.z())
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", apo.z() dist must higher than tab.z()"
|
||||
<< "( apoAbs=" << apoAbs
|
||||
<< "; tabAbs=" << tabAbs << ")";
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("HeigthError(apogaeum.z=%1 >= tap.z=%2)").arg(apoAbs.z()).arg(tabAbs.z());
|
||||
return false;
|
||||
}
|
||||
|
||||
// tabAbs must 'between' apo and idealHit;
|
||||
const QVector2D apoDist = QVector2D(apoAbs.toPointF() - origin.toPointF());
|
||||
const QVector2D tabDist = QVector2D(tabAbs.toPointF() - origin.toPointF());
|
||||
const QVector2D hitDist = QVector2D(idealHit.toPointF() - origin.toPointF());
|
||||
|
||||
if (tabDist.length() < apoDist.length())
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", tab dist must be less than apo dist"
|
||||
<< "( apoAbs=" << apoAbs
|
||||
<< "; tabAbs=" << tabAbs
|
||||
<< "; idealHit=" << idealHit << ")";
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("DistanceError(tabDist=%1 >= apoDist=%2)").arg(tabDist.length()).arg(apoDist.length());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (hitDist.length() < tabDist.length())
|
||||
{
|
||||
WARNING_LOG(logger) << LOGFUNCTION << ", hitDist dist must be less than tabDist dist"
|
||||
<< "( apoAbs=" << apoAbs
|
||||
<< "; tabAbs=" << tabAbs
|
||||
<< "; idealHit=" << idealHit << ")";
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("DistanceError(hitDist=%1 >= tabDist=%2)").arg(hitDist.length()).arg(tabDist.length());
|
||||
return false;
|
||||
}
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION
|
||||
<< ", apoAbs=" << apoAbs
|
||||
<< ", tabAbs=" << tabAbs
|
||||
<< ", idealHit=" << idealHit;
|
||||
return true;
|
||||
}
|
||||
|
||||
void TrajectoryGeneratorVulcano::getSpeedPerPhase(double v0_in, double betaRad,
|
||||
double &vGlidingOut, double &vFallingOut)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
Q_UNUSED(betaRad);
|
||||
|
||||
vGlidingOut = 700; // Gleitgeschwindigkeit
|
||||
vFallingOut = 375; // 3000m Höhe bei TapT=8 sec -> 375 m/s // Fallgeschwindigkeit
|
||||
|
||||
DEBUG_LOG(logger) << " v0_in " << v0_in
|
||||
<< ", betaDeg " << betaRad * LtsCoordTrans::rad2deg()
|
||||
<< ", vGlidingOut " << vGlidingOut << ", vFallingOut " << vFallingOut;
|
||||
}
|
||||
|
||||
void TrajectoryGeneratorVulcano::rasterLineByDistance(const QPoint3D &p0, const QPoint3D &p1,
|
||||
double desiredDelta_xy,
|
||||
TRAJECTORY_METRIC &trajectoryOut, double *realDelta_xy_out)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
// we can use x_delta_per_step_out and maybe secPerIntervall_out
|
||||
// dist from APO (or APO') to TAP: ( apoAbs , tapAbs )
|
||||
// DistXY:
|
||||
const double distMeter = QVector2D( p1.toPointF() - p0.toPointF() ).length();
|
||||
|
||||
const int steps = (distMeter/desiredDelta_xy); // implicit round
|
||||
const double realDelta_x = distMeter / steps;
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION
|
||||
<< " desiredDelta_xy " << desiredDelta_xy
|
||||
<< ", distMeter " << distMeter
|
||||
<< ", steps " << steps
|
||||
<< ", realDelta_x " << realDelta_x;
|
||||
|
||||
// split 3dim Vector between both points according step:
|
||||
const QPoint3D &dist3D = p1 - p0; // keep in mind: tapAbs - apoAbs
|
||||
|
||||
// step by distance
|
||||
for (int i=1; i <= steps; ++i) // use 'i <= steps'
|
||||
{
|
||||
// try to avoid to much calculation, like:
|
||||
// const QPoint3D &curPointAbs = p0 + (dist3D * i / steps);
|
||||
// very small neg values may occur: item (30000,70,2.27374e-13)
|
||||
// ^^^^^^^^^^^
|
||||
const QPoint3D curPointAbs = (i==steps) ? p0 + dist3D : p0 + (dist3D * i / steps);
|
||||
// DEBUG_LOG(logger) << " i " << i
|
||||
// << ", curPointAbs = " << curPointAbs;
|
||||
addItemLocal(trajectoryOut, curPointAbs);
|
||||
} // for
|
||||
|
||||
if (realDelta_xy_out)
|
||||
*realDelta_xy_out = realDelta_x;
|
||||
}
|
||||
|
||||
void TrajectoryGeneratorVulcano::rasterLineByTime(const QPoint3D &p0, const QPoint3D &p1,
|
||||
double timeOffsetSec, double speedMeterPerSec, double desiredDeltaTime,
|
||||
TRAJECTORY_METRIC &trajectoryOut, double *realDeltaTime_out)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
const QPoint3D &dist3D = p1 - p0; // keep in mind: tapAbs - apoAbs
|
||||
const double timeSpanForDistance = dist3D.length() / speedMeterPerSec;
|
||||
|
||||
Q_ASSERT(timeSpanForDistance > 1);
|
||||
|
||||
const int steps = timeSpanForDistance / desiredDeltaTime; // implicit round;
|
||||
const double realDeltaTime = timeSpanForDistance / steps;
|
||||
|
||||
Q_ASSERT(realDeltaTime > 0);
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION
|
||||
<< " timeOffsetSec " << timeOffsetSec
|
||||
<< ", speedMeterPerSec " << speedMeterPerSec
|
||||
<< ", desiredDeltaTime " << desiredDeltaTime
|
||||
<< ", dist3D.length " << dist3D.length()
|
||||
<< ", steps " << steps
|
||||
<< ", realDeltaTime " << realDeltaTime;
|
||||
|
||||
// split 3dim Vector between both points according step:
|
||||
|
||||
// step by time:
|
||||
for (int i=1; i <= steps; ++i) // use 'i <= steps'
|
||||
{
|
||||
// try to avoid to much calculation, like:
|
||||
// const QPoint3D &curPointAbs = p0 + (dist3D * i / steps);
|
||||
// very small neg values may occur: item (30000,70,2.27374e-13)
|
||||
// ^^^^^^^^^^^
|
||||
const QPoint3D &curPointAbs = (i==steps) ? p0 + dist3D : p0 + (dist3D * i / steps);
|
||||
double timeAbsToOffset = timeOffsetSec + (i*realDeltaTime);
|
||||
// DEBUG_LOG(logger) << " i " << i
|
||||
// << ", curPointAbs " << curPointAbs
|
||||
// << " - " << " timeAbsToOffset: " << timeAbsToOffset;
|
||||
addItemLocal(trajectoryOut, curPointAbs, timeAbsToOffset);
|
||||
} // for
|
||||
|
||||
if (realDeltaTime_out)
|
||||
*realDeltaTime_out = realDeltaTime;
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// some helper
|
||||
//
|
||||
namespace XY
|
||||
{
|
||||
double calculateBetaRad(double v0, double peakHeightRelToOrigin, QString *reasonOut)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION << ", v0 = " << v0 << ", peakHeightRelToOrigin = " << peakHeightRelToOrigin;
|
||||
|
||||
const double betaRad = asin(sqrt(2.0 * G * peakHeightRelToOrigin / (v0 * v0)));
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION << ", v0 = " << v0 << ", peakHeightRelToOrigin = " << peakHeightRelToOrigin
|
||||
<< " -> betaRad = " << betaRad << " betaDeg = " << betaRad * LtsCoordTrans::rad2deg();
|
||||
|
||||
if (std::isnan(betaRad))
|
||||
{
|
||||
ERROR_LOG(logger) << LOGFUNCTION << ", v0 = " << v0 << ", peakHeightRelToOrigin = " << peakHeightRelToOrigin
|
||||
<< " -> betaRad = " << betaRad << " betaDeg = " << betaRad * LtsCoordTrans::rad2deg();
|
||||
|
||||
if (reasonOut)
|
||||
*reasonOut = QObject::tr("calculate angle beta failed(v0=%1 ; pH %2 ; beta=%3)").arg(v0).arg(peakHeightRelToOrigin).arg(betaRad);
|
||||
}
|
||||
|
||||
return betaRad;
|
||||
}
|
||||
|
||||
QPointF calculateApogaeum(double v0, double betaRad)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
TRACE_LOG(logger) << LOGFUNCTION << ", v0 = " << v0 << ", betaRad = " << betaRad << ", betaDeg = " << betaRad * LtsCoordTrans::rad2deg();
|
||||
|
||||
QPointF apogaeum;
|
||||
|
||||
apogaeum.rx() = v0 * v0 * sin(2.0 * betaRad) / (2.0 * G);
|
||||
apogaeum.ry() = v0 * v0 * sin(betaRad) * sin(betaRad) / (2.0 * G);
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION << ", v0 = " << v0 << ", betaRad = " << betaRad << ", betaDeg = " << betaRad * LtsCoordTrans::rad2deg()
|
||||
<< ", apogaeum = " << apogaeum.x() << " # " << apogaeum.y();
|
||||
|
||||
return apogaeum;
|
||||
}
|
||||
|
||||
double calculateApogaeumStrichEval(const QPointF &origin, const QPointF &idealHit,
|
||||
double v0, double betaRad, const QPointF &tap_relToOrigin)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
DEBUG_LOG(logger) << LOGFUNCTION
|
||||
<< ", origin = " << origin
|
||||
<< ", idealHit = " << idealHit
|
||||
<< ", v0 = " << v0 << ", betaRad = " << betaRad << ", betaDeg = " << betaRad * LtsCoordTrans::rad2deg()
|
||||
<< ", tap_relToOrigin = " << tap_relToOrigin;
|
||||
|
||||
// Berechnung Flugzeit T_ApoStrich vom Start bis APO'
|
||||
const double a = - G / 2.0;
|
||||
const double b = v0 * sin(betaRad);
|
||||
const double h_TAP = tap_relToOrigin.y(); // y-Koordinate (Höhe) vom TAP
|
||||
|
||||
// Unter der nur hier getroffenen Annahme, dass das Geschoss über den
|
||||
// gesamten Verlauf der Fluggkurve mit einer konstanten Geschwindigkeit bewegt:
|
||||
const double T_TAP = tap_relToOrigin.x() / v0; // Flugzeit des Geschosses vom Abschuss bis zum TAP
|
||||
|
||||
double temp = a * (T_TAP * ( a * T_TAP + b) - h_TAP);
|
||||
|
||||
if (temp < 0)
|
||||
FATAL_LOG(logger) << " temp NEGATIVE! " << temp;
|
||||
else
|
||||
DEBUG_LOG(logger) << " temp POSITIVE! " << temp;
|
||||
|
||||
|
||||
// const double T_ApoStrich = T_TAP + sqrt(a * (T_TAP * ( a* T_TAP + b) - h_TAP)) / a;
|
||||
const double T_ApoStrich = T_TAP + sqrt(a * (T_TAP * ( a* T_TAP + b) - h_TAP)) / a;
|
||||
|
||||
// Berechnung Strecke vom Start bis APO' (aka distancePhase1a)
|
||||
const double apoStrich_x = v0 * cos(betaRad) * T_ApoStrich;
|
||||
|
||||
DEBUG_LOG(logger) << LOGFUNCTION << ", T_TAP = " << T_TAP
|
||||
<< ", temp = " << temp
|
||||
<< ", T_ApoStrich = " << T_ApoStrich
|
||||
<< ", apoStrich_x = " << apoStrich_x;
|
||||
return apoStrich_x;
|
||||
}
|
||||
|
||||
QPointF calculateTapRelToIdealHit(double vFalling, double alphaRad, double timeTapT_sec)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
DEBUG_LOG(logger) << LOGFUNCTION << ", v0 = " << vFalling
|
||||
<< ", alphaRad = " << alphaRad << ", alphaDeg = " << alphaRad * LtsCoordTrans::rad2deg()
|
||||
<< ", timeTapT_sec = " << timeTapT_sec;
|
||||
|
||||
QPointF tap;
|
||||
|
||||
tap.rx() = timeTapT_sec * vFalling * cos(alphaRad) * -1; // *-1 : move left to impact
|
||||
tap.ry() = timeTapT_sec * vFalling * sin(alphaRad);
|
||||
|
||||
#ifdef LTS_DEBUG
|
||||
const double lengthVec = QVector2D(tap).length();
|
||||
const double lengthProduct = timeTapT_sec * vFalling;
|
||||
|
||||
const double quot = fabs(lengthVec / lengthProduct);
|
||||
Q_ASSERT((quot-1.0) < 0.1);
|
||||
#endif
|
||||
|
||||
return tap;
|
||||
}
|
||||
|
||||
QVector<QPointF> generateBallisticCurveUntil(double v0, double betaRad,
|
||||
double max_x_distance, double secPerIntervall,
|
||||
double *deltaXperStep_out, double *secPerIntervall_out)
|
||||
{
|
||||
LOGGER_T logger = getLocalLogger();
|
||||
|
||||
QVector<QPointF> points;
|
||||
|
||||
const double perfectDelta_x = secPerIntervall * v0 * cos(betaRad);
|
||||
const double distancePhase1 = max_x_distance; // similar to Apo
|
||||
|
||||
const int steps = (distancePhase1/perfectDelta_x); // implicit round
|
||||
const double realDelta_x = distancePhase1 / steps;
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION
|
||||
<< ", betaDeg = " << betaRad*LtsCoordTrans::rad2deg()
|
||||
<< ", max_x_distance = " << max_x_distance
|
||||
<< ", perfectDelta_x = " << perfectDelta_x
|
||||
<< ", realDelta_x = " << realDelta_x
|
||||
<< ", steps = " << steps;
|
||||
|
||||
points.reserve(steps+5);
|
||||
for (int curStep=0; curStep <= steps; ++curStep)
|
||||
{
|
||||
double x_val = curStep * realDelta_x;
|
||||
double time = x_val / (v0 * cos(betaRad));
|
||||
double h = v0 * time * sin(betaRad) - G * time * time / 2.0;
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION
|
||||
<< ", x_valx " << x_val
|
||||
<< ", distancePhase1 " << distancePhase1
|
||||
<< ", time " << time
|
||||
<< ", h " << h;
|
||||
|
||||
points.append( QPointF(x_val, h) );
|
||||
};
|
||||
|
||||
// Out-vars:
|
||||
if (deltaXperStep_out)
|
||||
*deltaXperStep_out = realDelta_x;
|
||||
|
||||
if (secPerIntervall_out)
|
||||
*secPerIntervall_out = realDelta_x / (v0 * cos(betaRad));
|
||||
|
||||
INFO_LOG(logger) << LOGFUNCTION
|
||||
<< ", betaDeg = " << betaRad*LtsCoordTrans::rad2deg()
|
||||
<< ", max_x_distance = " << max_x_distance
|
||||
<< ", perfectDelta_x = " << perfectDelta_x
|
||||
<< ", realDelta_x = " << realDelta_x
|
||||
<< ", steps = " << steps
|
||||
<< ", points.size() = " << points.size()
|
||||
<< ", expectedDuration = " << points.size()* (realDelta_x / (v0 * cos(betaRad)))
|
||||
;
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
QPointF calculateTapRelToOrigin(const QPointF &idealHit, const QPointF &tap_relToIdealHit)
|
||||
{
|
||||
return idealHit + tap_relToIdealHit;
|
||||
}
|
||||
|
||||
} // end namespace
|
||||
} // namespace AreaEffectSimulation
|
||||
+222
@@ -0,0 +1,222 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* TrajectoryGenerator for Vulcano
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Norbert Thoden, SLTE, 2024-04-16
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../logger.h"
|
||||
#include <Trajectory/trajectory_export.h>
|
||||
|
||||
#include <LtsCoordTrans/lts_coord_trans.h>
|
||||
#include <LtsUtilsCore/qpoint3d.h>
|
||||
#include <QVector>
|
||||
#include <QVector2D>
|
||||
|
||||
#include <cmath>
|
||||
#include <iomanip> // std::setprecision
|
||||
|
||||
|
||||
/*
|
||||
LTD Laser Target Designator
|
||||
SAL Semi Active Laser
|
||||
TAP Target Acquisition Point
|
||||
OTL Oberserver Target Line
|
||||
GTL Gun Target Line
|
||||
PTL Projectile Target Line
|
||||
PRF Pulse Repitition Frequency
|
||||
|
||||
|
||||
Part 1: Ballistische Flugphase
|
||||
calcBalPhase
|
||||
|
||||
Part 2: Gleitphase
|
||||
calcTapPhase ??
|
||||
|
||||
Part 3: EndAnflugPhase
|
||||
calcSalPhase ???
|
||||
|
||||
unklar:
|
||||
calcTAP
|
||||
calcSalPhase
|
||||
isSalTarget
|
||||
|
||||
Wichtige Notizen:
|
||||
ETB-Bericht Rev. A
|
||||
In der SAL-Endanflugphase beginnt die Zielerfassung mit dem SAL-Sensor.
|
||||
Hierzu wird bei Erreichen des TAP in einer Höhe von ca. 3500m
|
||||
die ballistische Haube des Geschosses abgesprengt, so dass der SAL-Sensor
|
||||
freie Sicht hat.
|
||||
*/
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
/*
|
||||
zwei-dimensionale Betrachtung
|
||||
y
|
||||
| Apogaeum / Scheitelpunkt
|
||||
| |
|
||||
| _
|
||||
| / \ ----- gliding
|
||||
| / ----- gliding, continued until TAP (TargetAcquisitionPoint)
|
||||
| / \
|
||||
| / \
|
||||
|/ beta alpha \
|
||||
---------------------------------------------------------- x
|
||||
^ originator/gun ^ ideal impact / hit
|
||||
*/
|
||||
|
||||
using QPoint3D = LtsUtils::QPoint3D;
|
||||
|
||||
class QPoint4D : public QPoint3D
|
||||
{
|
||||
public:
|
||||
explicit QPoint4D(double xpos=0.0, double ypos=0.0, double zpos=0.0, double time=0.0)
|
||||
: QPoint3D(xpos, ypos, zpos)
|
||||
, m_time(time)
|
||||
{}
|
||||
void setTime(double time ) { m_time = time; }
|
||||
double time() const { return m_time; }
|
||||
private:
|
||||
double m_time;
|
||||
};
|
||||
inline std::ostream& operator<< (std::ostream& stream, const QPoint4D& pos)
|
||||
{
|
||||
stream << "(" << QString::number(pos.x(), 'f', 3)
|
||||
<< ", " << QString::number(pos.y(), 'f', 3)
|
||||
<< ", " << QString::number(pos.z(), 'f', 3)
|
||||
<< ", t=" << QString::number(pos.time(), 'f', 3) << ")";
|
||||
return stream;
|
||||
}
|
||||
|
||||
|
||||
|
||||
using TRAJECTORY_METRIC = QVector<QPoint4D>;
|
||||
|
||||
|
||||
/**
|
||||
* @brief The TrajectoryGeneratorVulcano class provides methods to generate a vulcano flight curve.
|
||||
* All calculations are using metrical coordinates.
|
||||
*/
|
||||
|
||||
//
|
||||
// 2-dim operation:
|
||||
//
|
||||
namespace XY
|
||||
{
|
||||
// speed/v [m/sec]
|
||||
// distance [m]
|
||||
|
||||
TRAJECTORY_EXPORT double calculateBetaRad(double v0, double peakHeightRelToOrigin, QString *reasonOut = nullptr);
|
||||
|
||||
TRAJECTORY_EXPORT QPointF calculateApogaeum(double v0, double betaRad);
|
||||
|
||||
|
||||
TRAJECTORY_EXPORT double calculateApogaeumStrichEval(const QPointF &origin, const QPointF &idealHit,
|
||||
double v0, double betaRad, const QPointF &tap_relToOrigin);
|
||||
|
||||
TRAJECTORY_EXPORT QPointF calculateTapRelToIdealHit(double vFalling, double alphaRad, double timeTapT_sec);
|
||||
TRAJECTORY_EXPORT QPointF calculateTapRelToOrigin(const QPointF &idealHit, const QPointF &tap_relToIdealHit);
|
||||
|
||||
TRAJECTORY_EXPORT QVector<QPointF> generateBallisticCurveUntil(double v0, double betaRad,
|
||||
double max_x_distance, double secPerIntervall,
|
||||
double *deltaXperStep_out = nullptr, double *secPerIntervall_out = nullptr);
|
||||
};
|
||||
|
||||
|
||||
|
||||
class TRAJECTORY_EXPORT TrajectoryGeneratorVulcano
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
// defaults:
|
||||
static inline double defaultTapTimeSec() { return 8; } // sec
|
||||
static inline double defaultv0() { return 870; } // m/sec , 730 ; _870_ ; 1020 , 1100
|
||||
|
||||
|
||||
// Options:
|
||||
enum Option {
|
||||
OptRasterApoTap = 1, // 0000001
|
||||
OptRasterApoTapTime = 2, // 0000010
|
||||
OptIncludeIdealHit = 4, // 0000100
|
||||
OptRasterTapHit = 8, // 0001000
|
||||
OptRasterMetrical = 16, // 0010000
|
||||
// OptFutureUse1 = 32,
|
||||
// OptFutureUse2 = 64,
|
||||
};
|
||||
Q_DECLARE_FLAGS(Options, Option)
|
||||
|
||||
|
||||
//
|
||||
// main:
|
||||
//
|
||||
|
||||
/**
|
||||
* @brief trajectoryVulcano generate vulcano flight curve.
|
||||
* phase 1: ballistic; phase 2: gliding from peak (APO|APO') to TAP; [ phase 3: TAP to ideal hit ]
|
||||
* All calculations are using metrical coordinates.
|
||||
* @param origin metrical coordinates
|
||||
* @param idealHit metrical coordinates
|
||||
* @param v0 Abschussgeschwindigkeit
|
||||
* @param betaRad Abschussgeschwindigkeit
|
||||
* @param vGliding Gleitgeschwindigkeit
|
||||
* @param vFalling Fallgeschwindigkeit
|
||||
* @param alphaRad Fallwinkel
|
||||
* @param tapTime Fallzeit
|
||||
* @param options
|
||||
* @param trajectoryOut metric, relativ to origin , time starts be zero
|
||||
* @return true on success, otherwise false
|
||||
*/
|
||||
static bool trajectoryVulcano(const QPoint3D &origin, const QPoint3D &idealHit,
|
||||
double v0, double betaRad, // Abschussgeschwindigkeit, -winkel
|
||||
double vGliding,
|
||||
double vFalling , double alphaRad, double tapTimeSec, // Fallwinkel, -geschwindigkeit, -zeit
|
||||
TrajectoryGeneratorVulcano::Options options,
|
||||
TRAJECTORY_METRIC &trajectoryOut,
|
||||
QPoint3D &idealTapOut,
|
||||
QString *reasonOut);
|
||||
|
||||
|
||||
//
|
||||
// checks:
|
||||
//
|
||||
static bool checkSimple(const QPoint3D &origin, const QPoint3D &apoAbs, const QPoint3D &idealHit, QString *reasonOut);
|
||||
static bool checkSimple(const QPoint3D &origin, const QPoint3D &apoAbs, const QPoint3D &tabAbs, const QPoint3D &idealHit, QString *reasonOut);
|
||||
|
||||
|
||||
//
|
||||
// helper:
|
||||
//
|
||||
|
||||
// get speed for every seperate phase
|
||||
static void getSpeedPerPhase(double v0_in, double betaRad,
|
||||
double &vGlidingOut, double &vFallingOut);
|
||||
|
||||
|
||||
static void rasterLineByDistance(const QPoint3D &p0, const QPoint3D &p1,
|
||||
double desiredDelta_xy,
|
||||
TRAJECTORY_METRIC &trajectoryOut, double *realDelta_xy_out);
|
||||
|
||||
static void rasterLineByTime(const QPoint3D &p0, const QPoint3D &p1,
|
||||
double timeOffsetSec, double speedMeterPerSec, double desiredDeltaTime,
|
||||
TRAJECTORY_METRIC &trajectoryOut, double *realDeltaTime_out);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
protected:
|
||||
// empty
|
||||
};
|
||||
|
||||
Q_DECLARE_OPERATORS_FOR_FLAGS(TrajectoryGeneratorVulcano::Options)
|
||||
|
||||
|
||||
} // namespace AreaEffectSimulation
|
||||
@@ -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
|
||||
@@ -0,0 +1,74 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* Trajectory Helper
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Norbert Thoden, SLTE, 2023-11-08
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AreaEffectSimulation/aes_export.h>
|
||||
#include <Trajectory/trajectory_export.h>
|
||||
|
||||
// Forward
|
||||
namespace LtsUtils {
|
||||
class QPoint3D;
|
||||
}
|
||||
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
class TRAJECTORY_EXPORT TrajectoryHelper
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief TrajectoryHelper::flightDurationSec
|
||||
* @param posStart start of the curve
|
||||
* @param posEnd end of the curve
|
||||
* @param peakHeightAbs absolute peak of the curve value
|
||||
* @param betaRadiantOut start angle
|
||||
* @param vOut firing speed
|
||||
* @param reasonOut error information
|
||||
* @return Flugzeit in Sekunden (Rueckgabewert -1 bei Eingabe zu kleiner Hoehe / -2 bei Hoehenunterschieden)
|
||||
*/
|
||||
static double flightDurationSec(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double peakHeightAbs,
|
||||
double *betaRadiantOut = nullptr, double *vOut = nullptr,
|
||||
QString *reasonOut = nullptr);
|
||||
|
||||
enum class TrajectoryMode
|
||||
{
|
||||
TrajectoryModeLow = 0, // flat
|
||||
TrajectoryModeHigh
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief TrajectoryHelper::peakHeightAbs
|
||||
* @param posStart start of the curve
|
||||
* @param posEnd end of the curve
|
||||
* @param timeSec flightDuration in seconds
|
||||
* @param trajMode LOW (0) or HIGH (1) curve
|
||||
* @param betaRadiantOut start angle
|
||||
* @param vOut firing speed
|
||||
* @param reasonOut error information
|
||||
* @return absolute peak
|
||||
*/
|
||||
static double peakHeightAbs(const LtsUtils::QPoint3D &posStart, const LtsUtils::QPoint3D &posEnd, double timeSec,
|
||||
double *betaRadiantOut = nullptr, double *vOut = nullptr,
|
||||
QString *reasonOut = nullptr);
|
||||
|
||||
/**
|
||||
* @brief calculate realStartPoint based on apogaeum and endpos (more of less mirroring)
|
||||
* @param apogaeum of ideal parabel
|
||||
* @param posEnd impact position
|
||||
* @return mirrored point (z values of posEnd is taken)
|
||||
*/
|
||||
static LtsUtils::QPoint3D realStartPoint(const LtsUtils::QPoint3D &apogaeum, const LtsUtils::QPoint3D &endPos);
|
||||
|
||||
};
|
||||
|
||||
|
||||
} // namespace AreaEffectSimulation
|
||||
@@ -0,0 +1,51 @@
|
||||
/**
|
||||
* Commom base types * $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Norbert Thoden, ESVG, 2026-05-19
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QVector>
|
||||
#include <LtsUtilsCore/qpoint3d.h>
|
||||
#include <Trajectory/trajectory_export.h>
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief The TrajectoryPoint class was introduced for ADLER / SAA
|
||||
* initially the positions are metrical and relative.
|
||||
*/
|
||||
class TrajectoryPoint
|
||||
{
|
||||
public:
|
||||
TrajectoryPoint() = default;
|
||||
|
||||
TrajectoryPoint(double x, double y, double z, double _time = -1)
|
||||
: position(x,y,z), time (_time)
|
||||
{}
|
||||
|
||||
TrajectoryPoint(const LtsUtils::QPoint3D &_position, double _time)
|
||||
: position(_position), time (_time)
|
||||
{}
|
||||
|
||||
LtsUtils::QPoint3D position; // x,y are relative, z is above sea level; both metrical
|
||||
double time {-1.0}; // relative
|
||||
};
|
||||
|
||||
Q_DECL_CONSTEXPR inline bool operator==(const TrajectoryPoint &p1, const TrajectoryPoint &p2)
|
||||
{
|
||||
return p1.position == p2.position && p1.time == p2.time;
|
||||
}
|
||||
Q_DECL_CONSTEXPR inline bool operator!=(const TrajectoryPoint &p1, const TrajectoryPoint &p2)
|
||||
{
|
||||
return p1.position != p2.position || p1.time != p2.time;
|
||||
}
|
||||
|
||||
} // namespace AreaEffectSimulation
|
||||
|
||||
|
||||
// at global namespace:
|
||||
TRAJECTORY_EXPORT std::ostream &operator<< (std::ostream &stream, const AreaEffectSimulation::TrajectoryPoint &value);
|
||||
@@ -0,0 +1,86 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* TrajectoryTransform
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Gerlinde Rudolphus, ESZS, 2026
|
||||
*/
|
||||
|
||||
#include "trajectory_transform.h"
|
||||
|
||||
// Algorithmus: Zeitgewichtete Bézier-Korrektur (empfohlen)
|
||||
// Kerngedanke
|
||||
// Die ursprüngliche Trajektorie bleibt die Basis
|
||||
// Eine glatte Korrekturkurve wird addiert
|
||||
// Die Korrektur ist:
|
||||
// 0 am Start
|
||||
// maximal am Ziel
|
||||
// zeitlich C¹-stetig
|
||||
// Formel
|
||||
// Für jeden Punkt:
|
||||
// Pneu(t) = Palt(t) + w(t) · ΔZ
|
||||
// mit
|
||||
// ΔZ = Zneu − Zalt
|
||||
// w(t) = 3s² − 2s³ (smoothstep)
|
||||
// s = (t − t0) / (tEnd − t0)
|
||||
// Dadurch:
|
||||
// Start bleibt exakt gleich
|
||||
// Zielpunkt wird exakt erreicht
|
||||
// Bewegung bleibt glatt
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
namespace TrajectoryTransform
|
||||
{
|
||||
|
||||
|
||||
double smoothStep(double x)
|
||||
{
|
||||
return x * x * (3.0 - 2.0 * x);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T clamp(T v, T lo, T hi) {
|
||||
return (v < lo) ? lo : (v > hi) ? hi : v; }
|
||||
|
||||
|
||||
|
||||
QVector<TrajectoryPoint> transformToNewTarget(
|
||||
const QVector<TrajectoryPoint>& originalTrajMetrical,
|
||||
const LtsUtils::QPoint3D& newTargetMetrical)
|
||||
{
|
||||
if (originalTrajMetrical.size() < 2)
|
||||
return originalTrajMetrical;
|
||||
|
||||
QVector<TrajectoryPoint> result = originalTrajMetrical;
|
||||
|
||||
const LtsUtils::QPoint3D &oldTarget = originalTrajMetrical.last().position;
|
||||
const LtsUtils::QPoint3D &delta = newTargetMetrical - oldTarget;
|
||||
|
||||
const double t0 = originalTrajMetrical.first().time;
|
||||
const double tEnd = originalTrajMetrical.last().time;
|
||||
const double dt = tEnd - t0;
|
||||
|
||||
for (int i = 0; i < result.size(); ++i)
|
||||
{
|
||||
double s = (result[i].time - t0) / dt;
|
||||
s = clamp(s, 0.0, 1.0);
|
||||
|
||||
double w = smoothStep(s);
|
||||
result[i].position += delta * w;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace TrajectoryTransform
|
||||
} // namespace AreaEffectSimulation
|
||||
|
||||
|
||||
std::ostream &operator<< (std::ostream &stream, const AreaEffectSimulation::TrajectoryPoint &value)
|
||||
{
|
||||
stream << "TrajectoryPoint(" << "position=" << value.position << "; time=" << value.time;
|
||||
return stream;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
|
||||
#include "trajectory_point.h"
|
||||
#include <Trajectory/trajectory_export.h>
|
||||
|
||||
#include <QVector>
|
||||
#include <LtsUtilsCore/qpoint3d.h>
|
||||
|
||||
namespace AreaEffectSimulation
|
||||
{
|
||||
|
||||
// prefer namespace instead of class with static member:
|
||||
// https://stackoverflow.com/questions/1434937/namespace-functions-versus-static-methods-on-a-class
|
||||
// https://stackoverflow.com/questions/1434937/namespace-functions-versus-static-methods-on-a-class#:~:text=You%20should%20use%20a%20namespace,can%20using%20a%20namespace%20member
|
||||
|
||||
|
||||
namespace TrajectoryTransform
|
||||
{
|
||||
TRAJECTORY_EXPORT QVector<TrajectoryPoint> transformToNewTarget(const QVector<TrajectoryPoint>& originalTrajMetrical,
|
||||
const LtsUtils::QPoint3D& newTargetMetrical);
|
||||
}
|
||||
|
||||
} // namespace AreaEffectSimulation
|
||||
Reference in new issue
Block a user