344 lines
13 KiB
C++
Executable File
344 lines
13 KiB
C++
Executable File
|
|
// **************************************************************************
|
|
//
|
|
// (C) Copyright 2009 Swiss SIMTEC AG
|
|
//
|
|
// **************************************************************************
|
|
//
|
|
// $HeadURL$
|
|
// $LastChangedBy$
|
|
// $LastChangedDate$
|
|
// $LastChangedRevision$
|
|
//
|
|
// **************************************************************************
|
|
|
|
// manual class headers
|
|
#include "GeoPairingImplAirTargetTracker.hpp"
|
|
|
|
// from cpp_includes
|
|
#include "../logger.h"
|
|
#include <GBL_ConfigSystem/geoconfig.h>
|
|
#include <GBL_Types/GeoCoord.hpp>
|
|
|
|
namespace N_GeoPairingImpl
|
|
{
|
|
|
|
// ---------------------------------------------------------------
|
|
// Constructor
|
|
// ---------------------------------------------------------------
|
|
GeoPairingImplAirTargetTracker::GeoPairingImplAirTargetTracker(std::vector<GeoPairingImplAirTarget *> *a_listTargets)
|
|
: m_List(a_listTargets)
|
|
, m_logger(CesGeoPairing::getLogger("Plugin.GeoPairing.GeoPairingImplAirTargetTracker"))
|
|
{
|
|
INFO_LOG(m_logger) << "Construct Tracker for " << a_listTargets->size()
|
|
<< " air targets";
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// sets the current position of the tracker
|
|
// ---------------------------------------------------------------
|
|
void GeoPairingImplAirTargetTracker::setPosition(Float64 a_positionLat,
|
|
Float64 a_positionLon, Float64 a_positionAlt)
|
|
{
|
|
N_Math::GeoCoord l_PositionUtm;
|
|
l_PositionUtm.setLlPositionDegree(a_positionLat, a_positionLon, a_positionAlt);
|
|
|
|
N_Types::Coord3 l_Position(l_PositionUtm.utmEasting(), l_PositionUtm.utmNorthing(),
|
|
l_PositionUtm.height());
|
|
INFO_LOG(m_logger) << "Tracker position : " << l_Position;
|
|
m_Position = l_Position;
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// track a air target
|
|
// ---------------------------------------------------------------
|
|
bool GeoPairingImplAirTargetTracker::track(Float64 a_azimuth,
|
|
Float64 a_elevation, int &a_playerId, UInt64 &a_impactTime)
|
|
{
|
|
if (m_List->size() == 0)
|
|
{
|
|
INFO_LOG(m_logger) << "No air targets to track";
|
|
return false;
|
|
}
|
|
|
|
// prepare window values
|
|
// QString FileName = CentralEffectSimulationGeoPairing::Settings::cesGeopairingIniPath() + QLatin1Char('/') + CentralEffectSimulationGeoPairing::Settings::cesGeopairingIniFile();
|
|
N_ConfigSystem::GeoConfig *l_config = N_ConfigSystem::GeoConfig::getInstance();
|
|
|
|
N_Types::Radian64 l_AzimuthWindow;
|
|
l_AzimuthWindow.setDegree(l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("AzimuthWindow"), 1.5));
|
|
N_Types::Radian64 l_ElevationWindow;
|
|
l_ElevationWindow.setDegree(l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("ElevationWindow"), 1.5));
|
|
|
|
Float64 l_FlightSpeed = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("FlightSpeed"), 600.0);
|
|
Float64 l_TimeRange = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("TimeRange"), 17);
|
|
|
|
N_Types::Angle3 l_sensorAngle;
|
|
l_sensorAngle.m_H.setDegree(-a_azimuth);
|
|
l_sensorAngle.m_P.setDegree(a_elevation);
|
|
|
|
INFO_LOG(m_logger) << "Tracking angle : " << l_sensorAngle;
|
|
|
|
N_Types::Radian64 l_ClosestAzimuth;
|
|
N_Types::Radian64 l_ClosestElevation;
|
|
SInt32 l_ClosestPlayerId = 0;
|
|
UInt64 l_ClosestPlayerImpactTime = 0;
|
|
Float64 l_ClosestPlayerDistance = 0;
|
|
|
|
INFO_LOG(m_logger) << "Iterating through air targets, Count : "
|
|
<< m_List->size();
|
|
for (const auto l_Target : *m_List)
|
|
{
|
|
N_Types::Coord3 l_targetPos = l_Target->m_Position;
|
|
// N_Types::Vec3 l_TargetDir = l_Target->m_Direction;
|
|
// Float64 l_TargetSpeed = l_Target->m_Speed;
|
|
UInt64 l_TargetPosTime = l_Target->m_PositionTime;
|
|
|
|
INFO_LOG(m_logger) << "Position of air target "
|
|
<< l_Target->m_PlayerID << " @ "
|
|
<< l_TargetPosTime << " s : "
|
|
<< l_targetPos;
|
|
|
|
SInt32 l_TimeDiff = m_Time - l_TargetPosTime;
|
|
|
|
/* if (l_TimeDiff != 0)
|
|
{
|
|
N_Types::Vec3 l_PosDiff = l_TargetDir * (l_TargetSpeed * l_TimeDiff);
|
|
INFO_LOG(m_logger) << "l_PosDiff = " << l_PosDiff;
|
|
|
|
l_targetPos.m_X += l_PosDiff.m_X;
|
|
l_targetPos.m_Y += l_PosDiff.m_Y;
|
|
l_targetPos.m_Z += l_PosDiff.m_Z;
|
|
}
|
|
*/
|
|
// instead of position interpolation we checkout the time difference
|
|
if (abs(l_TimeDiff) > 2)
|
|
{
|
|
WARNING_LOG(m_logger) << "l_TimeDiff differs by more that 2 s ! m_Time = "
|
|
<< m_Time << " l_TargetPosTime = " << l_TargetPosTime
|
|
;
|
|
}
|
|
|
|
N_Types::Coord3 l_LineVector = l_targetPos - m_Position;
|
|
N_Types::Polar3 l_LinePolar = l_LineVector.getPolar3();
|
|
N_Types::Angle3 l_LineAngle(l_LinePolar.m_Azimuth, l_LinePolar.m_Elevation, 0);
|
|
|
|
INFO_LOG(m_logger) << "Line of sight angle to air target : " << l_LineAngle;
|
|
|
|
N_Types::Radian64 l_AzimuthDiff = fabs(l_sensorAngle.m_H - l_LineAngle.m_H);
|
|
DEBUG_LOG(m_logger) << "l_AzimuthDiff=" << l_AzimuthDiff
|
|
<< " l_sensorAngle.m_H=" << l_sensorAngle.m_H
|
|
<< " l_LineAngle.m_H=" << l_LineAngle.m_H;
|
|
N_Types::Radian64 l_ElevationDiff = fabs(l_sensorAngle.m_P - l_LineAngle.m_P);
|
|
|
|
INFO_LOG(m_logger) << "l_AzimuthDiff : "
|
|
<< l_AzimuthDiff.getDegree() << " [Grad] "
|
|
<< l_AzimuthDiff << " [rad] ";
|
|
INFO_LOG(m_logger) << "l_AzimuthWindow : "
|
|
<< l_AzimuthWindow.getDegree() << " [Grad] "
|
|
<< l_AzimuthWindow << " [rad] ";
|
|
INFO_LOG(m_logger) << "l_ElevationDiff : "
|
|
<< l_ElevationDiff.getDegree() << " [Grad] "
|
|
<< l_ElevationDiff << " [rad] ";
|
|
INFO_LOG(m_logger) << "l_ElevationWindow : "
|
|
<< l_ElevationWindow.getDegree() << " [Grad] "
|
|
<< l_ElevationWindow << " [rad] ";
|
|
|
|
if (fabs(l_AzimuthDiff) > l_AzimuthWindow || fabs(l_ElevationDiff) > l_ElevationWindow)
|
|
{
|
|
INFO_LOG(m_logger) << "-> air target " << l_Target->m_PlayerID
|
|
<< " is NOT in window"
|
|
<< " fabs(l_AzimuthDiff)=" << fabs(l_AzimuthDiff)
|
|
<< " l_AzimuthWindow=" << l_AzimuthWindow
|
|
<< " fabs(l_ElevationDiff)=" << fabs(l_ElevationDiff)
|
|
<< " l_ElevationWindow=" << l_ElevationWindow;
|
|
continue;
|
|
}
|
|
|
|
Float64 l_TargetDistance = l_LineVector.magnitude();
|
|
Float64 l_TimeOfFlight = l_TargetDistance / l_FlightSpeed;
|
|
|
|
if (l_TimeOfFlight > l_TimeRange)
|
|
{
|
|
INFO_LOG(m_logger) << "-> Target flight time " << l_TimeOfFlight
|
|
<< " exceeding max time of " << l_TimeRange
|
|
;
|
|
continue;
|
|
}
|
|
|
|
if (l_ClosestPlayerId == 0) // as long as no reference
|
|
{
|
|
INFO_LOG(m_logger) << "=> air target " << l_Target->m_PlayerID
|
|
<< " is in window";
|
|
l_ClosestAzimuth = l_AzimuthDiff;
|
|
l_ClosestElevation = l_ElevationDiff;
|
|
l_ClosestPlayerId = l_Target->m_PlayerID;
|
|
l_ClosestPlayerImpactTime = m_Time + l_TimeOfFlight;
|
|
l_ClosestPlayerDistance = l_TargetDistance;
|
|
}
|
|
else // compare to saved values
|
|
{
|
|
if (fabs(l_AzimuthDiff) < l_ClosestAzimuth && fabs(l_ElevationDiff) < l_ClosestElevation)
|
|
{
|
|
if (l_TargetDistance < l_ClosestPlayerDistance)
|
|
{
|
|
INFO_LOG(m_logger) << "=> air target " << l_Target->m_PlayerID
|
|
<< " is in window";
|
|
l_ClosestAzimuth = l_AzimuthDiff;
|
|
l_ClosestElevation = l_ElevationDiff;
|
|
l_ClosestPlayerId = l_Target->m_PlayerID;
|
|
l_ClosestPlayerImpactTime = m_Time + l_TimeOfFlight;
|
|
l_ClosestPlayerDistance = l_TargetDistance;
|
|
}
|
|
else
|
|
{
|
|
INFO_LOG(m_logger) << "=> air target " << l_Target->m_PlayerID
|
|
<< " is in window, but ID " << l_ClosestPlayerId
|
|
<< " is closer";
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (l_ClosestPlayerId == 0)
|
|
{
|
|
INFO_LOG(m_logger) << "No air target was within window...";
|
|
return false;
|
|
}
|
|
|
|
a_playerId = l_ClosestPlayerId;
|
|
a_impactTime = l_ClosestPlayerImpactTime;
|
|
|
|
INFO_LOG(m_logger) << "a_impactTime = " << a_impactTime;
|
|
INFO_LOG(m_logger) << "l_TimeOfFlight = " << a_impactTime - m_Time;
|
|
INFO_LOG(m_logger) << "Closest air target in window: " << a_playerId;
|
|
|
|
return true;
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// calculates the probability of a target beeing hit according
|
|
// to given conditions
|
|
// ---------------------------------------------------------------
|
|
bool GeoPairingImplAirTargetTracker::hitCheck(SInt32 a_targetId,
|
|
bool a_modeAuto, bool a_changeOverPoint)
|
|
{
|
|
if (m_List->size() == 0)
|
|
{
|
|
ERROR_LOG(m_logger) << "No air targets to track, strange at this point!";
|
|
return false;
|
|
}
|
|
|
|
INFO_LOG(m_logger) << "Searching for air target, Count : " << m_List->size();
|
|
GeoPairingImplAirTarget *l_HitTarget = nullptr;
|
|
|
|
for (const auto l_Target : *m_List)
|
|
{
|
|
if (l_Target->m_PlayerID == a_targetId)
|
|
{
|
|
l_HitTarget = l_Target;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (l_HitTarget == nullptr)
|
|
{
|
|
ERROR_LOG(m_logger) << "No air targets found with ID " << a_targetId;
|
|
return false;
|
|
}
|
|
|
|
N_Types::Coord3 l_targetPos = l_HitTarget->m_Position;
|
|
N_Types::Vec3 l_TargetDir = l_HitTarget->m_Direction;
|
|
N_Types::Vec3 l_Line = l_targetPos - m_Position;
|
|
|
|
Float64 l_OriginalDistance = l_Line.magnitude();
|
|
INFO_LOG(m_logger) << "l_OriginalDistance = " << l_OriginalDistance;
|
|
|
|
l_Line = (l_TargetDir + l_targetPos) - m_Position;
|
|
Float64 l_DirectionDistance = l_Line.magnitude();
|
|
INFO_LOG(m_logger) << "l_DirectionDistance = " << l_DirectionDistance;
|
|
|
|
|
|
const bool l_FliesTowardsMe = l_DirectionDistance < l_OriginalDistance;
|
|
|
|
N_ConfigSystem::GeoConfig *l_config = N_ConfigSystem::GeoConfig::getInstance();
|
|
|
|
Float64 l_ModeAutoProbability = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("ModeAutoProbability"), 1.0);
|
|
Float64 l_ModeManuProbability = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("ModeManuProbability"), 1.0);
|
|
Float64 l_WrongCOPFactor = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("WrongCOPFactor"), 0.5);
|
|
Float64 l_IRJammerFactor = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("IRJammerFactor"), 0.5);
|
|
|
|
Float64 l_DistanceRange = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("DistRange"), 5200.0);
|
|
Float64 l_FlightHeightMax = l_config->getFloat(QStringLiteral("IGLA"), QStringLiteral("FlightHeightMax"), 3500.0);
|
|
|
|
if (l_OriginalDistance > l_DistanceRange)
|
|
{
|
|
INFO_LOG(m_logger) << "--> Target " << a_targetId << " distance "
|
|
<< l_OriginalDistance << " beyond max distance of "
|
|
<< l_DistanceRange;
|
|
return false;
|
|
}
|
|
if (l_targetPos.m_Z > l_FlightHeightMax)
|
|
{
|
|
INFO_LOG(m_logger) << "--> Target " << a_targetId << " flying on height = "
|
|
<< l_targetPos.m_Z << ", max Height for IGLA is "
|
|
<< l_FlightHeightMax;
|
|
return false;
|
|
}
|
|
|
|
Float64 l_Probability = l_ModeAutoProbability;
|
|
if (!a_modeAuto)
|
|
l_Probability = l_ModeManuProbability;
|
|
INFO_LOG(m_logger) << "Basic Probability = " << l_Probability
|
|
<< (a_modeAuto ? " (auto)" : " (manual)");
|
|
|
|
/* if (l_FliesTowardsMe == a_changeOverPoint)
|
|
{
|
|
INFO_LOG(m_logger) << "Wrong COP Factor applied";
|
|
l_Probability *= l_WrongCOPFactor;
|
|
}
|
|
*/
|
|
|
|
if (a_changeOverPoint)
|
|
INFO_LOG(m_logger) << "Depart Button pressed";
|
|
else
|
|
INFO_LOG(m_logger) << "Depart Button released";
|
|
|
|
INFO_LOG(m_logger) << "l_FliesTowardsMe = " << l_FliesTowardsMe;
|
|
// Wenn der Depart Button nicht gedrueckt ist, und das Ziel vom Schuetzen wegfliegt.
|
|
if (!l_FliesTowardsMe && !a_changeOverPoint)
|
|
{
|
|
INFO_LOG(m_logger) << "Wrong COP Factor applied";
|
|
l_Probability *= l_WrongCOPFactor;
|
|
}
|
|
|
|
if (l_HitTarget->m_IRJammer)
|
|
{
|
|
INFO_LOG(m_logger) << "IR Jammer Factor applied";
|
|
l_Probability *= l_IRJammerFactor;
|
|
}
|
|
|
|
Float64 l_Random = (rand() % 100) / 100.0;
|
|
if (l_Random > l_Probability)
|
|
{
|
|
INFO_LOG(m_logger) << "--> Target " << a_targetId << " not hit, Probability = "
|
|
<< l_Probability << ", Randomizer = " << l_Random;
|
|
return false;
|
|
}
|
|
|
|
INFO_LOG(m_logger) << "==> Target " << a_targetId << " hit, Probability = "
|
|
<< l_Probability << ", Randomizer = " << l_Random;
|
|
return true;
|
|
}
|
|
|
|
// ---------------------------------------------------------------
|
|
// sets the time of the tracking moment
|
|
// ---------------------------------------------------------------
|
|
void GeoPairingImplAirTargetTracker::setTime(UInt64 a_time)
|
|
{
|
|
m_Time = a_time;
|
|
INFO_LOG(m_logger) << "Set new tracker reference time : " << m_Time;
|
|
}
|
|
|
|
} // namespace N_GeoPairingImpl
|