raycast intersection with node bounding box

qgis · Jan 13, 2021 · 4f3f95a · 4f3f95a
1 parent a98ec51
commit 4f3f95a
Show file tree

Hide file tree

Showing 13 changed files with 141 additions and 18 deletions.
diff --git a/python/core/auto_generated/geometry/qgsbox3d.sip.in b/python/core/auto_generated/geometry/qgsbox3d.sip.in
@@ -219,6 +219,8 @@ If the point is a 2D point (no z-coordinate), then the containment test
 will be performed on the x/y extent of the box only.
 %End
 
+    float distanceFromPoint( double x, double y, double z );
+
     QgsRectangle toRectangle() const;
 %Docstring
 Converts the box to a 2D rectangle.

diff --git a/python/core/auto_generated/pointcloud/qgspointclouddataprovider.sip.in b/python/core/auto_generated/pointcloud/qgspointclouddataprovider.sip.in
@@ -56,7 +56,6 @@ Ctor
 
     ~QgsPointCloudDataProvider();
 
-
     SIP_PYLIST identify( float maxErrorInMapCoords, QgsGeometry extentGeometry, const QgsDoubleRange extentZRange = QgsDoubleRange(), int pointsLimit = 1000 );
 %Docstring
 Returns the list of points of the point cloud according to a zoom level
@@ -88,6 +87,7 @@ the number of points returned to ``pointsLimit`` points
       }
     }
 %End
+    QVector<IndexedPointCloudNode> getNodesIntersectingWithRay( const QgsPointCloudIndex *pc, IndexedPointCloudNode n, double maxError, double nodeError, double cameraFov, int screenSizePx, const QVector3D &rayOrigin, const QVector3D &rayDirection );
 
     virtual QgsPointCloudDataProvider::Capabilities capabilities() const;
 %Docstring
@@ -169,7 +169,6 @@ Only providers which report the CreateRenderer capability will return a 2D rende
 providers will return ``None``.
 %End
 
-
     SIP_PYOBJECT metadataStatistic( const QString &attribute, QgsStatisticalSummary::Statistic statistic ) const;
 %Docstring
 Returns a statistic for the specified ``attribute``, taken only from the metadata of the point cloud

diff --git a/python/core/auto_generated/pointcloud/qgspointcloudlayer.sip.in b/python/core/auto_generated/pointcloud/qgspointcloudlayer.sip.in
@@ -132,6 +132,7 @@ Ownership of ``renderer`` is transferred to the layer.
 .. seealso:: :py:func:`renderer`
 %End
 
+    QVector<QMap<QString, QVariant>> getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection, double maxScreenError, double cameraFov, int screenSizePx );
   private:
     QgsPointCloudLayer( const QgsPointCloudLayer &rhs );
 };

diff --git a/src/3d/chunks/qgschunkedentity_p.cpp b/src/3d/chunks/qgschunkedentity_p.cpp
@@ -54,6 +54,16 @@ static float screenSpaceError( float epsilon, float distance, float screenSize,
    *       camera
    */
   float phi = epsilon * screenSize / ( 2 * distance * tan( fov * M_PI / ( 2 * 180 ) ) );
+  {
+    static int k = 0;
+    if ( k % 20 == 0 )
+    {
+      qDebug() << "screenSize: " << screenSize;
+      qDebug() << "fov: " << fov;
+      qDebug() << "epsilon: " << epsilon;
+    }
+    ++k;
+  }
   return phi;
 }
 

diff --git a/src/3d/qgs3dmapscene.cpp b/src/3d/qgs3dmapscene.cpp
@@ -1099,15 +1099,32 @@ void Qgs3DMapScene::exportScene( const Qgs3DMapExportSettings &exportSettings )
   }
 }
 
