paella/Code/include/Geometry/MathFunctions.inl

////////////////////////////////////////////////////////////////////////////////
//
// Paella
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
// Authors :
//     Thomas FORGIONE
//     Emilie JALRAS
//     Marion LENFANT
//     Thierry MALON
//     Amandine PAILLOUX
//
// This file is part of the project Paella
// This software is provided 'as-is', without any express or implied warranty.
// In no event will the authors be held liable for any damages arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it freely,
// subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented;
//    you must not claim that you wrote the original software.
//    If you use this software in a product, an acknowledgment
//    in the product documentation would be appreciated but is not required.
//
// 2. Altered source versions must be plainly marked as such,
//    and must not be misrepresented as being the original software.
//
// 3. This notice may not be removed or altered from any source distribution.
////////////////////////////////////////////////////////////////////////////////
#include <limits>

#include <opencv2/features2d/features2d.hpp>

#include "Geometry/Vector.hpp"
#include "Geometry/MathFunctions.hpp"

// Forward declaration of cv::Point
namespace cv
{

}

namespace geo
{

namespace detail
{
    template<typename T>
    auto const& get_x(T const& t)
    {
        return t.x;
    }

    template<typename T, std::size_t N>
    auto const& get_x(Vector<T,N> const& v)
    {
        return v.x();
    }

    template <typename T>
    auto const& get_y(T const& t)
    {
        return t.y;
    }

    template<typename T, std::size_t N>
    auto const& get_y(Vector<T,N> const& v)
    {
        return v.y();
    }

    template <typename T>
    auto const& get_z(T const& t)
    {
        return t.z;
    }

    template<typename T, std::size_t N>
    auto const& get_z(Vector<T,N> const& v)
    {
        return v.z();
    }
}

template<typename T>
struct vector_size;

template<typename T, std::size_t N>
struct vector_size<Vector<T,N>>
{
    static constexpr std::size_t value = N;
};

template<>
struct vector_size<cv::Point2f>
{
    static constexpr std::size_t value = 2;
};

template<typename InputIt, typename T, typename Distance>
InputIt project(T const& elementToProject, InputIt first, InputIt last, Distance const& distance)
{
    float best_dist = std::numeric_limits<float>::max();
    auto best_element = first;

    for (; first != last; first++)
    {
        float current_dist = distance(elementToProject, *first);

        if (current_dist < best_dist)
        {
            best_dist = current_dist;
            best_element = first;
        }
    }

    return best_element;
}

template<typename T1, typename T2, typename T3, std::size_t N>
struct distance_to_segment_helper;

template<typename T1, typename T2, typename T3>
struct distance_to_segment_helper<T1,T2,T3,2>
{

    float operator()(T1 const& v, T2 const& p1, T3 const& p2)
    {
        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*get_x(v)+a*get_y(v))/a;
        else
            yH = (-a*xH+a*get_x(p1)+b*get_y(p1))/b;

        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( (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;

    }

};

template<typename T1, typename T2, typename T3>
struct distance_to_segment_helper<T1,T2,T3,3>
{
    float operator()(T1 const& v, T2 const& p1, T3 const& p2)
    {
        using detail::get_x;
        using detail::get_y;
        using detail::get_z;

        auto x12 = get_x(p2)-get_x(p1);
        auto y12 = get_y(p2)-get_y(p1);
        auto z12 = get_z(p2)-get_z(p1);

        auto x1v = get_x(v)-get_x(p1);
        auto y1v = get_y(v)-get_y(p1);
        auto z1v = get_z(v)-get_z(p1);

        auto x2v = get_x(v)-get_x(p2);
        auto y2v = get_y(v)-get_y(p2);
        auto z2v = get_z(v)-get_z(p2);

        auto scal1 = x12 * x1v + y12 * y1v + z12 * z1v;
        auto scal2 = x12 * x2v + y12 * y2v + z12 * z2v;
        auto norm12 = x12 * x12 + y12 * y12 + z12 * z12;

        auto xH = get_x(p1) + (scal1 / norm12) * x12;
        auto yH = get_y(p1) + (scal1 / norm12) * y12;
        auto zH = get_z(p1) + (scal1 / norm12) * z12;

        float dist;

        if ((scal1*scal2) <0)
        {
            dist = (get_x(v)-xH) * (get_x(v)-xH) + (get_y(v)-yH) * (get_y(v)-yH) + (get_z(v)-zH) * (get_z(v)*zH);
        }
        else
        {
            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_z(v)-get_z(p1))*(get_z(v)-get_z(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)) + (get_z(v)-get_z(p2))*(get_z(v)-get_z(p2)));
        }

        return dist;
    }

};

template<typename T1, typename T2, typename T3>
float distanceToSegment(T1 const& v, T2 const& p1, T3 const& p2)
{
    static_assert(vector_size<T1>::value == vector_size<T2>::value,
                  "When computing distance between point and segment,"
                  "all elements must have the same size");

    static_assert(vector_size<T1>::value == vector_size<T3>::value,
                  "When computing distance between point and segment,"
                  "all elements must have the same size");

    return distance_to_segment_helper<T1,T2,T3,vector_size<T1>::value>{}(v,p1,p2);
}

} // namespace geo