532 lines
18 KiB
C++
Executable File
532 lines
18 KiB
C++
Executable File
/**
|
|
* @RDE_HEADER@
|
|
* Manage and combine several ElevationMap
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
*/
|
|
|
|
|
|
#include "elevation_map_manager.h"
|
|
#include "elevation_map.h"
|
|
|
|
// Qt
|
|
#include <QPointer>
|
|
#include <QFileInfo>
|
|
#include <QRectF>
|
|
#include <QPolygonF>
|
|
|
|
// system
|
|
#include <vector>
|
|
|
|
namespace SituationMap
|
|
{
|
|
|
|
static long s_counter{0};
|
|
|
|
//------------------------------------------------------------------------------------------------------------
|
|
// ElevationMapManager::Private
|
|
//------------------------------------------------------------------------------------------------------------
|
|
|
|
class ElevationMapManager::Private
|
|
{
|
|
public:
|
|
// fine to rough
|
|
std::vector<ElevationMap *> m_elevationMapCont;
|
|
QRectF m_simpleExtent;
|
|
};
|
|
|
|
|
|
//------------------------------------------------------------------------------------------------------------
|
|
// ElevationMapManager
|
|
//------------------------------------------------------------------------------------------------------------
|
|
ElevationMapManager::ElevationMapManager()
|
|
: BaseClass()
|
|
, d(new Private())
|
|
, m_logger(getLogger("ElevationMapManager"))
|
|
{
|
|
++s_counter;
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START, s_counter=" << s_counter << " this=" << this;
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END, s_counter=" << s_counter << " this=" << this;
|
|
}
|
|
|
|
ElevationMapManager::~ElevationMapManager()
|
|
{
|
|
--s_counter;
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START, s_counter=" << s_counter << " this=" << this;
|
|
cleanupCurrentContent();
|
|
delete d;
|
|
d = nullptr;
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END, s_counter=" << s_counter << " this=" << this;
|
|
}
|
|
int ElevationMapManager::init(const QStringList &filePaths_rough_to_fine)
|
|
{
|
|
NOTICE_LOG(m_logger) << LOGFUNCTION << " START" << " filePaths_rough_to_fine=" << filePaths_rough_to_fine.join(QStringLiteral(" ; "));
|
|
|
|
cleanupCurrentContent();
|
|
|
|
d->m_elevationMapCont.reserve(filePaths_rough_to_fine.size());
|
|
int cnt = 0;
|
|
for (const QString &filePath : filePaths_rough_to_fine)
|
|
{
|
|
QFileInfo fileInfo(filePath);
|
|
|
|
const QString &fileName = fileInfo.absoluteFilePath();
|
|
|
|
if (!fileInfo.exists())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " elevation data file \'" << fileName << "\' doesn't exists.";
|
|
continue;
|
|
}
|
|
|
|
if (!fileInfo.isReadable())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " elevation data file \'" << fileName << "\' isn't readable.";
|
|
continue;
|
|
}
|
|
|
|
// ctor will spawn thread via QtConcurrent and read file async:
|
|
d->m_elevationMapCont.insert(d->m_elevationMapCont.begin(), new ElevationMap(fileInfo));
|
|
++cnt;
|
|
} // for
|
|
|
|
NOTICE_LOG(m_logger) << LOGFUNCTION << " END" << " filePaths_rough_to_fine=" << filePaths_rough_to_fine.join(QStringLiteral(" ; "))
|
|
<< ", following number of files are going to be loaded: " << cnt;
|
|
return cnt;
|
|
}
|
|
|
|
void ElevationMapManager::cleanupCurrentContent()
|
|
{
|
|
qDeleteAll(d->m_elevationMapCont);
|
|
d->m_elevationMapCont.clear();
|
|
}
|
|
|
|
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
//------------------------------------------------------------------------------
|
|
//------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
bool ElevationMapManager::waitForInitialization()
|
|
{
|
|
for (ElevationMap *elevationMap : d->m_elevationMapCont)
|
|
elevationMap->waitForInitialization();
|
|
return isValid();
|
|
}
|
|
|
|
bool ElevationMapManager::isValid() const
|
|
{
|
|
bool allAreValid = true;
|
|
for (const ElevationMap *elevationMap : d->m_elevationMapCont)
|
|
allAreValid = allAreValid && elevationMap->isValid();
|
|
|
|
// FATAL_LOG(m_logger) << LOGFUNCTION << " allAreValid = " << allAreValid;
|
|
return allAreValid;
|
|
}
|
|
|
|
double ElevationMapManager::elevation(double lon_rad, double lat_rad, bool *bIsCovered) const
|
|
{
|
|
// assume, that all children are ready
|
|
for (const ElevationMap *elevationMap : d->m_elevationMapCont)
|
|
{
|
|
// qCritical() << " EMM::elevation: "
|
|
// << " lon_rad=" << lon_rad
|
|
// << " lat_rad=" << lat_rad
|
|
// << " (try: " << elevationMap->filename() << ")...";
|
|
|
|
double singleResult = elevationMap->elevation(lon_rad, lat_rad);
|
|
|
|
if (singleResult > -300) // -300 -> ausserhalb der Höhenkarte !!
|
|
{
|
|
// qCritical() << " EMM::elevation: elevation= " << singleResult << " (taken from: " << elevationMap->filename() << ")";
|
|
if (bIsCovered)
|
|
*bIsCovered = true;
|
|
return singleResult;
|
|
}
|
|
}
|
|
|
|
// DEBUG_LOG(m_logger) << LOGFUNCTION << " invalid= "
|
|
// << " lon_rad=" << lon_rad
|
|
// << " lat_rad=" << lat_rad;
|
|
if (bIsCovered)
|
|
*bIsCovered = false;
|
|
return std::numeric_limits<double>::min();
|
|
}
|
|
|
|
double ElevationMapManager::elevation(double lon_rad, double lat_rad, double defaultValue, bool *bIsCovered) const
|
|
{
|
|
bool bIsCoveredLocal = false;
|
|
const double elevationValue = elevation(lon_rad, lat_rad, &bIsCoveredLocal);
|
|
|
|
// in any case: if provided, transfer isCovered back to caller
|
|
if (bIsCovered)
|
|
*bIsCovered = bIsCoveredLocal;
|
|
|
|
// which value to be returned?
|
|
if (!bIsCoveredLocal)
|
|
return defaultValue;
|
|
|
|
return elevationValue;
|
|
}
|
|
|
|
|
|
QRectF ElevationMapManager::simpleUnitedExtent() const
|
|
{
|
|
if (d->m_simpleExtent.isNull())
|
|
{
|
|
bool bFirst = true;
|
|
for (const ElevationMap *elevationMap : d->m_elevationMapCont)
|
|
{
|
|
const QRectF &singleExtent = elevationMap->extent();
|
|
if (bFirst)
|
|
{
|
|
bFirst = false;
|
|
d->m_simpleExtent = singleExtent;
|
|
// qCritical() << " bFirst.left " << singleExtent.left() << " bFirst.right " << singleExtent.right();
|
|
// qCritical() << " bFirst.top " << singleExtent.top() << " bFirst.bottom " << singleExtent.bottom();
|
|
|
|
} else {
|
|
// prefer following instead QRectF::united (top vs. bottom):
|
|
// qCritical() << " next.left " << singleExtent.left() << " secnextright " << singleExtent.right();
|
|
// qCritical() << " next.top " << singleExtent.top() << " next.bottom " << singleExtent.bottom();
|
|
d->m_simpleExtent.setLeft (std::min(d->m_simpleExtent.left(), singleExtent.left()));
|
|
d->m_simpleExtent.setTop (std::max(d->m_simpleExtent.top(), singleExtent.top())); // top - >use max !!!
|
|
d->m_simpleExtent.setBottom(std::min(d->m_simpleExtent.bottom(), singleExtent.bottom())); // bottom -> use min
|
|
d->m_simpleExtent.setRight (std::max(d->m_simpleExtent.right(), singleExtent.right()));
|
|
}
|
|
} // for
|
|
}
|
|
|
|
return d->m_simpleExtent;
|
|
}
|
|
|
|
bool ElevationMapManager::contains(const QPointF &point) const
|
|
{
|
|
for (const ElevationMap *elevationMap : d->m_elevationMapCont)
|
|
{
|
|
if (elevationMap->contains(point))
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void ElevationMapManager::LOSProfile(const QPointF &start, const QPointF &end, LOSVector &losProfile)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
|
|
if (d->m_elevationMapCont.empty())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no elevationMap";
|
|
return;
|
|
}
|
|
|
|
waitForInitialization();
|
|
|
|
QRectF simpleExtentLocal = simpleUnitedExtent();
|
|
|
|
// qCritical() << " simpleExtent.left " << simpleExtentLocal.left() << " simpleExtent.right " << simpleExtentLocal.right();
|
|
// qCritical() << " simpleExtent.top " << simpleExtentLocal.top() << " simpleExtent.bottom " << simpleExtentLocal.bottom();
|
|
|
|
if (!simpleExtentLocal.contains(start))
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " start coordinate not within area"
|
|
<< ", start: x=" << start.x() << " ; y=" << start.y()
|
|
<< ", area: "
|
|
<< " left " << simpleExtentLocal.left() << ", right " << simpleExtentLocal.right()
|
|
<< " top " << simpleExtentLocal.top() << ", bottom " << simpleExtentLocal.bottom()
|
|
;
|
|
return; // ausserhalb der Höhenkarte !!
|
|
}
|
|
|
|
bool clipped = false;
|
|
double totalLng;
|
|
QPointF startG = start;
|
|
QPointF endG = end;
|
|
|
|
if (!simpleExtentLocal.contains(end)) // Clippen
|
|
{
|
|
clipped = true;
|
|
totalLng = length(endG - startG);
|
|
if (endG.x() < simpleExtentLocal.left())
|
|
{
|
|
double m = (endG.y() - startG.y()) / (endG.x() - startG.x());
|
|
double x = simpleExtentLocal.left();
|
|
endG.setX(x);
|
|
endG.setY(startG.y() + m * (x - startG.x()));
|
|
}
|
|
else if (endG.x() > simpleExtentLocal.right())
|
|
{
|
|
double m = (endG.y() - startG.y()) / (endG.x() - startG.x());
|
|
double x = simpleExtentLocal.right();
|
|
endG.setX(x);
|
|
endG.setY(startG.y() + m * (x - startG.x()));
|
|
}
|
|
if (endG.y() < simpleExtentLocal.bottom())
|
|
{
|
|
double m = (endG.x() - startG.x()) / (endG.y() - startG.y());
|
|
double y = simpleExtentLocal.bottom();
|
|
endG.setY(y);
|
|
endG.setX(startG.x() + m * (y - startG.y()));
|
|
}
|
|
else if (endG.y() > simpleExtentLocal.top())
|
|
{
|
|
double m = (endG.x() - startG.x()) / (endG.y() - startG.y());
|
|
double y = simpleExtentLocal.top();
|
|
endG.setY(y);
|
|
endG.setX(startG.x() + m * (y - startG.y()));
|
|
}
|
|
}
|
|
|
|
|
|
// Korrektursteigung aufgrund WGS84 Erdellipsoid, Lineare Approximation, Fehler bis 200 km: < 1.0m
|
|
double startRadius = radiusWGS84(startG.y());
|
|
double corrM = (radiusWGS84(endG.y()) - startRadius) / length(endG - startG);
|
|
|
|
//
|
|
// getSamplePoints(start, end, elevProfile);
|
|
// find best elevationMap for equested path {startG, endG}
|
|
//
|
|
|
|
// 1st find maps covering the desired pathG:
|
|
const QPolygonF pathG( QVector<QPointF>{startG, endG} );
|
|
|
|
std::vector<ElevationMap *> coveredElevationMaps;
|
|
coveredElevationMaps.reserve(d->m_elevationMapCont.size());
|
|
for (auto curItem : qAsConst(d->m_elevationMapCont))
|
|
{
|
|
if (curItem && curItem->intersects(pathG))
|
|
coveredElevationMaps.push_back(curItem);
|
|
} // for
|
|
|
|
if (m_logger.isDebugEnabled())
|
|
{
|
|
DEBUG_LOG(m_logger) << " coveredElevationMap.size() = " << coveredElevationMaps.size();
|
|
for (auto curItem : qAsConst(coveredElevationMaps))
|
|
DEBUG_LOG(m_logger) << " coveredElevationMap = " << curItem->filename();
|
|
}
|
|
|
|
// if no maps covers the desired path, stay on old approach: use most bottom elevation map:
|
|
if (coveredElevationMaps.empty())
|
|
coveredElevationMaps = d->m_elevationMapCont;
|
|
|
|
// 2nd: find elevation map with highest resolution to achieve best results:
|
|
using ELE_VEC_IT = std::vector<ElevationMap*>::iterator;
|
|
ELE_VEC_IT eleIt = coveredElevationMaps.begin();
|
|
for (ELE_VEC_IT curItem = coveredElevationMaps.begin()+1; curItem != coveredElevationMaps.end(); ++curItem)
|
|
{
|
|
ElevationMap *elePtr = *curItem;
|
|
if (!elePtr)
|
|
continue;
|
|
|
|
if (elePtr->elexelSize().first < (*eleIt)->elexelSize().first)
|
|
eleIt = curItem;
|
|
} // for
|
|
|
|
Q_ASSERT_X((*eleIt), "ElevationMapManager::LOSProfile", "eleIt failed");
|
|
|
|
Vec3dVector elevProfile;
|
|
(*eleIt)->getSamplePoints(start, end, elevProfile);
|
|
|
|
INFO_LOG(m_logger) << "Using ElevationMap '" << (*eleIt)->filename() << "'"
|
|
<< " (of size = " << d->m_elevationMapCont.size() << ")"
|
|
<< " with_elexelSize " << (*eleIt)->elexelSize().first
|
|
<< " -> # SamplePoints = " << elevProfile.size();
|
|
|
|
for (Vec3dVector::iterator itr = elevProfile.begin(); itr != elevProfile.end(); ++itr)
|
|
{
|
|
double dist = length(distance_in_meter_WGS84(start, itr->toPointF()));
|
|
double height = elevation(itr->x(), itr->y()); // use elevation over all seperate elevationMaps
|
|
height += corrM * dist; // WGS84 Erdellipsoid Korrektur
|
|
height -= (startRadius * (1 - cos(atan(dist / startRadius)))); // Einbeziehen der Erdkruemmung
|
|
// alternativ als Naeherung: height -= ( dist * dist) / (startRadius * 2.0);
|
|
losProfile.push_back(LOSentry(dist, height));
|
|
}
|
|
|
|
if (clipped)
|
|
losProfile.push_back(LOSentry(totalLng, -NAN));
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const LOSVector &losProfile,
|
|
double centerHat,
|
|
double objHat) const
|
|
{
|
|
// Analog zu SightFanImpl::calculateSightBeam
|
|
|
|
|
|
if (losProfile.size() < 2) // Startpunkt undefiniert oder ausserhalb der Karte
|
|
{
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
LOSVector::const_iterator itr = losProfile.begin();
|
|
const double centerHeight = itr->height() + centerHat;
|
|
|
|
double d1 = 0.0;
|
|
double height1 = itr->height();
|
|
|
|
// // überspringen bis _innerRadius erreicht ist !
|
|
// while( ++itr != losProfile.end())
|
|
// {
|
|
// double d2 = itr->dist(); // *EarthRadius;
|
|
// double height2 = itr->height();
|
|
// if( d2 >= m_internal->m_innerRadius)
|
|
// {
|
|
// height1 = height1 + (m_internal->m_innerRadius - d1) * ( height2 - height1)/(d2 - d1);
|
|
// d1 = m_internal->m_innerRadius;
|
|
// --itr;
|
|
// break;
|
|
// }
|
|
// d1 = d2;
|
|
// height1 = height2;
|
|
// }
|
|
|
|
// bool visible = true;
|
|
// SightMode mode = START;
|
|
|
|
d1 = itr->dist();
|
|
double curM = (height1 - centerHeight ) / d1; // bestimmende Steigung
|
|
|
|
// sBeam.add( SightEntry( d1, START) );
|
|
|
|
while (++itr != losProfile.end())
|
|
{
|
|
double d2 = itr->dist(); // *EarthRadius;
|
|
double height2 = itr->height();
|
|
|
|
if (std::isnan(height2)) // Undefined
|
|
{
|
|
return LineOfSight::LOS_UNDEFINED;
|
|
}
|
|
|
|
double hObj = (height2 + objHat - centerHeight );
|
|
double objM = hObj / d2; // Steigung bezogen auf Objekte( Objekte verdecken nicht !)
|
|
|
|
LineOfSight newMode = ((objM >= curM)) ? LineOfSight::LOS_VISIBLE : LineOfSight::LOS_INVISIBLE;
|
|
|
|
if (newMode == LineOfSight::LOS_INVISIBLE)
|
|
return LineOfSight::LOS_INVISIBLE;
|
|
|
|
|
|
// if (newMode != mode)
|
|
// {
|
|
// if ( (mode != LineOfSight::LOS_UNDEFINED) && (d2 != d1) )
|
|
// {
|
|
// // Korrekturwert für Distanz berechnen, unter Berücksichtigung der Objekthoehe !!
|
|
// double localM = (height2 - height1) /(d2 - d1);
|
|
// // localM == curM kann nicht auftreten, da newMode != mode
|
|
// double dCorr = (hObj - (curM * d2)) / (localM - curM);
|
|
|
|
// if (dCorr < 0.0)
|
|
// dCorr = 0.0;
|
|
// dCorr = d2 - dCorr;
|
|
// if (dCorr < d1)
|
|
// dCorr = d1;
|
|
|
|
// sBeam.add( SightEntry( dCorr, mode) );
|
|
// }
|
|
// mode = newMode;
|
|
// }
|
|
|
|
double terrM = (height2 - centerHeight) / d2; // Steigung bezogen auf Gelaende
|
|
if (terrM >= curM)
|
|
{
|
|
curM = terrM; // neue bestimmende Steigung
|
|
}
|
|
|
|
// height1 = height2;
|
|
// d1 = d2;
|
|
} // while
|
|
|
|
return LineOfSight::LOS_VISIBLE;
|
|
}
|
|
|
|
ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const LtsUtils::QPoint3D &startPointGeo_absZ,
|
|
const LtsUtils::QPoint3D &endPointGeo_absZ)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START" << ", startPointGeo_absZ = " << startPointGeo_absZ << ", endPointGeo_absZ = " << endPointGeo_absZ;
|
|
|
|
if (!isValid())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, LtsElevationManager not valid";
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
// 1st: a) centerHat / startHat:
|
|
const double startHat = startPointGeo_absZ.z() - this->elevation(startPointGeo_absZ.x(), startPointGeo_absZ.y(), -10000);
|
|
if (startHat < 0)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, startHat is negativ";
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
// 1st: b) endHat / objHat:
|
|
const double endHat = endPointGeo_absZ.z() - this->elevation(endPointGeo_absZ.x(), endPointGeo_absZ.y(), -10000);
|
|
if (endHat < 0)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, objHat is negativ"
|
|
<< ", startPointGeo_absZ=" << startPointGeo_absZ
|
|
<< ", endPointGeo_absZ=" << endPointGeo_absZ;
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
SituationMap::ElevationMapManager::LineOfSight eleMapMgrResult = lineOfSightCalculation(startPointGeo_absZ.toPointF(), startHat,
|
|
endPointGeo_absZ.toPointF(), endHat);
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << ", startPointGeo = " << startPointGeo_absZ << ", endPointGeo = " << endPointGeo_absZ
|
|
<< " -> LineOfSight " << static_cast<int>(eleMapMgrResult);
|
|
return eleMapMgrResult;
|
|
}
|
|
|
|
ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const QPointF &startPointGeo, double startHat,
|
|
const QPointF &endPointGeo, double endHat)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START"
|
|
<< ", startPointGeo = " << startPointGeo << ", startHat = " << startHat
|
|
<< ", endPointGeo = " << endPointGeo << ", endHat = " << endHat ;
|
|
if (!isValid())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, LtsElevationManager not valid";
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
if (startHat < 0)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, startHat is negativ";
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
if (endHat < 0)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, endHat is negativ";
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
// 1st: create LOSVector:
|
|
SituationMap::LOSVector losProfile;
|
|
LOSProfile(startPointGeo, endPointGeo, losProfile);
|
|
if (losProfile.size() < 2)
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, losProfile not valid"
|
|
<< ", losProfile.size = " << losProfile.size();
|
|
return LineOfSight::LOS_ERROR;
|
|
}
|
|
|
|
// 2nd: lineOfSightCalculation
|
|
SituationMap::ElevationMapManager::LineOfSight eleMapMgrResult = this->lineOfSightCalculation(losProfile, startHat, endHat);
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END"
|
|
<< ", startPointGeo = " << startPointGeo << ", startHat = " << startHat
|
|
<< ", endPointGeo = " << endPointGeo << ", endPointHat = " << endHat
|
|
<< " -> LineOfSight " << static_cast<int>(eleMapMgrResult);
|
|
return eleMapMgrResult;
|
|
}
|
|
|
|
|
|
|
|
|
|
} // SituationMap
|
|
|