// ************************************************************************** // // (C) Copyright 2009 Swiss SIMTEC AG // // ************************************************************************** // // $HeadURL$ // $LastChangedBy$ // $LastChangedDate$ // $LastChangedRevision$ // // ************************************************************************** // manual class headers #include "GeoPairingImplAirTargetTracker.hpp" // from cpp_includes #include "../logger.h" #include #include namespace N_GeoPairingImpl { // --------------------------------------------------------------- // Constructor // --------------------------------------------------------------- GeoPairingImplAirTargetTracker::GeoPairingImplAirTargetTracker(std::vector *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