Cleaning distanceToSegment

Code/include/Geometry/MathFunctions.hpp

@@ -47,8 +47,8 @@ using Segment = std::pair<Vector<T,N>, Vector<T,N>>;
 /// \return The norm-2 distance between v and the closest point in s
 ///
 //////////////////////////////////////////////////////////////////
-template<typename T>
-float distanceToSegment(geo::Vector<T,2> const& v, geo::Segment<T,2> const& s);
+template<typename T1, typename T2, typename T3>
+float distanceToSegment(T1 const& v, T2 const& p1, T3 const& p2);
 
 } // namespace geo
 

Code/include/Geometry/MathFunctions.inl

@@ -5,6 +5,57 @@
 namespace geo
 {
 
+namespace detail
+{
+    template<typename T>
+    struct get_x_helper
+    {
+        auto const& operator()(T const& v)
+        {
+            return v.x;
+        }
+    };
+
+    template<typename T, std::size_t N>
+    struct get_x_helper<Vector<T,N>>
+    {
+        auto const& operator()(Vector<T,N> const& v)
+        {
+            return v.x();
+        }
+    };
+
+    template <typename T>
+    auto const& get_x(T const& v)
+    {
+        return get_x_helper<T>{}(v);
+    }
+
+    template<typename T>
+    struct get_y_helper
+    {
+        auto const& operator()(T const& v)
+        {
+            return v.y;
+        }
+    };
+
+    template<typename T, std::size_t N>
+    struct get_y_helper<Vector<T,N>>
+    {
+        auto const& operator()(Vector<T,N> const& v)
+        {
+            return v.y();
+        }
+    };
+
+    template <typename T>
+    auto const& get_y(T const& v)
+    {
+        return get_y_helper<T>{}(v);
+    }
+}
+
 template<typename InputIt, typename T, typename Distance>
 InputIt project(T const& elementToProject, InputIt first, InputIt last, Distance const& distance)
 {
@@ -25,26 +76,32 @@ InputIt project(T const& elementToProject, InputIt first, InputIt last, Distance
     return best_element;
 }
 
-template<typename T>
-float distanceToSegment(geo::Vector<T,2> const& v, geo::Segment<T,2> const& s)
+template<typename T1, typename T2, typename T3>
+float distanceToSegment(T1 const& v, T2 const& p1, T3 const& p2)
 {
-    geo::Vector<float,2> u = s.first-s.second;
-    auto a = -u.y();
-    auto b = u.x();
-    auto xH = (b*b*v.x() - a*b*v.y() + a*a*s.first.x() + a*b*s.first.y())/(a*a+b*b);
+    using detail::get_x;
+    using detail::get_y;
+
+    auto a = -get_y(p1) +get_y(p2);
+    auto b = get_x(p1) - get_x(p2);;
+    auto xH = (b*b*get_x(v) - a*b*get_y(v) + a*a*get_x(p1) + a*b*get_y(p1))/(a*a+b*b);
     auto yH = xH;
 
     if (std::abs(b) < std::abs(a))
-        yH = (b*xH-b*v.x()+a*v.y())/a;
+        yH = (b*xH-b*get_x(v)+a*get_y(v))/a;
     else
-        yH = (-a*xH+a*s.first.x()+b*s.first.y())/b;
+        yH = (-a*xH+a*get_x(p1)+b*get_y(p1))/b;
 
-    geo::Vector<T,2> ptH = {xH,yH};
-    T dist;
-    if ((ptH-s.first)*(ptH-s.second) <= 0)
-        dist = (v-ptH).norm2();
+    // geo::Vector<T,2> ptH = {xH,yH};
+
+    float prod = (xH - get_x(p1)) * (xH - get_x(p2)) + (yH - get_y(p1)) * (yH - get_y(p2));
+    float dist;
+
+    if (prod <= 0)
+        dist = (get_x(v)-xH)*(get_x(v)-xH) + (get_y(v)-yH)*(get_y(v)-yH)  ;
     else
-        dist = std::min((v-s.first).norm2(),(v-s.second).norm2());
+        dist = std::min( (get_x(v)-get_x(p1))*(get_x(v)-get_x(p1)) + (get_y(v)-get_y(p1))*(get_y(v)-get_y(p1)),
+                         (get_x(v)-get_x(p2))*(get_x(v)-get_x(p2)) + (get_y(v)-get_y(p2))*(get_y(v)-get_y(p2)));
 
     return dist;
 }

Code/src/Geometry/ProjectionTest.cpp

@@ -91,7 +91,12 @@ int main()
         {geo::Vector<float,2>{3.0f,3.0f},geo::Vector<float,2>{4.0f,3.0f}}
     };
 
-    auto const it3 = geo::project(element3, std::begin(space3), std::end(space3), geo::distanceToSegment<float>);
+    auto const it3 = geo::project(element3, std::begin(space3), std::end(space3),
+        [] (geo::Vector<float,2> const& p, geo::Segment<float,2> const& s)
+        {
+            return geo::distanceToSegment(p, s.first, s.second);
+        }
+    );
 
     test(std::distance(std::begin(space3), it3) == 3);