MRTriangleIntersection.h

Go to the documentation of this file.

#pragma once
 
#include "MRVector3.h"
#include "MRIntersectionPrecomputes.h"
#include "MRTriPoint.h"
#include "MRTriMath.h"
#include "MRLineSegm.h"
 
#include <algorithm>
#include <optional>
 
namespace MR
{

 
struct TriIntersectResult
{
    TriPointf bary;
    float t = 0;
    TriIntersectResult(float U, float V, float dist)
    {
        bary.a = U; bary.b = V; t = dist;
    }
};

template <typename T>
void rotateToLongestEdge( Vector3<T>& a, Vector3<T>& b, Vector3<T>& c )
{
    const auto ab2 = distanceSq( a, b );
    const auto bc2 = distanceSq( b, c );
    const auto ca2 = distanceSq( c, a );
    if ( ab2 >= bc2 && ab2 >= ca2 )
        return;
 
    if ( bc2 >= ca2 )
    {
        assert( bc2 >= ab2 );
        auto t = a;
        a = b;
        b = c;
        c = t;
        return;
    }
 
    assert( ca2 >= ab2 );
    assert( ca2 >= bc2 );
    auto t = a;
    a = c;
    c = b;
    b = t;
}

template <typename T>
bool doTrianglesIntersect(
    Vector3<T> a, Vector3<T> b, Vector3<T> c,
    Vector3<T> d, Vector3<T> e, Vector3<T> f
)
{
    if ( dirDblArea( a, b, c ) == Vector3<T>{} ) 
    {
        rotateToLongestEdge( a, b, c );
        return doTriangleSegmentIntersect( d, e, f, a, b );
    }
    if ( dirDblArea( d, e, f ) == Vector3<T>{} ) 
    {
        rotateToLongestEdge( d, e, f );
        return doTriangleSegmentIntersect( a, b, c, d, e );
    }
 
    const auto abcd = mixed( a - d, b - d, c - d );
    const auto abce = mixed( a - e, b - e, c - e );
    const auto abcf = mixed( a - f, b - f, c - f );
    const auto abc_de = abcd * abce >= 0; 
    const auto abc_fd = abcf * abcd >= 0; 
 
    if ( abc_de && abc_fd && abce * abcf >= 0 )
        return false; 
 
    const auto defa = mixed( d - a, e - a, f - a );
    const auto defb = mixed( d - b, e - b, f - b );
    const auto defc = mixed( d - c, e - c, f - c );
    const auto def_ab = defa * defb >= 0;  
    const auto def_ca = defc * defa >= 0;  
 
    if ( def_ab && def_ca && defb * defc >= 0 )
        return false; 
 
    if ( abc_de )
        std::swap( d, f );
    else if( abc_fd )
        std::swap( d, e );
 
    if ( def_ab )
        std::swap( a, c );
    else if ( def_ca )
        std::swap( a, b );
 
    const auto abde = mixed( a - e, b - e, d - e );
    const auto abdf = mixed( a - f, b - f, d - f );
 
    if ( abde * abdf < 0 )
        return true; 
 
    const auto acde = mixed( a - e, c - e, d - e );
 
    if ( abde * acde < 0 )
        return true; 
 
    if ( abdf == 0 && acde == 0 )
        return true; 
 
    const auto acdf = mixed( a - f, c - f, d - f );
 
    if ( acde * acdf < 0 )
        return true; 
 
    if ( abdf * acdf < 0 )
        return true; 
 
    if ( abde == 0 && acdf == 0 )
        return true; 
 
    return false;
}

template<typename T>
bool isPointInPlane( const Vector3<T>& p, const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& c )
{
    return mixed( p - a, p - b, p - c ) == T( 0 );
}
 
MR_BIND_TEMPLATE( bool isPointInPlane( const Vector3<float>& p, const Vector3<float>& a, const Vector3<float>& b, const Vector3<float>& c ) )
MR_BIND_TEMPLATE( bool isPointInPlane( const Vector3<double>& p, const Vector3<double>& a, const Vector3<double>& b, const Vector3<double>& c ) )
 

template<typename T>
bool isPointInLine( const Vector3<T>& p, const Vector3<T>& a, const Vector3<T>& b )
{
    return cross( p - a, p - b ).lengthSq() == T( 0 );
}

template<typename T>
bool isPointInLine( const Vector2<T>& p, const Vector2<T>& a, const Vector2<T>& b )
{
    return cross( p - a, p - b ) == T( 0 );
}

template<typename T>
bool isPointInSegm( const Vector3<T>& p, const Vector3<T>& a, const Vector3<T>& b )
{
    if ( !isPointInLine( p, a, b ) )
        return false;
 
    return dot( p - a, b - a ) >= 0 && dot( p - b, a - b ) >= 0;
}

template<typename T>
bool isPointInSegm( const Vector2<T>& p, const Vector2<T>& a, const Vector2<T>& b )
{
    if ( !isPointInLine( p, a, b ) )
        return false;
 
    return dot( p - a, b - a ) >= 0 && dot( p - b, a - b ) >= 0;
}
 
MR_BIND_TEMPLATE( bool isPointInLine( const Vector3<float>& p, const Vector3<float>& a, const Vector3<float>& b ) )
MR_BIND_TEMPLATE( bool isPointInLine( const Vector3<double>& p, const Vector3<double>& a, const Vector3<double>& b ) )
MR_BIND_TEMPLATE( bool isPointInSegm( const Vector3<float>& p, const Vector3<float>& a, const Vector3<float>& b ) )
MR_BIND_TEMPLATE( bool isPointInSegm( const Vector3<double>& p, const Vector3<double>& a, const Vector3<double>& b ) )
 

template<typename T>
bool isPointInTriangle( const Vector3<T>& p, const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& c )
{
    if ( !isPointInPlane( p, a, b, c ) )
        return false;
    const auto normDir = cross( b - a, c - a );
    if ( dot( normDir, cross( b - a, p - a ) ) < 0 )
        return false;
    if ( dot( normDir, cross( c - b, p - b ) ) < 0 )
        return false;
    if ( dot( normDir, cross( a - c, p - c ) ) < 0 )
        return false;
    if ( normDir.lengthSq() == 0 )
    {

        if ( a == b && b == c && p != a )
            return false; 
        if ( dot( b - a, c - a ) <= 0 )
            return isPointInSegm( p, b, c ); 
        else if ( ( b - a ).lengthSq() > ( c - a ).lengthSq() )
            return isPointInSegm( p, a, b ); 
        else
            return isPointInSegm( p, a, c ); 
    }
    return true;
}

template<typename T>
bool isPointInTriangle( const Vector2<T>& p, const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& c )
{
    const auto normSign = cross( b - a, c - a );
    if ( normSign * cross( b - a, p - a ) < 0 )
        return false;
    if ( normSign * cross( c - b, p - b ) < 0 )
        return false;
    if ( normSign * cross( a - c, p - c ) < 0 )
        return false;
    if ( normSign == 0 )
    {
        if ( a == b && b == c && p != a )
            return false; 
        if ( dot( b - a, c - a ) <= 0 )
            return isPointInSegm( p, b, c ); 
        else if ( ( b - a ).lengthSq() > ( c - a ).lengthSq() )
            return isPointInSegm( p, a, b ); 
        else
            return isPointInSegm( p, a, c ); 
    }
 
    return true;
}
 
MR_BIND_TEMPLATE( bool isPointInTriangle( const Vector3<float>& p, const Vector3<float>& a, const Vector3<float>& b, const Vector3<float>& c ) )
MR_BIND_TEMPLATE( bool isPointInTriangle( const Vector3<double>& p, const Vector3<double>& a, const Vector3<double>& b, const Vector3<double>& c ) )
MR_BIND_TEMPLATE( bool isPointInTriangle( const Vector2<float>& p, const Vector2<float>& a, const Vector2<float>& b, const Vector2<float>& c ) )
MR_BIND_TEMPLATE( bool isPointInTriangle( const Vector2<double>& p, const Vector2<double>& a, const Vector2<double>& b, const Vector2<double>& c ) )
 

template <typename T>
bool doesEdgeXySeparate(
    const Vector3<T> & x, const Vector3<T> & y, const Vector3<T> & z,
    const Vector3<T> & u, const Vector3<T> & v, const Vector3<T> & w,
    Vector3<T> d 
)
{
    const auto xy = ( y - x ).normalized();
    d = ( d - xy * dot( xy, d ) ).normalized();
    const auto dz = dot( d, z - x );
    return
        dz * dot( d, u - x ) < 0 &&
        dz * dot( d, v - x ) < 0 &&
        dz * dot( d, w - x ) < 0;
}

template <typename T>
bool doTrianglesIntersectExt(
    const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c,
    const Vector3<T> & d, const Vector3<T> & e, const Vector3<T> & f
)
{
    if ( !doTrianglesIntersect( a, b, c, d, e, f ) )
        return false;

    const auto dir = a + b + c - d - e - f;
 
    return
        !doesEdgeXySeparate( a, b, c, d, e, f, dir ) &&
        !doesEdgeXySeparate( b, c, a, d, e, f, dir ) &&
        !doesEdgeXySeparate( c, a, b, d, e, f, dir ) &&
        !doesEdgeXySeparate( d, e, f, a, b, c, dir ) &&
        !doesEdgeXySeparate( e, f, d, a, b, c, dir ) &&
        !doesEdgeXySeparate( f, d, e, a, b, c, dir );
}

template <typename T>
bool doTriangleLineIntersect(
    const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c,
    const Vector3<T> & d, const Vector3<T> & e
)
{
    const auto dabe = mixed( d - e, a - e, b - e );
    const auto dbce = mixed( d - e, b - e, c - e );
    if ( dabe * dbce <= 0 )
        return false; 
 
    const auto dcae = mixed( d - e, c - e, a - e );
    if ( dbce * dcae <= 0 )
        return false; 
 
    if ( dcae * dabe <= 0 )
        return false; 
 
    return true;
}

template <typename T>
bool doTriangleSegmentIntersect(
    const Vector3<T> & a, const Vector3<T> & b, const Vector3<T> & c,
    const Vector3<T> & d, const Vector3<T> & e
)
{
    const auto abcd = mixed( a - d, b - d, c - d );
    const auto abce = mixed( a - e, b - e, c - e );
    if ( abcd * abce >= 0 )
        return false; 
    return doTriangleLineIntersect( a, b, c, d, e );
}

template <typename T>
Vector3<T> findTriangleSegmentIntersection(
    const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& c,
    const Vector3<T>& d, const Vector3<T>& e
)
{
    const auto abcd = std::abs( mixed( a - d, b - d, c - d ) );
    const auto abce = std::abs( mixed( a - e, b - e, c - e ) );
    auto r = std::clamp( abcd / ( abcd + abce ), T( 0 ), T( 1 ) );
    return r * e + ( 1 - r ) * d;
}

template <typename T>
std::optional<Vector3<T>> findTriangleTriangleIntersection(
    const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& c,
    const Vector3<T>& d, const Vector3<T>& e, const Vector3<T>& f )
{
    if ( doTriangleSegmentIntersect( a, b, c, d, e ) )
        return findTriangleSegmentIntersection( a, b, c, d, e );
    if ( doTriangleSegmentIntersect( a, b, c, e, f ) )
        return findTriangleSegmentIntersection( a, b, c, e, f );
    if ( doTriangleSegmentIntersect( a, b, c, f, d ) )
        return findTriangleSegmentIntersection( a, b, c, f, d );
 
    if ( doTriangleSegmentIntersect( d, e, f, a, b ) )
        return findTriangleSegmentIntersection( d, e, f, a, b );
    if ( doTriangleSegmentIntersect( d, e, f, b, c ) )
        return findTriangleSegmentIntersection( d, e, f, b, c );
    if ( doTriangleSegmentIntersect( d, e, f, c, a ) )
        return findTriangleSegmentIntersection( d, e, f, c, a );
    return std::nullopt;
}
 
template <typename T>
std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<T>& oriA, const Vector3<T>& oriB, const Vector3<T>& oriC,
    const IntersectionPrecomputes<T>& prec )
{
    const T& Sx = prec.Sx;
    const T& Sy = prec.Sy;
    const T& Sz = prec.Sz;
 
    const T Ax = oriA[prec.idxX] - Sx * oriA[prec.maxDimIdxZ];
    const T Ay = oriA[prec.idxY] - Sy * oriA[prec.maxDimIdxZ];
    const T Bx = oriB[prec.idxX] - Sx * oriB[prec.maxDimIdxZ];
    const T By = oriB[prec.idxY] - Sy * oriB[prec.maxDimIdxZ];
    const T Cx = oriC[prec.idxX] - Sx * oriC[prec.maxDimIdxZ];
    const T Cy = oriC[prec.idxY] - Sy * oriC[prec.maxDimIdxZ];

    const T eps = std::numeric_limits<T>::epsilon() * std::max( { Ax, Bx, Cx, Ay, By, Cy } );
    T U = Cx * By - Cy * Bx;
    T V = Ax * Cy - Ay * Cx;
    T W = Bx * Ay - By * Ax;
 
    if( U < -eps || V < -eps || W < -eps )
    {
        if( U > eps || V > eps || W > eps )
        {
            return std::nullopt;
        }
    }
 
    T det = U + V + W;
    if( det == T( 0 ) )
        return std::nullopt;
    const T Az = Sz * oriA[prec.maxDimIdxZ];
    const T Bz = Sz * oriB[prec.maxDimIdxZ];
    const T Cz = Sz * oriC[prec.maxDimIdxZ];
    const T t = U * Az + V * Bz + W * Cz;
 
    auto invDet = T( 1 ) / det;
    return TriIntersectResult( float( V * invDet ), float( W * invDet ), float( t * invDet ) );
}
 
