331 lines
11 KiB
C++
Executable File
331 lines
11 KiB
C++
Executable File
/**
|
|
* @RDE_HEADER@
|
|
* Interface
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
*/
|
|
|
|
#include <QtWidgets/QApplication>
|
|
#include <QMainWindow>
|
|
#include <QFileInfo>
|
|
#include <QTimer>
|
|
#include <QElapsedTimer>
|
|
|
|
|
|
#include <nglapplication/nglapplication.h>
|
|
#include <Common/situation_map_logging.h>
|
|
|
|
#include <ElevationMap/elevation_map.h>
|
|
|
|
#include <arcgis/GeometryEngine.h>
|
|
|
|
#include <Map/situation_map_types.h>
|
|
using namespace SituationMap;
|
|
|
|
|
|
// some helper functions
|
|
inline Q_CONSTEXPR double pi() { return M_PI; }
|
|
inline Q_CONSTEXPR double deg2rad() { return pi() / 180.; }
|
|
inline Q_CONSTEXPR double rad2deg() { return 1. / deg2rad(); }
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
/*!
|
|
\brief Define visibility.
|
|
*/
|
|
enum class LineOfSightVisibility
|
|
{
|
|
Undefined, /*!< May be no data? */
|
|
Visible,
|
|
Obstructed
|
|
};
|
|
//----------------------------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
class ObserverTargetData
|
|
{
|
|
public:
|
|
explicit ObserverTargetData(const Point3D &oberserver = Point3D(), // z-value: elevation@terrain + observerHeight
|
|
const Point3D &target = Point3D()) // z-value: elevation@terrain + targetHeight
|
|
: m_oberserver(oberserver),
|
|
m_target(target)
|
|
{}
|
|
|
|
const Point3D &observer() const { return m_oberserver; }
|
|
const Point3D &target() const { return m_target; }
|
|
|
|
const Point3D &obstacle() const { return m_obstacle; }
|
|
void setObstacle(const Point3D &obstacle) { m_obstacle = obstacle; }
|
|
|
|
LineOfSightVisibility lineOfSightVisibility() const { return m_visible; }
|
|
void setLineOfSightVisibility(const LineOfSightVisibility &visible)
|
|
{
|
|
m_visible = visible;
|
|
}
|
|
|
|
bool isObserverAndTargetValid()
|
|
{
|
|
return (!m_oberserver.isNull()) && (!m_target.isNull());
|
|
}
|
|
|
|
private:
|
|
Point3D m_oberserver;
|
|
Point3D m_target;
|
|
|
|
Point3D m_obstacle;
|
|
LineOfSightVisibility m_visible = LineOfSightVisibility::Undefined;
|
|
|
|
};
|
|
//----------------------------------------------------------------------------
|
|
|
|
|
|
|
|
class LineOfSightCalculation
|
|
{
|
|
public:
|
|
explicit LineOfSightCalculation(ElevationMap *elevationMap)
|
|
: m_logger(getLogger("LineOfSightCalculation"))
|
|
, m_elevationMap(elevationMap)
|
|
{
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
|
}
|
|
|
|
bool calculate(ObserverTargetData &observerTarget, unsigned int sampleIntervallMeter = 5)
|
|
{
|
|
// same checks:
|
|
if (!m_elevationMap || !m_elevationMap->isValid())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " no elevationMap configured or not valid";
|
|
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Undefined);
|
|
return false;
|
|
}
|
|
|
|
if (!observerTarget.isObserverAndTargetValid())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " observer and/or target not valid";
|
|
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Undefined);
|
|
return false;
|
|
}
|
|
|
|
if (observerTarget.observer().toPointF() == observerTarget.target().toPointF())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " observer and target are identical regard x and y";
|
|
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Undefined);
|
|
return false;
|
|
}
|
|
|
|
|
|
bool success = true;
|
|
|
|
QElapsedTimer timer;
|
|
if (m_logger.isErrorEnabled())
|
|
timer.start();
|
|
|
|
//
|
|
//
|
|
// consider using ElevationMap::LOSProfile
|
|
//
|
|
//
|
|
|
|
// determine how many iterations we need:
|
|
double distanceInMeter = distanceMeter(observerTarget.observer(),
|
|
observerTarget.target());
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " distanceMeter = " << distanceInMeter;
|
|
|
|
int maxNumberOfIterations = distanceInMeter / sampleIntervallMeter; // check visibility every ~5m
|
|
// maxNumberOfIterations = 1000000;
|
|
|
|
if (maxNumberOfIterations<=0)
|
|
maxNumberOfIterations = 5;
|
|
|
|
|
|
double distanceFromObserverToTargetAsFraction = 0;
|
|
int i = 0;
|
|
while (i<=maxNumberOfIterations)
|
|
{
|
|
/*
|
|
|
|
z-axis (Elevation) pCandidate
|
|
|---------- /
|
|
| ----------
|
|
| ----------
|
|
| ----------
|
|
| ----------
|
|
|------------------------------------------------------------ xy-axis
|
|
^ ^ ^ ^ ^ ^ ^ ^ samples (every ~5meters)
|
|
^ ^
|
|
ObserverPosition TargetPosition
|
|
|
|
On every sample position, check if directLineElevation (maximum) is
|
|
obstructed by elevationMapElevation (real value)
|
|
if (elevationMapElevation>directLineElevation) -> obstructed/break
|
|
*/
|
|
|
|
distanceFromObserverToTargetAsFraction = (double)i/maxNumberOfIterations;
|
|
|
|
|
|
const Point3D &pCandidate = movePoint(observerTarget.observer(),
|
|
observerTarget.target(),
|
|
distanceFromObserverToTargetAsFraction);
|
|
|
|
// {
|
|
// static Point3D oldCandidate;
|
|
// double distanceInMeter = distanceMeter(pCandidate, oldCandidate);
|
|
// ERROR_LOG(m_logger) << LOGFUNCTION << " distanceMeter = " << distanceInMeter;
|
|
// oldCandidate = pCandidate;
|
|
// }
|
|
|
|
|
|
// z is the elevation of a *direct* line:
|
|
const double directLineElevation = pCandidate.z();
|
|
const double elevationMapElevation = m_elevationMap->elevation(pCandidate.x(), pCandidate.y());
|
|
|
|
// Coord covered by elevationMap?
|
|
if (elevationMapElevation <= std::numeric_limits<int>::min())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " Error, coordinate not covered be elevationData. Finish this LineOfSightCalculation."
|
|
<< " i=" << i
|
|
<< ", pCandidate = " << pCandidate;
|
|
|
|
// write back data
|
|
observerTarget.setObstacle(pCandidate);
|
|
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Undefined);
|
|
|
|
// finish the loop:
|
|
success = false;
|
|
break;
|
|
}
|
|
|
|
// analyze / check visibility:
|
|
if (elevationMapElevation>directLineElevation)
|
|
{
|
|
INFO_LOG(m_logger) << LOGFUNCTION << " i=" << i
|
|
<< " break - pCandidate = " << pCandidate
|
|
<< " -> directLineElevation = " << directLineElevation
|
|
<< " -> elevationMapElevation = " << elevationMapElevation;
|
|
|
|
// write result
|
|
observerTarget.setObstacle(pCandidate);
|
|
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Obstructed);
|
|
|
|
// finish the loop:
|
|
break;
|
|
}
|
|
|
|
|
|
++i;
|
|
} // while
|
|
|
|
|
|
if (m_logger.isErrorEnabled())
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " END"
|
|
<< ", success = " << success
|
|
<< ", " << i << " of "<< maxNumberOfIterations << " iterations"
|
|
<< ", distanceFromObserverToTargetAsFraction = " << distanceFromObserverToTargetAsFraction
|
|
<< ", pCandidate=" << observerTarget.obstacle()
|
|
<< ", lineOfSightVisibility=" << static_cast<int>(observerTarget.lineOfSightVisibility())
|
|
<< ", took [ms] " << timer.elapsed();
|
|
}
|
|
|
|
TRACE_LOG(m_logger) << LOGFUNCTION << " END" << ", success = " << success;
|
|
return success;
|
|
}
|
|
|
|
protected:
|
|
static Point3D movePoint(const Point3D &oberserver,
|
|
const Point3D &target,
|
|
double distanceFromObserverToTargetAsFraction) // 0.0 ... 1.0
|
|
{
|
|
// positions are lon/lat @ rad.
|
|
// for evaluation we do that very simple:
|
|
|
|
// strange, but: using Point3D seems to be faster then using QPointF
|
|
const Point3D &dist = target - oberserver;
|
|
const Point3D &point = oberserver + (dist * distanceFromObserverToTargetAsFraction);
|
|
|
|
// FATAL_LOG(m_logger) << LOGFUNCTION << " obs = " << oberserver;
|
|
// FATAL_LOG(m_logger) << LOGFUNCTION << " tgt = " << target;
|
|
// FATAL_LOG(m_logger) << LOGFUNCTION << " fct = " << distanceFromObserverToTargetInPercent;
|
|
// FATAL_LOG(m_logger) << LOGFUNCTION << " mvd = " << point;
|
|
|
|
return point;
|
|
}
|
|
|
|
static double distanceMeter(const Point3D &oberserver,
|
|
const Point3D &target)
|
|
{
|
|
Esri::ArcGISRuntime::Point geom1(oberserver.x()*rad2deg(), oberserver.y()*rad2deg(), oberserver.z(), Esri::ArcGISRuntime::SpatialReference::wgs84());
|
|
Esri::ArcGISRuntime::Point geom2(target.x() *rad2deg(), target.y() *rad2deg(), target.z() , Esri::ArcGISRuntime::SpatialReference::wgs84());
|
|
|
|
Esri::ArcGISRuntime::GeometryEngine *gEngine = Esri::ArcGISRuntime::GeometryEngine::instance();
|
|
Esri::ArcGISRuntime::GeodeticDistanceResult geodeticDistanceResult = gEngine->distanceGeodetic(geom1, geom2,
|
|
Esri::ArcGISRuntime::LinearUnitId::Meters,
|
|
Esri::ArcGISRuntime::AngularUnitId::Degrees,
|
|
Esri::ArcGISRuntime::GeodeticCurveType::Geodesic);
|
|
|
|
double distance = geodeticDistanceResult.distance();
|
|
|
|
return distance;
|
|
}
|
|
|
|
|
|
|
|
|
|
private:
|
|
LOGGER_T m_logger; ///< Interface of the Logging-Facility
|
|
ElevationMap *m_elevationMap = nullptr;
|
|
|
|
ObserverTargetData m_observerTarget;
|
|
};
|
|
|
|
|
|
|
|
#include <ElevationMap/elevation_map.h>
|
|
|
|
|
|
int main (int argc, char* argv[])
|
|
{
|
|
NGLApplication app ( argc, argv );
|
|
|
|
app.createLogger("lineOfSightEvaluation", "log4cxx_sitmapMain.props");
|
|
|
|
LOGGER_T m_logger=app.baseLogger();
|
|
|
|
STARTUP_LOG(m_logger);
|
|
|
|
|
|
QString elevationFileAbs(QLatin1String("/usr/share/arcgis/resources/elevation/srtm_38_02.tif")); // srtm_38_02.tif -> Bremen
|
|
QFileInfo fi (elevationFileAbs);
|
|
|
|
if (!fi.exists(elevationFileAbs))
|
|
{
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << ", elevationFile '" << elevationFileAbs << "' doesn't exists";
|
|
return -1;
|
|
}
|
|
|
|
ElevationMap *elevationMap = new ElevationMap(fi);
|
|
|
|
elevationMap->waitForInitialization();
|
|
ERROR_LOG(m_logger) << LOGFUNCTION << " elevation.isValid() =" << elevationMap->isValid();
|
|
|
|
//qCritical() << LOGFUNCTION << " elevation.isValid() = " << elevationMap->isValid()
|
|
// << " elevation =" << elevationMap->elevation(0.155, 0.926);
|
|
|
|
LineOfSightCalculation losCalc(elevationMap);
|
|
|
|
ObserverTargetData obsTgt(Point3D(0.1504, 0.926, 20),
|
|
Point3D(0.1530, 0.926, 5));
|
|
|
|
losCalc.calculate(obsTgt);
|
|
|
|
int iRet = app.exec();
|
|
|
|
delete elevationMap;
|
|
SHUTDOWN_LOG(m_logger);
|
|
|
|
return iRet;
|
|
}
|