-void Qgs3DMapScene::onRayCasted( const QVector3D &rayOrigin, QVector3D &rayDirection )
+void Qgs3DMapScene::onRayCasted( const QVector3D &rayOrigin, const QVector3D &rayDirection )
 {
-  qDebug() << __PRETTY_FUNCTION__;
+  qDebug() << __PRETTY_FUNCTION__ << " "  << rayOrigin << " " << rayDirection;
+//  QVector3D point2 = rayOrigin + rayOrigin.length() * rayDirection.normalized();
+  qDebug() << "Origin: " << mMap.origin().x() << " " << mMap.origin().y() << " " << mMap.origin().z();
+  QgsVector3D originMapCoords = mMap.worldToMapCoordinates( rayOrigin );
+  QgsVector3D pointMapCoords = mMap.worldToMapCoordinates( rayOrigin + rayOrigin.length() * rayDirection.normalized() );
+  QgsVector3D directionMapCoords = pointMapCoords - originMapCoords;
+  directionMapCoords.normalize();
+
+
+  QVector3D rayOriginMapCoords( originMapCoords.x(), originMapCoords.y(), originMapCoords.z() );
+  QVector3D rayDirectionMapCoords( directionMapCoords.x(), directionMapCoords.y(), directionMapCoords.z() );
+
+  QRect rect = mCameraController->viewport();
+  int screenSizePx = std::max( rect.width(), rect.height() ); // TODO: is this correct? (see _sceneState)
+  float fov = mCameraController->camera()->fieldOfView();
+
   for ( QgsMapLayer *layer : mMap.layers() )
   {
     if ( layer->type() != QgsMapLayerType::PointCloudLayer ) continue;
     if ( QgsPointCloudLayer *pc = dynamic_cast<QgsPointCloudLayer *>( layer ) )
     {
-      pc->getPointsOnRay( rayOrigin, rayDirection );
+      QgsPointCloudLayer3DRenderer *renderer = dynamic_cast<QgsPointCloudLayer3DRenderer *>( pc->renderer3D() );
+      double maxScreenError = renderer->maximumScreenError();
+      pc->getPointsOnRay( rayOriginMapCoords, rayDirectionMapCoords, maxScreenError, fov, screenSizePx );
     }
   }
 }
diff --git a/src/3d/qgs3dmapscene.h b/src/3d/qgs3dmapscene.h
@@ -59,6 +59,15 @@ class QgsPostprocessingEntity;
 
 #define SIP_NO_FILE
 
+//! Records some bits about the scene (context for update() method)
+struct CameraState
+{
+  QVector3D cameraPos;   //!< Camera position
+  float cameraFov;       //!< Field of view (in degrees)
+  int screenSizePx;      //!< Size of the viewport in pixels
+  QMatrix4x4 viewProjectionMatrix; //!< For frustum culling
+};
+
 /**
  * \ingroup 3d
  * Entity that encapsulates our 3D scene - contains all other entities (such as terrain) as children.
@@ -134,7 +143,7 @@ class _3D_EXPORT Qgs3DMapScene : public Qt3DCore::QEntity
   public slots:
     //! Updates the temporale entities
     void updateTemporal();
-    void onRayCasted( const QVector3D &rayOrigin, QVector3D &rayDirection );
+    void onRayCasted( const QVector3D &rayOrigin, const QVector3D &rayDirection );
 
   private slots:
     void onCameraChanged();

diff --git a/src/app/3d/qgs3dmapcanvas.cpp b/src/app/3d/qgs3dmapcanvas.cpp
@@ -274,5 +274,6 @@ void Qgs3DMapCanvas::mouseReleased( QMouseEvent *event )
   rayWorld = rayWorld.normalized();
 //  qDebug() << "rayEye: " << rayEye;
 //  qDebug() << "rayWorld: " << rayWorld;
-  mScene->onRayCasted( QVector3D( rayEye ), rayWorld );
+  QVector4D rayOrigin = viewMatrix.inverted() * QVector4D( 0.0f, 0.0f, 0.0f, 1.0f );
+  mScene->onRayCasted( QVector3D( rayOrigin ), rayWorld );
 }
diff --git a/src/core/geometry/qgsbox3d.cpp b/src/core/geometry/qgsbox3d.cpp
@@ -18,7 +18,6 @@
 #include "qgsbox3d.h"
 #include "qgspoint.h"
 
-
 QgsBox3d::QgsBox3d( double xmin, double ymin, double zmin, double xmax, double ymax, double zmax )
   : mBounds2d( xmin, ymin, xmax, ymax )
   , mZmin( zmin )
@@ -125,3 +124,11 @@ bool QgsBox3d::operator==( const QgsBox3d &other ) const
          qgsDoubleNear( mZmin, other.mZmin ) &&
          qgsDoubleNear( mZmax, other.mZmax );
 }
+
+float QgsBox3d::distanceFromPoint( double x, double y, double z )
+{
+  float dx = std::max( mBounds2d.xMinimum() - x, std::max( 0., x - mBounds2d.xMaximum() ) );
+  float dy = std::max( mBounds2d.yMinimum() - y, std::max( 0., y - mBounds2d.yMaximum() ) );
+  float dz = std::max( mZmin - z, std::max( 0., z - mZmax ) );
+  return sqrt( dx * dx + dy * dy + dz * dz );
+}
diff --git a/src/core/geometry/qgsbox3d.h b/src/core/geometry/qgsbox3d.h
@@ -198,6 +198,8 @@ class CORE_EXPORT QgsBox3d
      */
     bool contains( const QgsPoint &point ) const;
 
