diff --git a/olcUTIL_Geometry2D.h b/olcUTIL_Geometry2D.h index 0482473..667881e 100644 --- a/olcUTIL_Geometry2D.h +++ b/olcUTIL_Geometry2D.h @@ -899,8 +899,38 @@ namespace olc::utils::geom2d template inline olc::v_2d closest(const line& l1, const line& l2) { - // TODO: - return {}; + std::vector> intersections = intersects(l2, l1); + if (!intersections.empty()) { + return intersections[0]; + } + + olc::v_2d v1 = closest(l1, l2.start); + olc::v_2d v2 = closest(l1, l2.end); + olc::v_2d v3 = closest(l2, l1.start); + olc::v_2d v4 = closest(l2, l1.end); + + T1 d1 = (v1 - l2.start).mag2(); + T1 d2 = (v2 - l2.end).mag2(); + T2 d3 = (v3 - l1.start).mag2(); + T2 d4 = (v4 - l1.end).mag2(); + + double min = d1; + olc::v_2d& vClosest = v1; + + if (d2 < min) { + min = d2; + vClosest = v2; + } + if (d3 < min) { + min = d3; + vClosest = l1.start; + } + if (d4 < min) { + min = d4; + vClosest = l1.end; + } + + return vClosest; } // closest(r,l)