| 413 | } |
| 414 | |
| 415 | IntersectionResult< Point2D > line_line_intersection( |
| 416 | const InfiniteLine2D& line0, const InfiniteLine2D& line1 ) |
| 417 | { |
| 418 | // The intersection of two lines is a solution to P0 + s0*D0 = P1 + |
| 419 | // s1*D1. |
| 420 | // Rewrite this as s0*D0 - s1*D1 = P1 - P0 = Q. If DotPerp(D0, D1)) |
| 421 | // = 0, |
| 422 | // the lines are parallel. Additionally, if DotPerp(Q, D1)) = 0, |
| 423 | // the |
| 424 | // lines are the same. If Dotperp(D0, D1)) is not zero, then |
| 425 | // s0 = DotPerp(Q, D1))/DotPerp(D0, D1)) |
| 426 | // produces the point of intersection. Also, |
| 427 | // s1 = DotPerp(Q, D0))/DotPerp(D0, D1)) |
| 428 | |
| 429 | const Vector2D diff{ line0.origin(), line1.origin() }; |
| 430 | const auto D0DotPerpD1 = |
| 431 | dot_perpendicular( line0.direction(), line1.direction() ); |
| 432 | if( std::fabs( D0DotPerpD1 ) < 0. ) |
| 433 | { |
| 434 | // The lines are parallel. |
| 435 | return { INTERSECTION_TYPE::parallel }; |
| 436 | } |
| 437 | |
| 438 | const auto invD0DotPerpD1 = 1.0 / D0DotPerpD1; |
| 439 | const auto diffDotPerpD1 = dot_perpendicular( diff, line1.direction() ); |
| 440 | const auto s0 = diffDotPerpD1 * invD0DotPerpD1; |
| 441 | auto result = line0.origin() + line0.direction() * s0; |
| 442 | CorrectnessInfo< Point2D >::Correctness first_correctness{ |
| 443 | point_line_distance( result, line0 ) <= GLOBAL_EPSILON, |
| 444 | point_line_projection( result, line0 ) |
| 445 | }; |
| 446 | CorrectnessInfo< Point2D >::Correctness second_correctness{ |
| 447 | point_line_distance( result, line1 ) <= GLOBAL_EPSILON, |
| 448 | point_line_projection( result, line1 ) |
| 449 | }; |
| 450 | return { std::move( result ), |
| 451 | { first_correctness, second_correctness } }; |
| 452 | } |
| 453 | |
| 454 | IntersectionResult< Point2D > segment_segment_intersection( |
| 455 | const Segment2D& segment0, const Segment2D& segment1 ) |