diff --git a/geometry/distance.hpp b/geometry/distance.hpp index 0214dc4d2a..9446a2dae5 100644 --- a/geometry/distance.hpp +++ b/geometry/distance.hpp @@ -1,6 +1,7 @@ #pragma once #include "../base/base.hpp" +#include "../base/math.hpp" #include "../std/limits.hpp" #include "../std/static_assert.hpp" @@ -16,16 +17,24 @@ private: STATIC_ASSERT(numeric_limits::is_signed); public: - DistanceToLineSquare(PointT p0, PointT p1) - : m_P0(p0), m_P1(p1), m_D(m_P1 - m_P0), m_D2(DotProduct(m_D, m_D)), m_InvD2(1.0 / m_D2) + DistanceToLineSquare(PointT const & p0, PointT const & p1) + : m_P0(p0), m_P1(p1), m_D(m_P1 - m_P0), m_D2(DotProduct(m_D, m_D)) { + if (my::AlmostEqual(m_D2, 0.0)) + m_isZero = true; + else + { + m_InvD2 = 1.0 / m_D2; + m_isZero = false; + } } double operator () (PointT Y) const { PointT const YmP0 = Y - m_P0; double const t = DotProduct(m_D, YmP0); - if (t <= 0) + + if (m_isZero || t <= 0) { // Y is closest to P0. return DotProduct(YmP0, YmP0); @@ -39,9 +48,11 @@ public: // Closest point is interior to segment. return DotProduct(YmP0, YmP0) - t * t * m_InvD2; } + private: PointT m_P0, m_P1, m_D; double m_D2, m_InvD2; + bool m_isZero; }; } diff --git a/geometry/geometry_tests/distance_test.cpp b/geometry/geometry_tests/distance_test.cpp index 9869d87b83..b847bd7f2d 100644 --- a/geometry/geometry_tests/distance_test.cpp +++ b/geometry/geometry_tests/distance_test.cpp @@ -6,6 +6,7 @@ template void FloatingPointsTest() { mn::DistanceToLineSquare d(PointT(-1, 3), PointT(2, 1)); + TEST_ALMOST_EQUAL(d(PointT(-1, 3)), 0.0, ()); TEST_ALMOST_EQUAL(d(PointT(2, 1)), 0.0, ()); TEST_ALMOST_EQUAL(d(PointT(-0.5, 0.5)), 3.25, ()); @@ -22,12 +23,22 @@ UNIT_TEST(DistanceToLineSquare2D_Floating) FloatingPointsTest(); } -#include "../../base/logging.hpp" - UNIT_TEST(DistanceToLineSquare2D_Integer) { mn::DistanceToLineSquare dI(m2::PointI(-1, 3), m2::PointI(2, 1)); + TEST_ALMOST_EQUAL(dI(m2::PointI(-1, 3)), 0.0, ()); TEST_ALMOST_EQUAL(dI(m2::PointI(2, 1)), 0.0, ()); TEST_ALMOST_EQUAL(dI(m2::PointI(4, 4)), 13.0, ()); } + +UNIT_TEST(DistanceToLineSquare2D_DegenerateSection) +{ + typedef m2::PointD P; + mn::DistanceToLineSquare

d(P(5, 5), P(5, 5)); + + TEST_ALMOST_EQUAL(d(P(5, 5)), 0.0, ()); + TEST_ALMOST_EQUAL(d(P(6, 6)), 2.0, ()); + TEST_ALMOST_EQUAL(d(P(0, 0)), 50.0, ()); + TEST_ALMOST_EQUAL(d(P(-1, -2)), 36.0 + 49.0, ()); +}