Add skewLines intersection algorithm

python/core/geometry/qgsgeometryutils.sip.in

@@ -11,6 +11,7 @@
 
 
 
+
 class QgsGeometryUtils
 {
 %Docstring
@@ -491,6 +492,76 @@ Return the coefficients (a, b, c for equation "ax + by + c = 0") of a line defin
 :return: A line (segment) from p to perpendicular point on segment [s1, s2]
 %End
 
+
+    static double skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
+                                     const QVector3D &P2, const QVector3D &P22 );
+%Docstring
+An algorithm to calculate the shortest distance between two skew lines.
+
+:param P1: is the first point of the first line,
+:param P12: is the second point on the first line,
+:param P2: is the first point on the second line,
+:param P22: is the second point on the second line.
+
+:return: the shortest distance
+%End
+
+    static bool skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
+                                     const QVector3D &P2, const QVector3D &P22,
+                                     QVector3D &X1  /Out/,
+                                     double epsilon = 0.0001 );
+%Docstring
+A method to project one skew line onto another.
+
+:param P1: is a first point that belonds to first skew line,
+:param P12: is the second point that belongs to first skew line,
+:param P2: is the first point that belongs to second skew line,
+:param P22: is the second point that belongs to second skew line,
+:param X1: is the result projection point of line P2P22 onto line P1P12.
+
+:return: true if such point exists, false - otherwise.
+%End
+
+    static bool linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
+                                     const QVector3D &Lb1, const QVector3D &Lb2,
+                                     QVector3D &intersection  /Out/ );
+%Docstring
+An algorithm to calculate an (approximate) intersection of two lines in 3D.
+
+:param La1: is the first point on the first line,
+:param La2: is the second point on the first line,
+:param Lb1: is the first point on the second line,
+:param Lb2: is the second point on the second line,
+:param intersection: is the result intersection, of it can be found.
+
+:return: true if the intersection can be found, false - otherwise.
+example:
+.. code-block:: python
+
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,3,0))
+       # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,0,0))
+       # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,3,0))
+       # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,0,0))
+       # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,3,0))
+       # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,0,0))
+       # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,1,0), QVector3D(3,2,0))
+       # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,2,0), QVector3D(3,1,0))
+       # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(5,5,5), QVector3D(0,0,0), QVector3D(0,5,5), QVector3D(5,0,0))
+       # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(0,5,0), QVector3D(2.5,2.5,2.5), QVector3D(5,0,0))
+       # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
+       QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(5,0,0), QVector3D(0,5,5), QVector3D(5,5,5))
+       # (True, PyQt5.QtGui.QVector3D(0.0, 5.0, 5.0))
+%End
+
     static bool setZValueFromPoints( const QgsPointSequence &points, QgsPoint &point );
 %Docstring
 A Z dimension is added to ``point`` if one of the point in the list

src/core/geometry/qgsgeometryutils.cpp

@@ -311,6 +311,7 @@ bool QgsGeometryUtils::segmentIntersection( const QgsPoint &p1, const QgsPoint &
 
   double lambdaw = QgsVector( intersectionPoint.x() - q1.x(), intersectionPoint.y() - q1.y() ) * w;
   return !( lambdaw < 0. + tolerance || lambdaw >= wl - tolerance );
+
 }
 
 bool QgsGeometryUtils::lineCircleIntersection( const QgsPointXY &center, const double radius,
@@ -1330,6 +1331,131 @@ double QgsGeometryUtils::averageAngle( double a1, double a2 )
   return normalizedAngle( resultAngle );
 }
 
