734 lines
26 KiB
C++
Executable File
734 lines
26 KiB
C++
Executable File
/**
|
|
* Controller ShootbackAreas (L400)
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by N. Thoden, NLE, 06.04.2020
|
|
*/
|
|
|
|
#include "shootback_controller.h"
|
|
#include "object_shootback_area.h"
|
|
#include "object_shootback_target.h"
|
|
#include "object_target_area_assignment.h"
|
|
#include "main_handler.h"
|
|
|
|
#include <LtsUtilsCore/outdated_position.h> // isPositionTimeOutdated
|
|
#include <LtsCoordTrans/lts_coord_trans.h>
|
|
|
|
#include <ElevationMap/elevation_map.h>
|
|
#include <ElevationMap/line_of_sight_calculation.h>
|
|
|
|
#include <QDebug>
|
|
#include <QMapIterator>
|
|
|
|
//------------------------------------------------------------------------------
|
|
namespace TargetManagement
|
|
{
|
|
|
|
// which data can change:
|
|
// ~static data
|
|
// - onShootbackArea
|
|
// - targetAreaAssignment
|
|
|
|
// ~dynamic data
|
|
// - participantPosition
|
|
// - targetPosition (moving target)
|
|
|
|
|
|
// Step 1: Which participants are in an area?
|
|
// onParticipantPositionChanged, onShootbackAreaChanged1, onShootbackAreaChanged2
|
|
// -> updateInternal_participantPerArea (better updateParticipantsPerAreaCont!?)
|
|
|
|
// Step 2: updateInternalLos -> updateInternalLosSingleAssignment
|
|
|
|
// Step 3: syncExternal -> transfer data to DDS
|
|
|
|
//
|
|
// some helper ease the logging of some data
|
|
//
|
|
QString toString(const QSet<int> &cont)
|
|
{
|
|
QString ret = QString(QStringLiteral("QSet<int>(%1)")).arg(cont.size());
|
|
for (auto val : cont)
|
|
ret += QString(QStringLiteral("(%1)")).arg(val);
|
|
return ret;
|
|
}
|
|
|
|
QString toString(const std::vector<int> &cont)
|
|
{
|
|
QString ret = QString(QStringLiteral("std::vector<int>(%1)")).arg(cont.size());
|
|
for (auto val : cont)
|
|
ret += QString(QStringLiteral("(%1)")).arg(val);
|
|
return ret;
|
|
}
|
|
|
|
|
|
//
|
|
// class ShootbackController
|
|
//
|
|
ShootbackController::ShootbackController(MainHandler *mainHandler)
|
|
: BaseClass(mainHandler)
|
|
, m_mainHandler(mainHandler)
|
|
, m_elevationMap(nullptr)
|
|
, m_losCalculation(nullptr)
|
|
, m_logger(getLogger("ShootbackController"))
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
ShootbackController::~ShootbackController()
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
|
|
// disconnect external "signals"..
|
|
disconnect(m_mainHandler->dds()->dtcPpm().get());
|
|
disconnect(m_handlerShootbackArea.get());
|
|
|
|
// the assignments..
|
|
disconnect(m_handlerTargetAreaAssignment.get());
|
|
|
|
delete m_losCalculation;
|
|
m_losCalculation = nullptr;
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
bool ShootbackController::initLOS(SituationMap::ElevationMap *elevationMap)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START, elevationMap = " << elevationMap
|
|
<< " isValid = " << elevationMap->isValid();
|
|
|
|
if (!elevationMap->isValid())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " elevationMap not valid -> return";
|
|
return false;
|
|
}
|
|
|
|
delete m_losCalculation;
|
|
m_elevationMap = elevationMap;
|
|
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " m_elevationMap.isValid() =" << m_elevationMap->isValid();
|
|
m_losCalculation = new SituationMap::LineOfSightCalculation(m_elevationMap);
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
return true;
|
|
}
|
|
|
|
bool ShootbackController::initDds(std::shared_ptr<HandlerShootbackArea> handlerArea,
|
|
std::shared_ptr<HandlerShootbackTarget> handlerTarget,
|
|
std::shared_ptr<HandlerTargetAreaAssignment> handlerTargetAreaAssignment)
|
|
{
|
|
m_handlerShootbackArea = handlerArea;
|
|
m_handlerShootbackTarget = handlerTarget;
|
|
m_handlerTargetAreaAssignment = handlerTargetAreaAssignment;
|
|
|
|
// ShootbackArea
|
|
connect(m_handlerShootbackArea.get(), &HandlerShootbackArea::objectChanged,
|
|
this, &ShootbackController::onShootbackAreaChanged1, Qt::UniqueConnection);
|
|
|
|
m_handlerShootbackArea->objectShootbackAreas([this](const TargetManagement::ObjectShootbackArea & data)
|
|
{
|
|
onShootbackAreaChanged2(data);
|
|
});
|
|
|
|
// ShootbackTarget
|
|
connect(m_handlerShootbackTarget.get(), &HandlerShootbackTarget::objectChanged,
|
|
this, &ShootbackController::onShootbackTargetChanged, Qt::UniqueConnection);
|
|
|
|
// TargetAreaAssignment
|
|
connect(m_handlerTargetAreaAssignment.get(), &HandlerTargetAreaAssignment::objectChanged,
|
|
this, &ShootbackController::onTargetAreaAssignmentChanged, Qt::UniqueConnection);
|
|
|
|
// finally:
|
|
|
|
// PPM:
|
|
connect(m_mainHandler->dds()->dtcPpm().get(), &ParticipantManagement::Interface::Dtc::participantPositionChanged,
|
|
this, &ShootbackController::onParticipantPositionChanged, Qt::UniqueConnection);
|
|
|
|
return true;
|
|
}
|
|
|
|
void ShootbackController::onShootbackAreaChanged1(qint32 areaId, bool removed)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << " shootbackAreaId=" << areaId << " removed=" << removed;
|
|
|
|
if (removed)
|
|
{
|
|
// handle removed area:
|
|
if (m_participantsPerArea.contains(areaId))
|
|
{
|
|
m_participantsPerArea.remove(areaId);
|
|
updateInternalLos({}, {}, {});
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const QHash<int, ObjectShootbackArea *> &areaMap = m_handlerShootbackArea->getObjectMap();
|
|
auto it = areaMap.find(areaId);
|
|
if (it == areaMap.end())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " obj changes, but connot be found" << " shootbackAreaId=" << areaId;
|
|
return;
|
|
}
|
|
onShootbackAreaChanged2(*(it.value()));
|
|
updateInternalLos({}, {areaId}, {});
|
|
}
|
|
|
|
syncExternal();
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << " shootbackAreaId=" << areaId << " removed=" << removed;
|
|
}
|
|
|
|
void ShootbackController::onShootbackAreaChanged2(const TargetManagement::ObjectShootbackArea &area)
|
|
{
|
|
const int areaId = area.getId();
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << " shootbackAreaId=" << areaId;
|
|
|
|
// handle new/modified area and check all participants:
|
|
const QMap<qint32, ParticipantManagement::ParticipantPosition> &participantPositions =
|
|
m_mainHandler->dds()->dtcPpm()->participantPositions();
|
|
for (const ParticipantManagement::ParticipantPosition &posData : participantPositions)
|
|
{
|
|
// ToDo: posData relevant???
|
|
updateInternal_participantPerArea(area, posData);
|
|
} // for
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << " shootbackAreaId=" << areaId;
|
|
}
|
|
|
|
void ShootbackController::onShootbackTargetChanged(qint32 targetId, bool removed)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << ", targetId=" << targetId << ", removed=" << removed;
|
|
|
|
// Note:
|
|
// updateInternalLos is not required if (removed == true)
|
|
// syncExternal depends on m_visibleParticipantsPerAssignment (via visibleParticipantsPerTargetParticipantId)
|
|
|
|
if (removed)
|
|
{
|
|
// Due to the foreignKey the according assignments should be removed before
|
|
// to nothing:
|
|
return;
|
|
}
|
|
else
|
|
updateInternalLos({}, {}, {targetId});
|
|
syncExternal();
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << " targetId=" << targetId << " removed=" << removed;
|
|
}
|
|
|
|
void ShootbackController::onTargetAreaAssignmentChanged(qint32 assignmentId, bool removed)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << " assignmentId=" << assignmentId << " removed=" << removed;
|
|
|
|
// Note:
|
|
// updateInternalLos is not required on removed == true
|
|
// syncExternal depends on m_visibleParticipantsPerAssignment (via visibleParticipantsPerTargetParticipantId)
|
|
|
|
if (removed)
|
|
m_visibleParticipantsPerAssignment.remove(assignmentId);
|
|
else
|
|
updateInternalLos({assignmentId}, {}, {});
|
|
syncExternal();
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << " assignmentId=" << assignmentId << " removed=" << removed;
|
|
}
|
|
|
|
void ShootbackController::onParticipantPositionChanged(const ParticipantManagement::ParticipantPosition &posData, DataModification modType)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " participantId changed, participantId=" << posData << " mod=" << modType;
|
|
|
|
if (!participantPositionTopicIsValid(posData))
|
|
{
|
|
// DEBUG_LOG(m_logger) << LOGFUNCTION << " NOT VALID, participantId=" << posData.participantId();
|
|
return;
|
|
}
|
|
|
|
const int participantId = posData.participantId();
|
|
|
|
bool bIsTarget = false;
|
|
if (isTarget(participantId, bIsTarget) && bIsTarget)
|
|
{
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " TARGET, participantId=" << participantId;
|
|
|
|
ObjectShootbackTarget *osbTarget = m_handlerShootbackTarget->dataForTargetParticipantId(participantId);
|
|
if (!osbTarget)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " ObjectShootbackTarget not found for participantId=" << participantId;
|
|
return;
|
|
}
|
|
osbTarget->updateVolatileData(posData.pos());
|
|
}
|
|
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " participantId=" << participantId << ", isTarget=" << bIsTarget;
|
|
|
|
if ((modType == DataModification::eAdded) || (modType == DataModification::eModified))
|
|
{
|
|
// update
|
|
const QHash<int, ObjectShootbackArea *> &objMap = m_handlerShootbackArea->getObjectMap();
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " objMap.size" << objMap.size();
|
|
|
|
QMapIterator< int, QSet<int>> it(m_participantsPerArea);
|
|
while (it.hasNext())
|
|
{
|
|
it.next();
|
|
DEBUG_LOG(m_logger) << " areaId = " << it.key() << "-> partIdRef VORHERXX" << toString(it.value());
|
|
}
|
|
|
|
for (const auto &area : qAsConst(objMap))
|
|
{
|
|
if (area)
|
|
updateInternal_participantPerArea(*area, posData);
|
|
} // for
|
|
|
|
{
|
|
QMapIterator<int, QSet<int>> it_inner(m_participantsPerArea);
|
|
while (it_inner.hasNext())
|
|
{
|
|
it_inner.next();
|
|
DEBUG_LOG(m_logger) << " areaId = " << it_inner.key() << "-> partIdRef NACHHERXX" << toString(it_inner.value());
|
|
}
|
|
}
|
|
|
|
updateInternalLos({}, {}, {}, participantId);
|
|
|
|
}
|
|
else
|
|
{
|
|
removeParticipantFromCont(m_participantsPerArea, participantId);
|
|
removeParticipantFromCont(m_visibleParticipantsPerAssignment, participantId);
|
|
}
|
|
|
|
syncExternal();
|
|
}
|
|
|
|
|
|
bool ShootbackController::participantPositionTopicIsValid(const ParticipantManagement::ParticipantPosition &posData)
|
|
{
|
|
// maybe add or replace:
|
|
// (!LtsUtils::isPositionTimeOutdated(ddsHandler->currentPartitionTime(), sampleTime)) &&
|
|
// participantTypeId // maybe check: typeId -> typeClassId...
|
|
|
|
return ((posData.participantId() > 0) &&
|
|
(posData.partyId() >= 2) && // 0->POOLS ; 1 -> O/C Service ; 2 RED...
|
|
(!LtsUtils::isPositionTimeOutdated(QDateTime::currentDateTimeUtc().toMSecsSinceEpoch(), posData.sampleTime()))
|
|
);
|
|
}
|
|
|
|
bool ShootbackController::isTarget(qint32 participantId, bool &bIsTarget) const
|
|
{
|
|
// maybe move this code to ppm as convenience!?
|
|
|
|
std::shared_ptr<ParticipantManagement::Interface::Dtc> dtcPpm = m_mainHandler->dds()->dtcPpm();
|
|
if (!dtcPpm)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no dtcPpm, return";
|
|
return false;
|
|
}
|
|
|
|
const ParticipantManagement::Participant &participant = dtcPpm->participant(participantId);
|
|
if (participant.id() != participantId)
|
|
return false;
|
|
|
|
const int typeId = participant.typeId();
|
|
const ParticipantManagement::Type &type = dtcPpm->type(typeId);
|
|
if (type.id() != typeId)
|
|
return false;
|
|
|
|
const int equipmentTypeId = type.equipmentTypeId();
|
|
const ParticipantManagement::EquipmentType &eqType = dtcPpm->equipmentType(equipmentTypeId);
|
|
if (eqType.id() != equipmentTypeId)
|
|
return false;
|
|
|
|
switch (eqType.wmsTypeClass().underlying())
|
|
{
|
|
case ParticipantManagement::WmsTypeClass::None:
|
|
case ParticipantManagement::WmsTypeClass::Building:
|
|
case ParticipantManagement::WmsTypeClass::Vehicle:
|
|
case ParticipantManagement::WmsTypeClass::Human:
|
|
case ParticipantManagement::WmsTypeClass::Airborne:
|
|
case ParticipantManagement::WmsTypeClass::VehicleMarker:
|
|
bIsTarget = false;
|
|
break;
|
|
case ParticipantManagement::WmsTypeClass::TargetVehicle:
|
|
case ParticipantManagement::WmsTypeClass::TargetHuman:
|
|
bIsTarget = true;
|
|
break;
|
|
default:
|
|
break;
|
|
} // switch
|
|
|
|
return true;
|
|
}
|
|
|
|
void ShootbackController::updateInternal_participantPerArea(const ObjectShootbackArea &area,
|
|
const ParticipantManagement::ParticipantPosition &posData)
|
|
{
|
|
const int areaId = area.getId();
|
|
const int participantId = posData.participantId();
|
|
|
|
bool contains = areaContainsPosition(area, posData);
|
|
|
|
QSet<int> &partIdRef = m_participantsPerArea[areaId];
|
|
bool alreadyInCont = partIdRef.contains(participantId);
|
|
|
|
DEBUG_LOG(m_logger) << " areaId = " << area.getId() << " ---> partIdRef VORHER" << toString(partIdRef);
|
|
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " "
|
|
<< " shootbackAreaId=" << areaId
|
|
<< " shootbackAreaName=" << area.getName()
|
|
<< " area.position=" << area.getData().position()
|
|
<< " participantId=" << participantId
|
|
<< " ParticipantPosition=" << posData
|
|
<< " contains=" << contains
|
|
<< " alreadyInCont=" << alreadyInCont;
|
|
|
|
if (contains && !alreadyInCont)
|
|
{
|
|
// ENTER
|
|
partIdRef.insert(participantId);
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " ENTER: participantId=" << participantId << " areaId=" << areaId;
|
|
|
|
}
|
|
else if (!contains && alreadyInCont)
|
|
{
|
|
// LEAVE
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " ENTER: participantId=" << participantId << " areaId=" << areaId;
|
|
partIdRef.remove(participantId);
|
|
}
|
|
|
|
DEBUG_LOG(m_logger) << " areaId = " << area.getId() << " ---> partIdRef NACHER" << toString(partIdRef);
|
|
}
|
|
|
|
bool ShootbackController::areaContainsPosition(const TargetManagement::ObjectShootbackArea &area,
|
|
const ParticipantManagement::ParticipantPosition &posData)
|
|
{
|
|
const ShootbackArea &sbArea = area.getData();
|
|
const LtsCoordTrans::UtmCoordinate &utmCoordCenter =
|
|
LtsCoordTrans::UtmCoordinate::fromGeographic(sbArea.position().lon(), sbArea.position().lat());
|
|
|
|
double xMin = utmCoordCenter.x - (sbArea.width() / 2.0);
|
|
double xMax = utmCoordCenter.x + (sbArea.width() / 2.0);
|
|
double yMin = utmCoordCenter.y - (sbArea.height() / 2.0);
|
|
double yMax = utmCoordCenter.y + (sbArea.height() / 2.0);
|
|
|
|
const LtsCoordTrans::UtmCoordinate &utmCoordParticipant =
|
|
LtsCoordTrans::UtmCoordinate::fromGeographic(posData.pos().lon(), posData.pos().lat());
|
|
|
|
if (
|
|
(xMin <= utmCoordParticipant.x) && (utmCoordParticipant.x <= xMax) &&
|
|
(yMin <= utmCoordParticipant.y) && (utmCoordParticipant.y <= yMax)
|
|
)
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
void ShootbackController::removeParticipantFromCont(QMap<int, QSet<int>> &contRef, int participantId)
|
|
{
|
|
// removed
|
|
QMutableMapIterator<int, QSet<int>>it(contRef);
|
|
while (it.hasNext())
|
|
{
|
|
it.next();
|
|
QSet<int> &partIdRef = it.value();
|
|
partIdRef.remove(participantId);
|
|
}
|
|
}
|
|
|
|
QSet<int> ShootbackController::areasWithParticipant(qint32 participantId) const
|
|
{
|
|
QSet<int> relAreaIds;
|
|
|
|
QMapIterator<int, QSet<int>>it(m_participantsPerArea);
|
|
while (it.hasNext())
|
|
{
|
|
it.next();
|
|
const QSet<int> &partIdRef = it.value();
|
|
if (partIdRef.contains(participantId))
|
|
relAreaIds.insert(it.key()); // it.key() == areaId
|
|
}
|
|
return relAreaIds;
|
|
}
|
|
|
|
QSet<int> ShootbackController::assignmentsWithTargetParticipantId(qint32 targetParticipantId) const
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << " targetParticipantId=" << targetParticipantId;
|
|
|
|
QSet<int> ret;
|
|
// all assignments
|
|
const QHash<int, ObjectTargetAreaAssignment *> &assignmentMap = m_handlerTargetAreaAssignment->getObjectMap();
|
|
for (ObjectTargetAreaAssignment *targetAreaAssignment : assignmentMap)
|
|
{
|
|
if (!targetAreaAssignment)
|
|
continue;
|
|
|
|
if (targetAreaAssignment->getData().targetParticipantId() == targetParticipantId)
|
|
ret += targetAreaAssignment->getId();
|
|
} // for
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << " areaId=" << targetParticipantId;
|
|
return ret;
|
|
}
|
|
|
|
bool idIsRelevantHelper(const QSet<int> &container, int id)
|
|
{
|
|
if (container.isEmpty())
|
|
return true;
|
|
return container.contains(id);
|
|
}
|
|
|
|
void ShootbackController::updateInternalLos(const QSet<int> &changingAssignmentIds, // ~static
|
|
const QSet<int> &changingAreaIds, // ~static
|
|
const QSet<int> &changingTartgetIds, // ~dynamic
|
|
int changedParticipantId) // ~dynamic
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " START"
|
|
<< ", changingAssigmentIds=" << toString(changingAssignmentIds)
|
|
<< ", changingAreaIds=" << toString(changingAreaIds)
|
|
<< ", changingTartgetIds=" << toString(changingTartgetIds)
|
|
<< ", changedParticipantId=" << changedParticipantId;
|
|
|
|
// loop over every assignment
|
|
const QHash<int, ObjectTargetAreaAssignment *> &allAssignments = m_handlerTargetAreaAssignment->getObjectMap();
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " allAssignments.size() " << allAssignments.size();
|
|
|
|
for (ObjectTargetAreaAssignment *assignment : allAssignments)
|
|
{
|
|
if (!assignment)
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " continue, curAssignment " << assignment->getId();
|
|
continue;
|
|
}
|
|
|
|
// 1. check changingAssigmentIds
|
|
const int assignmentId = assignment->getId();
|
|
if (!idIsRelevantHelper(changingAssignmentIds, assignmentId))
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " continue, curAssignment " << assignment->getId();
|
|
continue;
|
|
}
|
|
|
|
const TargetAreaAssignment &assignmentData = assignment->getData();
|
|
|
|
// 2. check changingAreaIds
|
|
const int areaId = assignmentData.areaId();
|
|
if (!idIsRelevantHelper(changingAreaIds, areaId))
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " continue, curAssignment " << assignment->getId();
|
|
continue;
|
|
}
|
|
|
|
// 3. check changingTartgetIds
|
|
const int targetParticpantId = assignmentData.targetParticipantId();
|
|
const int targetDataId = m_handlerShootbackTarget->targetDataIdForTargetParticipantId(targetParticpantId);
|
|
if (!idIsRelevantHelper(changingTartgetIds, targetDataId))
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " continue, curAssignment " << assignment->getId()
|
|
<< ", changingTartgetIds=" << toString(changingTartgetIds)
|
|
<< ", targetParticpantId=" << targetParticpantId
|
|
<< ", targetDataId=" << targetDataId;
|
|
continue;
|
|
}
|
|
|
|
// 4. check changedParticipantId
|
|
if (changedParticipantId > 0)
|
|
{
|
|
// Note: m_participantsPerArea is up-to-date
|
|
QSet<int> areasWithPart = areasWithParticipant(changedParticipantId);
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " areasWithPart=" << areasWithPart.size();
|
|
for (auto curAreaId : qAsConst(areasWithPart))
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " -> curAreaId=" << curAreaId;
|
|
|
|
if (!idIsRelevantHelper(areasWithPart, areaId))
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " continue, curAssignment " << assignment->getId();
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// real work:
|
|
// one assignment -> single target, single area, some participants (which are in that single area):
|
|
|
|
if (!updateInternalLosSingleAssignment(assignment))
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " updateInternalLosSingleAssignment failed, assignmentId=" << assignment->getId();
|
|
|
|
} // for
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END"
|
|
<< ", changingAssigmentIds=" << changingAssignmentIds.size()
|
|
<< ", changingAreaIds=" << changingAreaIds.size()
|
|
<< ", changingTartgetIds=" << changingTartgetIds.size()
|
|
<< ", changedParticipantId=" << changedParticipantId;
|
|
}
|
|
|
|
bool ShootbackController::updateInternalLosSingleAssignment(ObjectTargetAreaAssignment *assignment)
|
|
{
|
|
if (!assignment)
|
|
return false;
|
|
const int assignmentId = assignment->getId();
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START, assignmentId=" << assignmentId;
|
|
|
|
|
|
const TargetAreaAssignment &assignmentData = assignment->getData();
|
|
const int areaId = assignmentData.areaId();
|
|
const int targetParticipantId = assignmentData.targetParticipantId();
|
|
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " START, assignmentId=" << assignmentId
|
|
<< ", areaId = " << areaId
|
|
<< ", targetParticipantId = " << targetParticipantId;
|
|
|
|
ObjectShootbackArea *area = m_handlerShootbackArea->data(areaId);
|
|
if (!area)
|
|
{
|
|
// The area seems to be removed (no foreignKey area <-> assignment??)
|
|
// Neverless, for this assignment -> area there are no participants visible:
|
|
// clear container:
|
|
m_visibleParticipantsPerAssignment[assignmentId] = {};
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no area for areaId=" << areaId;
|
|
return false;
|
|
}
|
|
|
|
ObjectShootbackTarget *target = m_handlerShootbackTarget->dataForTargetParticipantId(targetParticipantId);
|
|
if (!target)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no target for targetParticipantId=" << targetParticipantId;
|
|
return false;
|
|
}
|
|
|
|
QSet<int> visiPartCont;
|
|
|
|
if (m_participantsPerArea.contains(areaId))
|
|
{
|
|
QSet<int> &partIdRef = m_participantsPerArea[areaId];
|
|
for (auto participantId : qAsConst(partIdRef))
|
|
{
|
|
// check lineOfSight:
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " check lineOfSight for: assignmentId=" << assignmentId
|
|
<< ", areaId = " << areaId
|
|
<< ", targetParticipantId = " << targetParticipantId
|
|
<< ", participantId=" << participantId;
|
|
|
|
// 1 targetGeoPos
|
|
ShootbackTarget sbTarget = target->getData();
|
|
const Core::GeoPosition &sbTargetGeoPos = sbTarget.position();
|
|
|
|
// 2 participantId -> geoPos:
|
|
const ParticipantManagement::ParticipantPosition &participantPos =
|
|
m_mainHandler->dds()->dtcPpm()->participantPosition(participantId);
|
|
|
|
if (participantPos.participantId() != participantId)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no position for participantId=" << participantId;
|
|
return false;
|
|
}
|
|
const Core::GeoPosition &participantGeoPos = participantPos.pos();
|
|
|
|
bool visi = lineOfSight(sbTargetGeoPos, participantGeoPos);
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " current targetId = " << target->getId() << " visi = " << visi;
|
|
|
|
if (visi)
|
|
visiPartCont.insert(participantId);
|
|
else
|
|
visiPartCont.remove(participantId);
|
|
} // for
|
|
|
|
} // if (m_participantsPerArea.contains(areaId))
|
|
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " assignmentId = " << assignmentId
|
|
<< " visiPartCont.size() " << visiPartCont.size();
|
|
|
|
// store at member
|
|
m_visibleParticipantsPerAssignment[assignmentId] = visiPartCont;
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END, assignmentId=" << assignmentId;
|
|
return true;
|
|
}
|
|
|
|
QSet<int> ShootbackController::visibleParticipantsPerTargetParticipantId(int targetParticipantId)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START, targetParticipantId = " << targetParticipantId;
|
|
|
|
QSet<int> visiPartPerTarget;
|
|
|
|
const QSet<int> &assignmentsWithTargetParticipantIdCont = assignmentsWithTargetParticipantId(targetParticipantId);
|
|
for (auto assignmentId : assignmentsWithTargetParticipantIdCont)
|
|
{
|
|
if (m_visibleParticipantsPerAssignment.contains(assignmentId))
|
|
visiPartPerTarget += m_visibleParticipantsPerAssignment[assignmentId];
|
|
}
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END, targetParticipantId = " << targetParticipantId
|
|
<< ", visiPartPerTarget = " << toString(visiPartPerTarget);
|
|
return visiPartPerTarget;
|
|
}
|
|
|
|
void ShootbackController::syncExternal()
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
|
|
// loop over all targets
|
|
const QHash<int, ObjectShootbackTarget *> &allTargets = m_handlerShootbackTarget->getObjectMap();
|
|
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " allTargets.size() " << allTargets.size();
|
|
|
|
for (ObjectShootbackTarget *target : allTargets)
|
|
{
|
|
if (!target)
|
|
continue;
|
|
|
|
const QSet<int> &visiPartPerTarget = visibleParticipantsPerTargetParticipantId(target->getTargetParticipantId()); // QSet -> unique and sorted
|
|
std::vector<int32_t> visiPartPerTargetStd;
|
|
visiPartPerTargetStd.reserve(visiPartPerTarget.size());
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " targetId " << target->getId() << ", visiPartPerTarget = " << toString(visiPartPerTarget);
|
|
for (auto id : qAsConst(visiPartPerTarget))
|
|
visiPartPerTargetStd.push_back(id);
|
|
|
|
|
|
const ShootbackTarget &sbTarget = target->getData();
|
|
if (visiPartPerTargetStd != sbTarget.visibleParticipants())
|
|
{
|
|
// data has changed
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " targetId differ -> updateObject..";
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " visiOld =" << toString(sbTarget.visibleParticipants());
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " visiNew =" << toString(visiPartPerTargetStd);
|
|
target->updateVolatileData(visiPartPerTargetStd); // update and publish
|
|
}
|
|
} // for
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
|
|
bool ShootbackController::lineOfSight(const Core::GeoPosition &observerPos, const Core::GeoPosition &targetPos) const
|
|
{
|
|
DEBUG_LOG(m_logger) << LOGFUNCTION << " calculate m_losCalculation = " << m_losCalculation
|
|
<< ", observerPos=" << observerPos
|
|
<< ", targetPos=" << targetPos;
|
|
|
|
if (!m_losCalculation)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error: no m_losCalculation";
|
|
return false;
|
|
}
|
|
|
|
SituationMap::ObserverTargetData observerTarget(Point3D(observerPos.lon(), observerPos.lat(), observerPos.z() + 2.0), // 2.0m als Augenhöhe
|
|
Point3D(targetPos.lon(), targetPos.lat(), observerPos.z() + 2.0) // 2.0m als Augenhöhe
|
|
);
|
|
|
|
bool success = m_losCalculation->calculate(observerTarget);
|
|
|
|
if (!success)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " calculate failed:"
|
|
<< " observerPos=" << observerPos
|
|
<< " targetPos=" << targetPos;
|
|
return false;
|
|
}
|
|
|
|
const SituationMap::LineOfSightVisibility visi = observerTarget.lineOfSightVisibility();
|
|
return SituationMap::LineOfSightVisibility::Visible == visi;
|
|
}
|
|
|
|
} // Namespace TargetManagement
|