qgsraycastingutils.cpp

Go to the documentation of this file.

/***************************************************************************
  qgsraycastingutils_h.cpp
  --------------------------------------
  Date                 : June 2018
  Copyright            : (C) 2018 by Martin Dobias
  Email                : wonder dot sk at gmail dot com
 ***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/
 
#include "qgsraycastingutils.h"
 
#include "qgsaabb.h"
#include "qgslogger.h"
#include "qgsray3d.h"
 
#include <Qt3DCore/QAttribute>
#include <Qt3DCore/QBuffer>
#include <Qt3DCore/QGeometry>
#include <Qt3DRender/QGeometryRenderer>

 
 
namespace QgsRayCastingUtils
{
  bool rayBoxIntersection( const QgsRay3D &ray, const QgsAABB &nodeBbox )
  {
    // https://tavianator.com/fast-branchless-raybounding-box-intersections/
    // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
 
    const QVector3D dirInv = ray.directionInversed();
 
    double boxXMax = nodeBbox.xMax;
    double boxYMax = nodeBbox.yMax;
    double boxZMax = nodeBbox.zMax;
 
    // intersection() does not like yMin==yMax (excludes borders)
    if ( boxXMax == nodeBbox.xMin )
      boxXMax += 0.1;
    if ( boxYMax == nodeBbox.yMin )
      boxYMax += 0.1;
    if ( boxZMax == nodeBbox.zMin )
      boxZMax += 0.1;
 
 
    double t1 = ( nodeBbox.xMin - ray.origin().x() ) * dirInv.x();
    double t2 = ( boxXMax - ray.origin().x() ) * dirInv.x();
    double tmin = std::min( t1, t2 );
    double tmax = std::max( t1, t2 );
 
    t1 = ( nodeBbox.yMin - ray.origin().y() ) * dirInv.y();
    t2 = ( boxYMax - ray.origin().y() ) * dirInv.y();
    tmin = std::max( tmin, std::min( std::min( t1, t2 ), tmax ) );
    tmax = std::min( tmax, std::max( std::max( t1, t2 ), tmin ) );
 
    t1 = ( nodeBbox.zMin - ray.origin().z() ) * dirInv.z();
    t2 = ( boxZMax - ray.origin().z() ) * dirInv.z();
    tmin = std::max( tmin, std::min( std::min( t1, t2 ), tmax ) );
    tmax = std::min( tmax, std::max( std::max( t1, t2 ), tmin ) );
 
    return tmax > std::max( tmin, 0.0 );
  }
 
 
  // copied from intersectsSegmentTriangle() from qt3d/src/render/backend/triangleboundingvolume.cpp
  // by KDAB, licensed under the terms of LGPL
  bool rayTriangleIntersection( const QgsRay3D &ray, float maxDist, const QVector3D &a, const QVector3D &b, const QVector3D &c, QVector3D &uvw, float &t )
  {
    // Note: a, b, c in clockwise order
    // RealTime Collision Detection page 192
 
    const QVector3D ab = b - a;
    const QVector3D ac = c - a;
    const QVector3D qp = ( ray.origin() - ray.point( maxDist ) );
 
    const QVector3D n = QVector3D::crossProduct( ab, ac );
    const float d = QVector3D::dotProduct( qp, n );
 
    if ( d <= 0.0f || std::isnan( d ) )
      return false;
 
    const QVector3D ap = ray.origin() - a;
    t = QVector3D::dotProduct( ap, n );
 
    if ( t < 0.0f || t > d )
      return false;
 
    const QVector3D e = QVector3D::crossProduct( qp, ap );
    uvw.setY( QVector3D::dotProduct( ac, e ) );
 
    if ( uvw.y() < 0.0f || uvw.y() > d )
      return false;
 
    uvw.setZ( -QVector3D::dotProduct( ab, e ) );
 
    if ( uvw.z() < 0.0f || uvw.y() + uvw.z() > d )
      return false;
 
    const float ood = 1.0f / d;
    t *= ood;
    uvw.setY( uvw.y() * ood );
    uvw.setZ( uvw.z() * ood );
    uvw.setX( 1.0f - uvw.y() - uvw.z() );
 
    return true;
  }
 
  bool rayMeshIntersection( Qt3DRender::QGeometryRenderer *geometryRenderer, const QgsRay3D &r, float maxDist, const QMatrix4x4 &worldTransform, QVector3D &intPt, int &triangleIndex )
  {
    if ( geometryRenderer->primitiveType() != Qt3DRender::QGeometryRenderer::Triangles )
    {
      QgsDebugError( QString( "Unsupported primitive type for intersection: " ).arg( geometryRenderer->primitiveType() ) );
      return false;
    }
    if ( geometryRenderer->instanceCount() != 1
         || geometryRenderer->indexOffset() != 0
         || geometryRenderer->indexBufferByteOffset() != 0
         || geometryRenderer->firstVertex() != 0
         || geometryRenderer->firstInstance() != 0 )
    {
      QgsDebugError( QString( "Unsupported geometry renderer for intersection." ) );
      return false;
    }
 
    Qt3DCore::QGeometry *geometry = geometryRenderer->geometry();
 
    Qt3DCore::QAttribute *positionAttr = nullptr;
    Qt3DCore::QAttribute *indexAttr = nullptr;
    for ( Qt3DCore::QAttribute *attr : geometry->attributes() )
    {
      if ( attr->name() == Qt3DCore::QAttribute::defaultPositionAttributeName() )
      {
        positionAttr = attr;
      }
      else if ( attr->attributeType() == Qt3DCore::QAttribute::IndexAttribute )
      {
        indexAttr = attr;
      }
    }
 
    if ( !positionAttr )
    {
      QgsDebugError( "Could not find position attribute!" );
      return false;
    }
 
    if ( positionAttr->vertexBaseType() != Qt3DCore::QAttribute::Float || positionAttr->vertexSize() != 3 )
    {
      QgsDebugError( QString( "Unsupported position attribute: base type %1, vertex size %2" ).arg( positionAttr->vertexBaseType() ).arg( positionAttr->vertexSize() ) );
      return false;
    }
 
    const QByteArray vertexBuf = positionAttr->buffer()->data();
    const char *vertexPtr = vertexBuf.constData();
    vertexPtr += positionAttr->byteOffset();
    int vertexByteStride = positionAttr->byteStride() == 0 ? 3 * sizeof( float ) : positionAttr->byteStride();
 
    const uchar *indexPtrUChar = nullptr;
    const ushort *indexPtrUShort = nullptr;
    const uint *indexPtrUInt = nullptr;
    if ( indexAttr )
    {
      if ( indexAttr->byteStride() != 0 || indexAttr->vertexSize() != 1 )
      {
        QgsDebugError( QString( "Unsupported index attribute: stride %1, vertex size %2" ).arg( indexAttr->byteStride() ).arg( indexAttr->vertexSize() ) );
        return false;
      }
 
      const QByteArray indexBuf = indexAttr->buffer()->data();
      if ( indexAttr->vertexBaseType() == Qt3DCore::QAttribute::UnsignedByte )
      {
        indexPtrUChar = reinterpret_cast<const uchar *>( indexBuf.constData() + indexAttr->byteOffset() );
      }
      else if ( indexAttr->vertexBaseType() == Qt3DCore::QAttribute::UnsignedShort )
      {
        indexPtrUShort = reinterpret_cast<const ushort *>( indexBuf.constData() + indexAttr->byteOffset() );
      }
      else if ( indexAttr->vertexBaseType() == Qt3DCore::QAttribute::UnsignedInt )
      {
        indexPtrUInt = reinterpret_cast<const uint *>( indexBuf.constData() + indexAttr->byteOffset() );
      }
      else
      {
        QgsDebugError( QString( "Unsupported index attribute: base type %1" ).arg( indexAttr->vertexBaseType() ) );
        return false;
      }
    }
 
    int vertexCount = geometryRenderer->vertexCount();
    if ( vertexCount == 0 && indexAttr )
    {
      vertexCount = indexAttr->count();
    }
    if ( vertexCount == 0 )
    {
      vertexCount = positionAttr->count();
    }
 
    QVector3D intersectionPt, minIntersectionPt;
    float minDistance = -1;
 
    for ( int i = 0; i < vertexCount; i += 3 )
    {
      int v0index = 0, v1index = 0, v2index = 0;
      if ( !indexAttr )
      {
        v0index = i;
        v1index = i + 1;
        v2index = i + 2;
      }
      else if ( indexPtrUShort )
      {
        v0index = indexPtrUShort[i];
        v1index = indexPtrUShort[i + 1];
        v2index = indexPtrUShort[i + 2];
      }
      else if ( indexPtrUChar )
      {
        v0index = indexPtrUChar[i];
        v1index = indexPtrUChar[i + 1];
        v2index = indexPtrUChar[i + 2];
      }
      else if ( indexPtrUInt )
      {
        v0index = indexPtrUInt[i];
        v1index = indexPtrUInt[i + 1];
        v2index = indexPtrUInt[i + 2];
      }
      else
        Q_ASSERT( false );
 
      const float *v0ptr = reinterpret_cast<const float *>( vertexPtr + v0index * vertexByteStride );
      const float *v1ptr = reinterpret_cast<const float *>( vertexPtr + v1index * vertexByteStride );
      const float *v2ptr = reinterpret_cast<const float *>( vertexPtr + v2index * vertexByteStride );
 
      const QVector3D a( v0ptr[0], v0ptr[1], v0ptr[2] );
      const QVector3D b( v1ptr[0], v1ptr[1], v1ptr[2] );
      const QVector3D c( v2ptr[0], v2ptr[1], v2ptr[2] );
 
      // Currently the worldTransform only has vertical offset, so this could be optimized by applying the transform
      // to the ray and the resulting intersecting point instead of all triangles
      // Need to check for potential performance gains.
      const QVector3D tA = worldTransform * a;
      const QVector3D tB = worldTransform * b;
      const QVector3D tC = worldTransform * c;
 
      QVector3D uvw;
      float t = 0;
 
      // We're testing both triangle orientations here and ignoring the culling mode.
      // We should probably respect the culling mode used for the entity and perform a
      // single test using the properly oriented triangle.
      if ( QgsRayCastingUtils::rayTriangleIntersection( r, maxDist, tA, tB, tC, uvw, t ) || QgsRayCastingUtils::rayTriangleIntersection( r, maxDist, tA, tC, tB, uvw, t ) )
      {
        intersectionPt = r.point( t * maxDist );
        const float distance = r.projectedDistance( intersectionPt );
 
        // we only want the first intersection of the ray with the mesh (closest to the ray origin)
        if ( minDistance == -1 || distance < minDistance )
        {
          triangleIndex = static_cast<int>( i / 3 );
          minDistance = distance;
          minIntersectionPt = intersectionPt;
        }
      }
    }
 
    if ( minDistance != -1 )
    {
      intPt = minIntersectionPt;
      return true;
    }
    else
      return false;
  }
} // namespace QgsRayCastingUtils