Skip to content

Commit 9d649e7

Browse files
lbartolettinyalldawson
lbartoletti
authored and
nyalldawson
committed
Add skewLines intersection algorithm
1 parent d87f75a commit 9d649e7

File tree

4 files changed

+302
-0
lines changed

4 files changed

+302
-0
lines changed

‎python/core/geometry/qgsgeometryutils.sip.in

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111

1212

1313

14+
1415
class QgsGeometryUtils
1516
{
1617
%Docstring
@@ -491,6 +492,76 @@ Return the coefficients (a, b, c for equation "ax + by + c = 0") of a line defin
491492
:return: A line (segment) from p to perpendicular point on segment [s1, s2]
492493
%End
493494

495+
496+
static double skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
497+
const QVector3D &P2, const QVector3D &P22 );
498+
%Docstring
499+
An algorithm to calculate the shortest distance between two skew lines.
500+
501+
:param P1: is the first point of the first line,
502+
:param P12: is the second point on the first line,
503+
:param P2: is the first point on the second line,
504+
:param P22: is the second point on the second line.
505+
506+
:return: the shortest distance
507+
%End
508+
509+
static bool skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
510+
const QVector3D &P2, const QVector3D &P22,
511+
QVector3D &X1 /Out/,
512+
double epsilon = 0.0001 );
513+
%Docstring
514+
A method to project one skew line onto another.
515+
516+
:param P1: is a first point that belonds to first skew line,
517+
:param P12: is the second point that belongs to first skew line,
518+
:param P2: is the first point that belongs to second skew line,
519+
:param P22: is the second point that belongs to second skew line,
520+
:param X1: is the result projection point of line P2P22 onto line P1P12.
521+
522+
:return: true if such point exists, false - otherwise.
523+
%End
524+
525+
static bool linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
526+
const QVector3D &Lb1, const QVector3D &Lb2,
527+
QVector3D &intersection /Out/ );
528+
%Docstring
529+
An algorithm to calculate an (approximate) intersection of two lines in 3D.
530+
531+
:param La1: is the first point on the first line,
532+
:param La2: is the second point on the first line,
533+
:param Lb1: is the first point on the second line,
534+
:param Lb2: is the second point on the second line,
535+
:param intersection: is the result intersection, of it can be found.
536+
537+
:return: true if the intersection can be found, false - otherwise.
538+
example:
539+
.. code-block:: python
540+
541+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,3,0))
542+
# (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
543+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,0,0))
544+
# (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
545+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,3,0))
546+
# (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
547+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,0,0))
548+
# (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
549+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,3,0))
550+
# (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
551+
QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,0,0))
552+
# (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
553+
QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,1,0), QVector3D(3,2,0))
554+
# (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
555+
QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,2,0), QVector3D(3,1,0))
556+
# (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
557+
QgsGeometryUtils.linesIntersection3D(QVector3D(5,5,5), QVector3D(0,0,0), QVector3D(0,5,5), QVector3D(5,0,0))
558+
# (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
559+
QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(0,5,0), QVector3D(2.5,2.5,2.5), QVector3D(5,0,0))
560+
# (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
561+
QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(5,0,0), QVector3D(0,5,5), QVector3D(5,5,5))
562+
# (True, PyQt5.QtGui.QVector3D(0.0, 5.0, 5.0))
563+
%End
564+
494565
static bool setZValueFromPoints( const QgsPointSequence &points, QgsPoint &point );
495566
%Docstring
496567
A Z dimension is added to ``point`` if one of the point in the list

‎src/core/geometry/qgsgeometryutils.cpp

Lines changed: 126 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -311,6 +311,7 @@ bool QgsGeometryUtils::segmentIntersection( const QgsPoint &p1, const QgsPoint &
311311

312312
double lambdaw = QgsVector( intersectionPoint.x() - q1.x(), intersectionPoint.y() - q1.y() ) * w;
313313
return !( lambdaw < 0. + tolerance || lambdaw >= wl - tolerance );
314+
314315
}
315316

316317
bool QgsGeometryUtils::lineCircleIntersection( const QgsPointXY &center, const double radius,
@@ -1330,6 +1331,131 @@ double QgsGeometryUtils::averageAngle( double a1, double a2 )
13301331
return normalizedAngle( resultAngle );
13311332
}
13321333