+    float distanceFromPoint( double x, double y, double z );
+
     /**
      * Converts the box to a 2D rectangle.
      */

diff --git a/src/core/pointcloud/qgspointclouddataprovider.cpp b/src/core/pointcloud/qgspointclouddataprovider.cpp
@@ -22,6 +22,7 @@
 #include "qgspointcloudrequest.h"
 #include "qgsgeometryengine.h"
 #include <mutex>
+#include <QDebug>
 
 #include <QtConcurrent/QtConcurrentMap>
 
@@ -406,26 +407,95 @@ QVector<IndexedPointCloudNode> QgsPointCloudDataProvider::traverseTree(
 
   return nodes;
 }
-QVector<QMap<QString, QVariant>> QgsPointCloudDataProvider::getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection )
+QVector<QMap<QString, QVariant>> QgsPointCloudDataProvider::getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection, double maxScreenError, double cameraFov, int screenSizePx )
 {
   QVector<QMap<QString, QVariant>> points;
+  qDebug() << "Ray: " << rayOrigin << " " << rayDirection;
+  QgsPointCloudIndex *index = this->index();
+  IndexedPointCloudNode root = index->root();
+  QgsRectangle rootNodeExtentMapCoords = index->nodeMapExtent( root );
+  const float rootErrorInMapCoordinates = rootNodeExtentMapCoords.width() / index->span();
+
+  QVector<IndexedPointCloudNode> nodes = getNodesIntersectingWithRay( index, root, maxScreenError, rootErrorInMapCoordinates, cameraFov, screenSizePx, rayOrigin, rayDirection );
+  qDebug() << "Intersected nodes: " << nodes.size();
   return points;
 }
 
+bool __boxIntesects( const QgsBox3d &box, const QVector3D &rayOrigin, const QVector3D &rayDirection )
+{
+  double tminX = box.xMinimum() - rayOrigin.x(), tmaxX = box.xMaximum() - rayOrigin.x();
+  double tminY = box.yMinimum() - rayOrigin.y(), tmaxY = box.yMaximum() - rayOrigin.y();
+  double tminZ = box.zMinimum() - rayOrigin.z(), tmaxZ = box.zMaximum() - rayOrigin.z();
+  if ( rayDirection.x() < 0 ) std::swap( tminX, tmaxX );
+  if ( rayDirection.y() < 0 ) std::swap( tminY, tmaxY );
+  if ( rayDirection.z() < 0 ) std::swap( tminZ, tmaxZ );
+  if ( rayDirection.x() != 0 )
+  {
+    tminX /= rayDirection.x();
+    tmaxX /= rayDirection.x();
+  }
+  else
+  {
+    tminX = std::numeric_limits<double>::lowest();
+    tmaxX = std::numeric_limits<double>::max();
+  }
+  if ( rayDirection.y() != 0 )
+  {
+    tminY /= rayDirection.y();
+    tmaxY /= rayDirection.y();
+  }
+  else
+  {
+    tminY = std::numeric_limits<double>::lowest();
+    tmaxY = std::numeric_limits<double>::max();
+  }
+  if ( rayDirection.z() != 0 )
+  {
+    tminZ /= rayDirection.z();
+    tmaxZ /= rayDirection.z();
+  }
+  else
+  {
+    tminZ = std::numeric_limits<double>::lowest();
+    tmaxZ = std::numeric_limits<double>::max();
+  }
+  QgsDoubleRange tRange( qMax( qMax( tminX, tminY ), tminZ ), qMin( qMin( tmaxX, tmaxY ), tmaxZ ) );
+  return !tRange.isEmpty();
+}
+
 QVector<IndexedPointCloudNode> QgsPointCloudDataProvider::getNodesIntersectingWithRay(
   const QgsPointCloudIndex *pc, IndexedPointCloudNode n,
-  const QVector3D &rayOrigin, const QVector3D &rayDirectione )
+  double maxError, double nodeError, double cameraFov, int screenSizePx,
+  const QVector3D &rayOrigin, const QVector3D &rayDirection )
 {
   QVector<IndexedPointCloudNode> nodes;
 
-  // TODO: check if the node actually intersects with the ray
+  QgsRectangle n2DExtent = pc->nodeMapExtent( n );
+  QgsDoubleRange zRange = pc->nodeZRange( n );
+  QgsBox3d box( n2DExtent.xMinimum(), n2DExtent.yMinimum(), zRange.lower(), n2DExtent.xMaximum(), n2DExtent.yMaximum(), zRange.upper() );
+
+  // calculate screen space error:
+  float distance = box.distanceFromPoint( rayOrigin.x(), rayOrigin.y(), rayOrigin.z() );
+  qDebug() << "Node error: " << nodeError;
+  float phi = nodeError * screenSizePx / ( 2 * distance * tan( cameraFov * M_PI / ( 2 * 180 ) ) );
+
+  if ( !__boxIntesects( box, rayOrigin, rayDirection ) )
+  {
+    return nodes;
+  }
 
   nodes.append( n );
 
+  float childrenError = nodeError / 2.0f;
+  if ( phi < maxError )
+    return nodes;
+
+  qDebug() << "Passed";
+
   const QList<IndexedPointCloudNode> children = pc->nodeChildren( n );
   for ( const IndexedPointCloudNode &nn : children )
   {
-    nodes += getNodesIntersectingWithRay( pc, nn, rayOrigin, rayDirectione );
+    nodes += getNodesIntersectingWithRay( pc, nn, maxError, childrenError, cameraFov, screenSizePx, rayOrigin, rayDirection );
   }
 
   return nodes;

diff --git a/src/core/pointcloud/qgspointclouddataprovider.h b/src/core/pointcloud/qgspointclouddataprovider.h
@@ -121,8 +121,8 @@ class CORE_EXPORT QgsPointCloudDataProvider: public QgsDataProvider
     }
     % End
 #endif
