Skip to content

Commit

Permalink
Add skewLines intersection algorithm
Browse files Browse the repository at this point in the history
  • Loading branch information
@lbartoletti @nyalldawson
lbartoletti authored and nyalldawson committed Apr 20, 2018
1 parent d87f75a commit 9d649e7
Show file tree
Hide file tree
Showing 4 changed files with 302 additions and 0 deletions.
71 changes: 71 additions & 0 deletions python/core/geometry/qgsgeometryutils.sip.in
View file
Expand Up @@ -11,6 +11,7 @@




class QgsGeometryUtils
{
%Docstring
Expand Down Expand Up @@ -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
Expand Down
126 changes: 126 additions & 0 deletions src/core/geometry/qgsgeometryutils.cpp
View file
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down
66 changes: 66 additions & 0 deletions src/core/geometry/qgsgeometryutils.h
View file
Expand Up @@ -24,6 +24,8 @@ email : marco.hugentobler at sourcepole dot com
#include "qgsabstractgeometry.h"


#include <QVector3D>

class QgsLineString;

/**
Expand Down Expand Up @@ -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.
*
Expand Down
39 changes: 39 additions & 0 deletions tests/src/core/testqgsgeometryutils.cpp
View file
Expand Up @@ -57,6 +57,7 @@ class TestQgsGeometryUtils: public QObject
void testCoefficients();
void testPerpendicularSegment();
void testClosestPoint();
void testlinesIntersection3D();
void testSegmentIntersection();
void testLineCircleIntersection();
void testCircleCircleIntersection();
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 9d649e7

Please sign in to comment.