Initial commit
This commit is contained in:
torsten committed
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
@@ -0,0 +1,229 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* LineOfSightCalculation (L400)
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by N. Thoden, NLE, 06.04.2020
|
||||
*/
|
||||
|
||||
#include "line_of_sight_calculation.h"
|
||||
#include "elevation_map.h"
|
||||
|
||||
#include <LtsCoordTrans/lts_coord_trans.h>
|
||||
|
||||
//
|
||||
// ESRI / ARCGIS
|
||||
// if you link libEsriCommonQt, you propably have to add ArcGISQt5Hack AND: # make sure it's loaded *before* libEsriCommonQt.so
|
||||
// this may effect all executables linking situationMapElevationMap. CHECK!
|
||||
//
|
||||
|
||||
|
||||
// Qt
|
||||
#include <QTimer>
|
||||
#include <QElapsedTimer>
|
||||
|
||||
|
||||
// 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(); }
|
||||
|
||||
|
||||
namespace SituationMap
|
||||
{
|
||||
|
||||
LineOfSightCalculation::LineOfSightCalculation(ElevationMap *elevationMap)
|
||||
: m_logger(getLogger("LineOfSightCalculation"))
|
||||
, m_elevationMap(elevationMap)
|
||||
{
|
||||
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
||||
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
||||
}
|
||||
|
||||
LineOfSightCalculation::~LineOfSightCalculation()
|
||||
{
|
||||
TRACE_LOG(m_logger) << LOGFUNCTION << " START";
|
||||
TRACE_LOG(m_logger) << LOGFUNCTION << " END";
|
||||
}
|
||||
|
||||
bool LineOfSightCalculation::calculate(ObserverTargetData &observerTarget, unsigned int sampleIntervallMeter)
|
||||
{
|
||||
// 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 regarding x and y";
|
||||
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Undefined);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool success = true;
|
||||
|
||||
QElapsedTimer timer;
|
||||
if (m_logger.isInfoEnabled())
|
||||
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)
|
||||
{
|
||||
++i;
|
||||
|
||||
/*
|
||||
|
||||
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)
|
||||
{
|
||||
DEBUG_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;
|
||||
} else {
|
||||
observerTarget.setLineOfSightVisibility(LineOfSightVisibility::Visible);
|
||||
}
|
||||
|
||||
} // while
|
||||
|
||||
|
||||
if (m_logger.isInfoEnabled())
|
||||
{
|
||||
INFO_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;
|
||||
}
|
||||
|
||||
Point3D LineOfSightCalculation::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);
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
double LineOfSightCalculation::distanceMeter(const Point3D &oberserver, const Point3D &target)
|
||||
{
|
||||
// Try to avoid the dependency to ESRI (keep ArcGISQt5Hack in mind)
|
||||
// And good accuracy is not required here
|
||||
// use LtsCoordTrans::distance
|
||||
|
||||
// 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();
|
||||
|
||||
|
||||
double distance = LtsCoordTrans::distance(oberserver.toPointF(), target.toPointF());
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
||||
} // namespace SituationMap
|
||||
|
||||
Reference in new issue
Block a user