MR_BIND_TEMPLATE( std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<float >& oriA, const Vector3<float >& oriB, const Vector3<float >& oriC, const IntersectionPrecomputes<float >& prec ) )
MR_BIND_TEMPLATE( std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<double>& oriA, const Vector3<double>& oriB, const Vector3<double>& oriC, const IntersectionPrecomputes<double>& prec ) )
 
template <typename T>
std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<T>& oriA, const Vector3<T>& oriB, const Vector3<T>& oriC,
    const Vector3<T>& dir )
{
    const IntersectionPrecomputes<T> prec( dir );
    return rayTriangleIntersect( oriA, oriB, oriC, prec );
}
 
MR_BIND_TEMPLATE( std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<float >& oriA, const Vector3<float >& oriB, const Vector3<float >& oriC, const Vector3<float >& dir ) )
MR_BIND_TEMPLATE( std::optional<TriIntersectResult> rayTriangleIntersect( const Vector3<double>& oriA, const Vector3<double>& oriB, const Vector3<double>& oriC, const Vector3<double>& dir ) )
 

template<typename T>
bool doTrianglesOverlap( const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& c, const Vector2<T>& d, const Vector2<T>& e, const Vector2<T>& f )
{

    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,b }, LineSegm<Vector2<T>>{ d,e } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,b }, LineSegm<Vector2<T>>{ d,f } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,b }, LineSegm<Vector2<T>>{ e,f } ) )
        return true;

    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,c }, LineSegm<Vector2<T>>{ d,e } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,c }, LineSegm<Vector2<T>>{ d,f } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ a,c }, LineSegm<Vector2<T>>{ e,f } ) )
        return true;

    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ b,c }, LineSegm<Vector2<T>>{ d,e } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ b,c }, LineSegm<Vector2<T>>{ d,f } ) )
        return true;
    if ( doSegmentsIntersect( LineSegm<Vector2<T>>{ b,c }, LineSegm<Vector2<T>>{ e,f } ) )
        return true;

    if ( isPointInTriangle( a, d, e, f ) )
        return true;
    if ( isPointInTriangle( d, a, b, c ) )
        return true;
 
    return false;
}
 
MR_BIND_TEMPLATE( bool doTrianglesOverlap( const Vector2<float>& a, const Vector2<float>& b, const Vector2<float>& c, const Vector2<float>& d, const Vector2<float>& e, const Vector2<float>& f ) )
MR_BIND_TEMPLATE( bool doTrianglesOverlap( const Vector2<double>& a, const Vector2<double>& b, const Vector2<double>& c, const Vector2<double>& d, const Vector2<double>& e, const Vector2<double>& f ) )
 

 
}