1334+
double QgsGeometryUtils::skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
1335+
const QVector3D &P2, const QVector3D &P22 )
1336+
{
1337+
QVector3D u1 = P12 - P1;
1338+
QVector3D u2 = P22 - P2;
1339+
QVector3D u3 = QVector3D::crossProduct( u1, u2 );
1340+
if ( u3.length() == 0 ) return 1;
1341+
u3.normalize();
1342+
QVector3D dir = P1 - P2;
1343+
return std::fabs( ( QVector3D::dotProduct( dir, u3 ) ) ); // u3 is already normalized
1344+
}
1345+
1346+
bool QgsGeometryUtils::skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
1347+
const QVector3D &P2, const QVector3D &P22,
1348+
QVector3D &X1, double epsilon )
1349+
{
1350+
QVector3D d = P2 - P1;
1351+
QVector3D u1 = P12 - P1;
1352+
u1.normalize();
1353+
QVector3D u2 = P22 - P2;
1354+
u2.normalize();
1355+
QVector3D u3 = QVector3D::crossProduct( u1, u2 );
1356+
1357+
if ( std::fabs( u3.x() ) <= epsilon &&
1358+
std::fabs( u3.y() ) <= epsilon &&
1359+
std::fabs( u3.z() ) <= epsilon )
1360+
{
1361+
// The rays are almost parallel.
1362+
return false;
1363+
}
1364+
1365+
// X1 and X2 are the closest points on lines
1366+
// we want to find X1 (lies on u1)
1367+
// solving the linear equation in r1 and r2: Xi = Pi + ri*ui
1368+
// we are only interested in X1 so we only solve for r1.
1369+
float a1 = QVector3D::dotProduct( u1, u1 ), b1 = QVector3D::dotProduct( u1, u2 ), c1 = QVector3D::dotProduct( u1, d );
1370+
float a2 = QVector3D::dotProduct( u1, u2 ), b2 = QVector3D::dotProduct( u2, u2 ), c2 = QVector3D::dotProduct( u2, d );
1371+
if ( !( std::fabs( b1 ) > epsilon ) )
1372+
{
1373+
// Denominator is close to zero.
1374+
return false;
1375+
}
1376+
if ( !( a2 != -1 && a2 != 1 ) )
1377+
{
1378+
// Lines are parallel
1379+
return false;
1380+
}
1381+
1382+
double r1 = ( c2 - b2 * c1 / b1 ) / ( a2 - b2 * a1 / b1 );
1383+
X1 = P1 + u1 * r1;
1384+
1385+
return true;
1386+
}
1387+
1388+
bool QgsGeometryUtils::linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
1389+
const QVector3D &Lb1, const QVector3D &Lb2,
1390+
QVector3D &intersection )
1391+
{
1392+
1393+
// if all Vector are on the same plane (have the same Z), use the 2D intersection
1394+
// else return a false result
1395+
if ( qgsDoubleNear( La1.z(), La2.z() ) && qgsDoubleNear( La1.z(), Lb1.z() ) && qgsDoubleNear( La1.z(), Lb2.z() ) )
1396+
{
1397+
QgsPoint ptInter;
1398+
bool isIntersection;
1399+
segmentIntersection( QgsPoint( La1.x(), La1.y() ),
1400+
QgsPoint( La2.x(), La2.y() ),
1401+
QgsPoint( Lb1.x(), Lb1.y() ),
1402+
QgsPoint( Lb2.x(), Lb2.y() ),
1403+
ptInter,
1404+
isIntersection,
1405+
1e-8,
1406+
true );
1407+
intersection.setX( ptInter.x() );
1408+
intersection.setY( ptInter.y() );
1409+
intersection.setZ( La1.z() );
1410+
return true;
1411+
}
1412+
1413+
// first check if lines have an exact intersection point
1414+
// do it by checking if the shortest distance is exactly 0
1415+
float distance = skewLinesDistance( La1, La2, Lb1, Lb2 );
1416+
if ( qgsDoubleNear( distance, 0.0 ) )
1417+
{
1418+
// 3d lines have exact intersection point.
1419+
QVector3D C = La2;
1420+
QVector3D D = Lb2;
1421+
QVector3D e = La1 - La2;
1422+
QVector3D f = Lb1 - Lb2;
1423+
QVector3D g = D - C;
1424+
if ( qgsDoubleNear( ( QVector3D::crossProduct( f, g ) ).length(), 0.0 ) || qgsDoubleNear( ( QVector3D::crossProduct( f, e ) ).length(), 0.0 ) )
1425+
{
1426+
// Lines have no intersection, are they parallel?
1427+
return false;
1428+
}
1429+
1430+
QVector3D fgn = QVector3D::crossProduct( f, g );
1431+
fgn.normalize();
1432+
1433+
QVector3D fen = QVector3D::crossProduct( f, e );
1434+
fen.normalize();
1435+
1436+
int di = -1;
1437+
if ( fgn == fen ) // same direction?
1438+
di *= -1;
1439+
1440+
intersection = C + e * di * ( QVector3D::crossProduct( f, g ).length() / QVector3D::crossProduct( f, e ).length() );
1441+
return true;
1442+
}
1443+
1444+
// try to calculate the approximate intersection point
1445+
QVector3D X1, X2;
1446+
bool firstIsDone = skewLinesProjection( La1, La2, Lb1, Lb2, X1 );
1447+
bool secondIsDone = skewLinesProjection( Lb1, Lb2, La1, La2, X2 );
1448+
1449+
if ( !firstIsDone || !secondIsDone )
1450+
{
1451+
// Could not obtain projection point.
1452+
return false;
1453+
}
1454+
1455+
intersection = ( X1 + X2 ) / 2.0;
1456+
return true;
1457+
}
1458+
13331459
bool QgsGeometryUtils::setZValueFromPoints( const QgsPointSequence &points, QgsPoint &point )
13341460
{
13351461
bool rc = false;

‎src/core/geometry/qgsgeometryutils.h

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,8 @@ email : marco.hugentobler at sourcepole dot com
2424
#include "qgsabstractgeometry.h"
2525

2626

27+
#include <QVector3D>
28+
2729
class QgsLineString;
2830

2931
/**
@@ -511,7 +513,71 @@ class CORE_EXPORT QgsGeometryUtils
511513
*/
512514
static QgsLineString perpendicularSegment( const QgsPoint &p, const QgsPoint &s1, const QgsPoint &s2 );
513515

516+
517+
/**
518+
* An algorithm to calculate the shortest distance between two skew lines.
519+
* \param P1 is the first point of the first line,
520+
* \param P12 is the second point on the first line,
521+
* \param P2 is the first point on the second line,
522+
* \param P22 is the second point on the second line.
523+
* \return the shortest distance
524+
*/
525+
static double skewLinesDistance( const QVector3D &P1, const QVector3D &P12,
526+
const QVector3D &P2, const QVector3D &P22 );
527+
514528
/**
529+
* A method to project one skew line onto another.
530+
* \param P1 is a first point that belonds to first skew line,
531+
* \param P12 is the second point that belongs to first skew line,
532+
* \param P2 is the first point that belongs to second skew line,
533+
* \param P22 is the second point that belongs to second skew line,
534+
* \param X1 is the result projection point of line P2P22 onto line P1P12.
535+
* \return true if such point exists, false - otherwise.
536+
*/
537+
static bool skewLinesProjection( const QVector3D &P1, const QVector3D &P12,
538+
const QVector3D &P2, const QVector3D &P22,
539+
QVector3D &X1 SIP_OUT,
540+
double epsilon = 0.0001 );
541+
542+
/**
543+
* An algorithm to calculate an (approximate) intersection of two lines in 3D.
544+
* \param La1 is the first point on the first line,
545+
* \param La2 is the second point on the first line,
546+
* \param Lb1 is the first point on the second line,
547+
* \param Lb2 is the second point on the second line,
548+
* \param intersection is the result intersection, of it can be found.
549+
* \return true if the intersection can be found, false - otherwise.
550+
* example:
551+
* \code{.py}
552+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,3,0))
553+
* # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
554+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(2,1,0), QVector3D(2,0,0))
555+
* # (True, PyQt5.QtGui.QVector3D(2.0, 0.0, 0.0))
556+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,3,0))
557+
* # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
558+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(0,1,0), QVector3D(0,0,0))
559+
* # (True, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
560+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,3,0))
561+
* # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
562+
* QgsGeometryUtils.linesIntersection3D(QVector3D(0,0,0), QVector3D(5,0,0), QVector3D(5,1,0), QVector3D(5,0,0))
563+
* # (False, PyQt5.QtGui.QVector3D(0.0, 0.0, 0.0))
564+
* QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,1,0), QVector3D(3,2,0))
565+
* # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
566+
* QgsGeometryUtils.linesIntersection3D(QVector3D(1,1,0), QVector3D(2,2,0), QVector3D(3,2,0), QVector3D(3,1,0))
567+
* # (True, PyQt5.QtGui.QVector3D(3.0, 3.0, 0.0))
568+
* QgsGeometryUtils.linesIntersection3D(QVector3D(5,5,5), QVector3D(0,0,0), QVector3D(0,5,5), QVector3D(5,0,0))
569+
* # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
570+
* QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(0,5,0), QVector3D(2.5,2.5,2.5), QVector3D(5,0,0))
571+
* # (True, PyQt5.QtGui.QVector3D(2.5, 2.5, 2.5))
572+
* QgsGeometryUtils.linesIntersection3D(QVector3D(2.5,2.5,2.5), QVector3D(5,0,0), QVector3D(0,5,5), QVector3D(5,5,5))
573+
* # (True, PyQt5.QtGui.QVector3D(0.0, 5.0, 5.0))
574+
* \endcode
575+
*/
576+
static bool linesIntersection3D( const QVector3D &La1, const QVector3D &La2,
577+
const QVector3D &Lb1, const QVector3D &Lb2,
578+
QVector3D &intersection SIP_OUT );
579+
580+
/*
515581
* A Z dimension is added to \a point if one of the point in the list
516582
* \a points is in 3D. Moreover, the Z value of \a point is updated with.
517583
*

‎tests/src/core/testqgsgeometryutils.cpp

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ class TestQgsGeometryUtils: public QObject
5757
void testCoefficients();
5858
void testPerpendicularSegment();
5959
void testClosestPoint();
60+
void testlinesIntersection3D();
6061
void testSegmentIntersection();
6162
void testLineCircleIntersection();
6263
void testCircleCircleIntersection();
@@ -668,6 +669,44 @@ void TestQgsGeometryUtils::testClosestPoint()
668669
QGSCOMPARENEAR( pt4.m(), 1, 0.0001 );
669670
}
670671

672+
void TestQgsGeometryUtils::testlinesIntersection3D()
673+
{
674+
QVector3D x;
675+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 2, 1, 10 ), QVector3D( 2, 3, 10 ), x ) );
676+
QVERIFY( x == QVector3D( 2.0, 0.0, 10.0 ) );
677+
678+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 2, 1, 10 ), QVector3D( 2, 0, 10 ), x ) );
679+
QVERIFY( x == QVector3D( 2.0, 0.0, 10.0 ) );
680+
681+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 0, 1, 10 ), QVector3D( 0, 3, 10 ), x ) );
682+
QVERIFY( x == QVector3D( 0.0, 0.0, 10.0 ) );
683+
684+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 0, 1, 10 ), QVector3D( 0, 0, 10 ), x ) );
685+
QVERIFY( x == QVector3D( 0.0, 0.0, 10.0 ) );
686+
687+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 5, 1, 10 ), QVector3D( 5, 3, 10 ), x ) );
688+
QVERIFY( x == QVector3D( 5.0, 0.0, 10.0 ) );
689+
690+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 0, 0, 10 ), QVector3D( 5, 0, 10 ), QVector3D( 5, 1, 10 ), QVector3D( 5, 0, 10 ), x ) );
691+
QVERIFY( x == QVector3D( 5.0, 0.0, 10.0 ) );
692+
693+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 1, 1, 10 ), QVector3D( 2, 2, 10 ), QVector3D( 3, 1, 10 ), QVector3D( 3, 2, 10 ), x ) );
694+
QVERIFY( x == QVector3D( 3.0, 3.0, 10.0 ) );
695+
696+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 1, 1, 10 ), QVector3D( 2, 2, 10 ), QVector3D( 3, 2, 10 ), QVector3D( 3, 1, 10 ), x ) );
697+
QVERIFY( x == QVector3D( 3.0, 3.0, 10.0 ) );
698+
699+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 5, 5, 5 ), QVector3D( 0, 0, 0 ), QVector3D( 0, 5, 5 ), QVector3D( 5, 0, 0 ), x ) );
700+
QVERIFY( x == QVector3D( 2.5, 2.5, 2.5 ) );
701+
702+
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 ) );
703+
QVERIFY( x == QVector3D( 2.5, 2.5, 2.5 ) );
704+
705+
QVERIFY( QgsGeometryUtils::linesIntersection3D( QVector3D( 2.5, 2.5, 2.5 ), QVector3D( 5, 0, 0 ), QVector3D( 0, 5, 5 ), QVector3D( 5, 5, 5 ), x ) );
706+
QVERIFY( x == QVector3D( 0.0, 5.0, 5.0 ) );
707+
708+
}
709+
671710
void TestQgsGeometryUtils::testSegmentIntersection()
672711
{
673712
const double epsilon = 1e-8;

0 commit comments

Comments
 (0)