qt5-legatus-legacy/Lts/Viewer/SituationMap/ElevationMap/line_of_sight_calculation.cpp

/**
 * @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