-    QVector<QMap<QString, QVariant>> getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection ) SIP_SKIP;
-    QVector<IndexedPointCloudNode> getNodesIntersectingWithRay( const QgsPointCloudIndex *pc, IndexedPointCloudNode n, const QVector3D &rayOrigin, const QVector3D &rayDirection );
+    QVector<QMap<QString, QVariant>> getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection, double maxScreenError, double cameraFov, int screenSizePx ) SIP_SKIP;
+    QVector<IndexedPointCloudNode> getNodesIntersectingWithRay( const QgsPointCloudIndex *pc, IndexedPointCloudNode n, double maxError, double nodeError, double cameraFov, int screenSizePx, const QVector3D &rayOrigin, const QVector3D &rayDirection );
 
     /**
      * Returns flags containing the supported capabilities for the data provider.

diff --git a/src/core/pointcloud/qgspointcloudlayer.cpp b/src/core/pointcloud/qgspointcloudlayer.cpp
@@ -622,9 +622,14 @@ void QgsPointCloudLayer::setRenderer( QgsPointCloudRenderer *renderer )
   emit styleChanged();
 }
 
-QVector<QMap<QString, QVariant>> QgsPointCloudLayer::getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection )
+QVector<QMap<QString, QVariant>> QgsPointCloudLayer::getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection, double maxScreenError, double cameraFov, int screenSizePx )
 {
-  QVector<QMap<QString, QVariant>> points = mDataProvider->getPointsOnRay( rayOrigin, rayDirection );
-  qDebug() << __PRETTY_FUNCTION__;
+  qDebug() << __PRETTY_FUNCTION__ << rayOrigin << " " << rayDirection;
+  QVector<QMap<QString, QVariant>> points;
+  QVector3D adjutedRayOrigin = QVector3D( rayOrigin.x(), rayOrigin.y(), ( rayOrigin.z() - mElevationProperties->zOffset() ) / mElevationProperties->zScale() );
+  QVector3D adjutedRayDirection = QVector3D( rayDirection.x(), rayDirection.y(), rayDirection.z() / mElevationProperties->zScale() );
+  adjutedRayDirection.normalize();
+
+  points = mDataProvider->getPointsOnRay( adjutedRayOrigin, adjutedRayDirection, maxScreenError, cameraFov, screenSizePx );
   return points;
 }
diff --git a/src/core/pointcloud/qgspointcloudlayer.h b/src/core/pointcloud/qgspointcloudlayer.h
@@ -167,7 +167,7 @@ class CORE_EXPORT QgsPointCloudLayer : public QgsMapLayer
      */
     void setRenderer( QgsPointCloudRenderer *renderer SIP_TRANSFER );
 
-    QVector<QMap<QString, QVariant>> getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection );
+    QVector<QMap<QString, QVariant>> getPointsOnRay( const QVector3D &rayOrigin, const QVector3D &rayDirection, double maxScreenError, double cameraFov, int screenSizePx );
   private slots:
     void onPointCloudIndexGenerationStateChanged( QgsPointCloudDataProvider::PointCloudIndexGenerationState state );