+double QgsGeometryUtils::skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
+    const QVector3D &P2, const QVector3D &P22 )
+{
+  QVector3D u1 = P12 - P1;
+  QVector3D u2 = P22 - P2;
+  QVector3D u3 = QVector3D::crossProduct( u1, u2 );
+  if ( u3.length() == 0 ) return 1;
+  u3.normalize();
+  QVector3D dir = P1 - P2;
+  return std::fabs( ( QVector3D::dotProduct( dir, u3 ) ) ); // u3 is already normalized
+}
+
+bool QgsGeometryUtils::skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
+    const QVector3D &P2, const QVector3D &P22,
+    QVector3D &X1, double epsilon )
+{
+  QVector3D d = P2 - P1;
+  QVector3D u1 = P12 - P1;
+  u1.normalize();
+  QVector3D u2 = P22 - P2;
+  u2.normalize();
+  QVector3D u3 = QVector3D::crossProduct( u1, u2 );
+
+  if ( std::fabs( u3.x() ) <= epsilon &&
+       std::fabs( u3.y() ) <= epsilon &&
+       std::fabs( u3.z() ) <= epsilon )
+  {
+    // The rays are almost parallel.
+    return false;
+  }
+
+  // X1 and X2 are the closest points on lines
+  // we want to find X1 (lies on u1)
+  // solving the linear equation in r1 and r2: Xi = Pi + ri*ui
+  // we are only interested in X1 so we only solve for r1.
+  float a1 = QVector3D::dotProduct( u1, u1 ), b1 = QVector3D::dotProduct( u1, u2 ), c1 = QVector3D::dotProduct( u1, d );
+  float a2 = QVector3D::dotProduct( u1, u2 ), b2 = QVector3D::dotProduct( u2, u2 ), c2 = QVector3D::dotProduct( u2, d );
+  if ( !( std::fabs( b1 ) > epsilon ) )
+  {
+    // Denominator is close to zero.
+    return false;
+  }
+  if ( !( a2 != -1 && a2 != 1 ) )
+  {
+    // Lines are parallel
+    return false;
+  }
+
+  double r1 = ( c2 - b2 * c1 / b1 ) / ( a2 - b2 * a1 / b1 );
+  X1 = P1 + u1 * r1;
+
+  return true;
+}
+
+bool QgsGeometryUtils::linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
+    const QVector3D &Lb1, const QVector3D &Lb2,
+    QVector3D &intersection )
+{
+
+  // if all Vector are on the same plane (have the same Z), use the 2D intersection
+  // else return a false result
+  if ( qgsDoubleNear( La1.z(), La2.z() ) && qgsDoubleNear( La1.z(), Lb1.z() ) && qgsDoubleNear( La1.z(), Lb2.z() ) )
+  {
+    QgsPoint ptInter;
+    bool isIntersection;
+    segmentIntersection( QgsPoint( La1.x(), La1.y() ),
+                         QgsPoint( La2.x(), La2.y() ),
+                         QgsPoint( Lb1.x(), Lb1.y() ),
+                         QgsPoint( Lb2.x(), Lb2.y() ),
+                         ptInter,
+                         isIntersection,
+                         1e-8,
+                         true );
+    intersection.setX( ptInter.x() );
+    intersection.setY( ptInter.y() );
+    intersection.setZ( La1.z() );
+    return true;
+  }
+
+  // first check if lines have an exact intersection point
+  // do it by checking if the shortest distance is exactly 0
+  float distance = skewLinesDistance( La1, La2, Lb1, Lb2 );
+  if ( qgsDoubleNear( distance, 0.0 ) )
+  {
+    // 3d lines have exact intersection point.
+    QVector3D C = La2;
+    QVector3D D = Lb2;
+    QVector3D e = La1 - La2;
+    QVector3D f = Lb1 - Lb2;
+    QVector3D g = D - C;
+    if ( qgsDoubleNear( ( QVector3D::crossProduct( f, g ) ).length(), 0.0 ) || qgsDoubleNear( ( QVector3D::crossProduct( f, e ) ).length(), 0.0 ) )
+    {
+      // Lines have no intersection, are they parallel?
+      return false;
+    }
+
+    QVector3D fgn = QVector3D::crossProduct( f, g );
+    fgn.normalize();
+
+    QVector3D fen = QVector3D::crossProduct( f, e );
+    fen.normalize();
+
+    int di = -1;
+    if ( fgn == fen ) // same direction?
+      di *= -1;
+
+    intersection = C + e * di * ( QVector3D::crossProduct( f, g ).length() / QVector3D::crossProduct( f, e ).length() );
+    return true;
+  }
+
+  // try to calculate the approximate intersection point
+  QVector3D X1, X2;
+  bool firstIsDone = skewLinesProjection( La1, La2, Lb1, Lb2, X1 );
+  bool secondIsDone = skewLinesProjection( Lb1, Lb2, La1, La2, X2 );
+
+  if ( !firstIsDone || !secondIsDone )
+  {
+    // Could not obtain projection point.
+    return false;
+  }
+
+  intersection = ( X1 + X2 ) / 2.0;
+  return true;
+}
+
 bool QgsGeometryUtils::setZValueFromPoints( const QgsPointSequence &points, QgsPoint &point )
 {
   bool rc = false;

src/core/geometry/qgsgeometryutils.h

@@ -24,6 +24,8 @@ email                : marco.hugentobler at sourcepole dot com
 #include "qgsabstractgeometry.h"
 
 
+#include <QVector3D>
+
 class QgsLineString;
 
 /**
@@ -511,7 +513,71 @@ class CORE_EXPORT QgsGeometryUtils
      */
     static QgsLineString perpendicularSegment( const QgsPoint &p, const QgsPoint &s1, const QgsPoint &s2 );
 
+
+    /**
+     * An algorithm to calculate the shortest distance between two skew lines.
+     * \param P1 is the first point of the first line,
+     * \param P12 is the second point on the first line,
+     * \param P2 is the first point on the second line,
+     * \param P22 is the second point on the second line.
+     * \return the shortest distance
+     */
+    static double skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
+                                     const QVector3D &P2, const QVector3D &P22 );
+
     /**
+     * A method to project one skew line onto another.
+     * \param P1 is a first point that belonds to first skew line,
+     * \param P12 is the second point that belongs to first skew line,
+     * \param P2 is the first point that belongs to second skew line,
+     * \param P22 is the second point that belongs to second skew line,
+     * \param X1 is the result projection point of line P2P22 onto line P1P12.
+     * \return true if such point exists, false - otherwise.
+     */
+    static bool skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
+                                     const QVector3D &P2, const QVector3D &P22,
+                                     QVector3D &X1  SIP_OUT,
+                                     double epsilon = 0.0001 );
+
+    /**
+     * An algorithm to calculate an (approximate) intersection of two lines in 3D.
+     * \param La1 is the first point on the first line,
+     * \param La2 is the second point on the first line,
+     * \param Lb1 is the first point on the second line,
+     * \param Lb2 is the second point on the second line,
+     * \param intersection is the result intersection, of it can be found.
+     * \return true if the intersection can be found, false - otherwise.
+     * example:
+     * \code{.py}
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,3,0))
+     *   # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,0,0))
+     *   # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,3,0))
+     *   # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,0,0))
+     *   # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,3,0))
+     *   # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,0,0))
+     *   # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,1,0), QVector3D(3,2,0))
+     *   # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,2,0), QVector3D(3,1,0))
+     *   # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(5,5,5), QVector3D(0,0,0), QVector3D(0,5,5), QVector3D(5,0,0))
+     *   # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(0,5,0), QVector3D(2.5,2.5,2.5), QVector3D(5,0,0))
+     *   # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
+     *   QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(5,0,0), QVector3D(0,5,5), QVector3D(5,5,5))
+     *   # (True, PyQt5.QtGui.QVector3D(0.0, 5.0, 5.0))
+     *   \endcode
+     */
+    static bool linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
+                                     const QVector3D &Lb1, const QVector3D &Lb2,
+                                     QVector3D &intersection  SIP_OUT );
+
+    /*
      * A Z dimension is added to \a point if one of the point in the list
      * \a points is in 3D. Moreover, the Z value of \a point is updated with.
      *

tests/src/core/testqgsgeometryutils.cpp

@@ -57,6 +57,7 @@ class TestQgsGeometryUtils: public QObject
     void testCoefficients();
     void testPerpendicularSegment();
     void testClosestPoint();
+    void testlinesIntersection3D();
     void testSegmentIntersection();
     void testLineCircleIntersection();
     void testCircleCircleIntersection();
@@ -668,6 +669,44 @@ void TestQgsGeometryUtils::testClosestPoint()
   QGSCOMPARENEAR( pt4.m(), 1, 0.0001 );
 }
 
+void TestQgsGeometryUtils::testlinesIntersection3D()
+{
+  QVector3D x;
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 2, 1, 10 ), QVector3D( 2, 3, 10 ), x ) );
+  QVERIFY( x == QVector3D( 2.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 2, 1, 10 ), QVector3D( 2, 0, 10 ), x ) );
+  QVERIFY( x == QVector3D( 2.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 0, 1, 10 ), QVector3D( 0, 3, 10 ), x ) );
+  QVERIFY( x == QVector3D( 0.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 0, 1, 10 ), QVector3D( 0, 0, 10 ), x ) );
+  QVERIFY( x == QVector3D( 0.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 5, 1, 10 ), QVector3D( 5, 3, 10 ), x ) );
+  QVERIFY( x == QVector3D( 5.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 5, 1, 10 ), QVector3D( 5, 0, 10 ), x ) );
+  QVERIFY( x == QVector3D( 5.0, 0.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 1, 1, 10 ), QVector3D( 2, 2, 10 ), QVector3D( 3, 1, 10 ), QVector3D( 3, 2, 10 ), x ) );
+  QVERIFY( x == QVector3D( 3.0, 3.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 1, 1, 10 ), QVector3D( 2, 2, 10 ), QVector3D( 3, 2, 10 ), QVector3D( 3, 1, 10 ), x ) );
+  QVERIFY( x == QVector3D( 3.0, 3.0, 10.0 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 5, 5, 5 ), QVector3D( 0, 0, 0 ), QVector3D( 0, 5, 5 ), QVector3D( 5, 0, 0 ), x ) );
+  QVERIFY( x == QVector3D( 2.5, 2.5, 2.5 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 2.5, 2.5, 2.5 ), QVector3D( 0, 5, 0 ), QVector3D( 2.5, 2.5, 2.5 ), QVector3D( 5, 0, 0 ), x ) );
+  QVERIFY( x == QVector3D( 2.5, 2.5, 2.5 ) );
+
+  QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 2.5, 2.5, 2.5 ), QVector3D( 5, 0, 0 ), QVector3D( 0, 5, 5 ), QVector3D( 5, 5, 5 ), x ) );
+  QVERIFY( x == QVector3D( 0.0, 5.0, 5.0 ) );
+
+}
+
 void TestQgsGeometryUtils::testSegmentIntersection()
 {
   const double epsilon = 1e-8;