| 400 | } |
| 401 | |
| 402 | std::optional<Vector3i> findTwoSegmentsIntersection( const Vector3i& ai, const Vector3i& bi, const Vector3i& ci, const Vector3i& di ) |
| 403 | { |
| 404 | const auto ab = Vector3i64{ bi - ai }; |
| 405 | const auto ac = Vector3i64{ ci - ai }; |
| 406 | const auto ad = Vector3i64{ di - ai }; |
| 407 | const auto abc = cross( ab, ac ); |
| 408 | const auto abd = cross( ab, ad ); |
| 409 | |
| 410 | if ( dot( Vector3i128fast( abc ), Vector3i128fast( abd ) ) > 0 ) |
| 411 | return std::nullopt; // CD is on one side of AB |
| 412 | |
| 413 | const auto cd = Vector3i64{ di - ci }; |
| 414 | const auto cb = Vector3i64{ bi - ci }; |
| 415 | const auto cda = cross( cd, -ac ); |
| 416 | const auto cdb = cross( cd, cb ); |
| 417 | if ( dot( Vector3i128fast( cda ), Vector3i128fast( cdb ) ) > 0 ) |
| 418 | return std::nullopt; // AB is on one side of CD |
| 419 | |
| 420 | constexpr Vector3i64 zero; |
| 421 | if ( ( abc == zero && abd == zero ) || ( cda == zero && cdb == zero ) ) // collinear |
| 422 | { |
| 423 | const auto dAC = dot( ab, ac ); |
| 424 | const auto dAD = dot( ab, ad ); |
| 425 | if ( dAC < 0 && dAD < 0 ) |
| 426 | return std::nullopt; // both C and D are lower than A (on the AB segment) |
| 427 | |
| 428 | const auto dBC = dot( -ab, -cb ); |
| 429 | const auto dBD = dot( -ab, Vector3i64{ di - bi } ); |
| 430 | if ( dBC < 0 && dBD < 0 ) |
| 431 | return std::nullopt; // both C and D are greater than B (on the AB segment) |
| 432 | |
| 433 | // have common points |
| 434 | auto onePoint = dAC < 0 ? ai : ci; // find point that is closer to B |
| 435 | auto otherPoint = dBD < 0 ? bi : di; // find point that is closer to A |
| 436 | return ( onePoint + otherPoint ) / 2; // return middle point of overlapping segment |
| 437 | } |
| 438 | |
| 439 | // common intersection - AB and CD are non-collinear |
| 440 | const Vector3i64 n = abc - abd; // not unit |
| 441 | FastInt128 ck = dot( Vector3i128fast( n ), Vector3i128fast( abc ) ); |
| 442 | FastInt128 dk = dot( Vector3i128fast( n ), Vector3i128fast( abd ) ); |
| 443 | assert( ck >=0 && dk <= 0 ); |
| 444 | |
| 445 | // scale down ck and dk to make sure that below products can be computed in 128 bits |
| 446 | // assume that abs( di ) <= 2^30 and abs( ci ) <= 2^30 |
| 447 | constexpr FastInt128 x = FastInt128( 1 ) << 96; //2^96 |
| 448 | if ( ck > x || -dk > x ) |
| 449 | { |
| 450 | ck = ck >> 32; |
| 451 | dk = -( (-dk) >> 32 ); |
| 452 | } |
| 453 | return Vector3i( divRound( ck * Vector3i128fast{ di } - dk * Vector3i128fast{ ci }, ck - dk ) ); |
| 454 | } |
| 455 | |
| 456 | Vector3f findTriangleSegmentIntersectionPrecise( |
| 457 | const Vector3f& a, const Vector3f& b, const Vector3f& c, |