Distance.hpp

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants 
Licensee a limited, non-exclusive, non-transferable, royalty-free license 
(without the right to sublicense) to use the API solely for the purpose of 
Licensee's internal development efforts to develop applications for which 
the API was provided.
 
The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Base
File: Distance
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/Polygon.h>
#include <NDEVR/Polyline.h>
#include <NDEVR/Plane.h>
#include <NDEVR/Intersection.h>
namespace NDEVR
{
    class Distance
    {};
    template<uint01 t_dims, class t_type, class t_vertex>
    t_type distanceSquared(const Bounds<t_dims, t_type, t_vertex>& bounds, const Vector<t_dims, t_type>& vertex)
    {
        t_type distance = 0;
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            if (vertex[dim] < bounds[MIN][dim])
            {
                t_type val = bounds[MIN][dim] - vertex[dim];
                distance += val * val;
            }
            else if (vertex[dim] > bounds[MAX][dim] )
            {
                t_type val = vertex[dim] - bounds[MAX][dim];
                distance += val * val;
            }
        }
        return distance;
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    t_type distanceSquared(const Vector<t_dims, t_type>& vertex, const Bounds<t_dims, t_type, t_vertex>& bounds)
    {
        return distanceSquared(bounds, vertex);
    }
 
    
    template<uint01 t_dims, class t_type, class t_vertex_a, class t_vertex_b>
    t_type distanceSquared(const LineSegment<t_dims, t_type, t_vertex_a>& line, const Bounds<t_dims, t_type, t_vertex_b>& bounds)
    {
        if (bounds.contains(line))
            return cast<t_type>(0);
        Vector<t_dims, t_type> p = line.vertex(A);
        Vector<t_dims, t_type> v = line.ray();
        for (uint01 i = 0; i < t_dims; ++i)
        {
            if (   bounds.doesIntersect(line.vertex(A)[i] - bounds[MIN][i], line.vertex(B)[i] - bounds[MIN][i], line.vertex(A), v, i)
                || bounds.doesIntersect(line.vertex(A)[i] - bounds[MAX][i], line.vertex(B)[i] - bounds[MAX][i], line.vertex(A), v, i))
                return cast<t_type>(0);
        }
        
        Vector<t_dims, t_type> t1 = (bounds[MIN] - p) / v;
        Vector<t_dims, t_type> t2 = (bounds[MAX] - p) / v;
        t_type p1 = getMax(t_type(0), t1.template dimensionalValue<MIN>(), t2.template dimensionalValue<MIN>());
        t_type p2 = getMax(t_type(0), getMin(t1.template dimensionalValue<MAX>(), t2.template dimensionalValue<MAX>()));
        Vector<t_dims, t_type> val = getMin(getMax((p + v * p1 + p + v * p2) / t_type(2), bounds[MIN]), bounds[MAX]);
        t_type t = getMax(t_type(0), ((val - p) * v).sum() / v.magnitudeSquared());
        val = p + v * t - val;
        return val.magnitudeSquared();
    }
 
    template<uint01 t_dims, class t_type, class t_vertex_a, class t_vertex_b>
    t_type distanceSquared(const Bounds<t_dims, t_type, t_vertex_a>& bounds, const LineSegment<t_dims, t_type, t_vertex_b>& line)
    {
        return distanceSquared(line, bounds);
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    t_type distanceSquared(const Bounds<t_dims, t_type, t_vertex>& a, const Bounds<t_dims, t_type, t_vertex>& b)
    {
        t_type distance = 0;
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            if (a[MAX][dim] < b[MIN][dim])
            {
                t_type val = b[MIN][dim] - a[MAX][dim];
                distance += val * val;
            }
            else if(a[MIN][dim] < b[MAX][dim])
            {
                t_type val = a[MIN][dim] - b[MAX][dim];
                distance += val * val;
            }
        }
        return distance;
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    t_type distanceSquared(const LineSegment<t_dims, t_type, t_vertex>& left, const LineSegment<t_dims, t_type, t_vertex>& right, const t_type& epsilon = cast<t_type>(0))
    {
        return left.template closestPoints<t_type>(right, epsilon).lengthSquared();
    }

    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Triangle<t_dims, t_type, t_vertex>& tri, const t_vertex& vertex)
    {
        return distanceSquared(vertex, ClosestPoint(tri, vertex));
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const t_vertex& vertex, const Triangle<t_dims, t_type, t_vertex>& tri)
    {
        return distanceSquared(vertex, ClosestPoint(tri, vertex));
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const LineSegment<t_dims, t_type, t_vertex>& line, const Triangle<t_dims, t_type, t_vertex>& tri)
    {
        if (classify(line, tri) != IntersectionTypes::e_outside)
        {
            return cast<t_type>(0);
        }
        t_type dist_a = distanceSquared(line, tri.edge(A, B));
        t_type dist_b = distanceSquared(line, tri.edge(B, C));
        t_type dist_c = distanceSquared(line, tri.edge(C, A));
        return getMin(dist_a, dist_b, dist_c);
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Triangle<t_dims, t_type, t_vertex>& tri, const LineSegment<t_dims, t_type, t_vertex>& line)
    {
        return distanceSquared(line, tri);
    }
 
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Bounds<t_dims, t_type, t_vertex>&, const Triangle<t_dims, t_type, t_vertex>&)
    {
        lib_assert(false, "Not yet finished");
        return Constant<t_type>::Invalid;// distanceSquared(line, closestPoint(tri, line));
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Triangle<t_dims, t_type, t_vertex>&, const Bounds<t_dims, t_type, t_vertex>&)
    {
        lib_assert(false, "Not yet finished");
        return Constant<t_type>::Invalid;// distanceSquared(line, closestPoint(tri, line));
    }
    template<class epsilon_type, uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const t_vertex& vertex, const LineSegment<t_dims, t_type, t_vertex>& line)
    {
        return sqrt(cast<epsilon_type>(distanceSquared(line, vertex)));
    }
    template<class t_type, class t_other_type>
    t_type distanceSquared(const t_other_type& original_object, const Polygon<t_type>& poly)
    {
        auto object = original_object.template as<2, t_type>();
        if (classify(poly, object) != IntersectionTypes::e_outside)
        {
            return cast<t_type>(0);
        }
        else
        {
            t_type min = Constant<t_type>::Max;
            for (uint04 i = 0; i < poly.vertexCount(); i++)
            {
                const t_type current = distanceSquared(poly.edge(i), object);
                if (current < min)
                {
                    min = current;
                }
            }
            return min;
        }
    }
    template<class t_type, class t_other_type>
    t_type distanceSquared(const Polygon<t_type>& poly, const t_other_type& original_object)
    {
        return distanceSquared(original_object, poly);
    }
 
    template<uint01 t_dims, class t_type, class t_other_type>
    t_type distanceSquared(const t_other_type& original_object, const Polyline<t_dims, t_type>& poly)
    {
        t_type min = Constant<t_type>::Max;
        if (poly.vertexCount() == 0)
            return Constant<fltp08>::Invalid;
        if (poly.vertexCount() == 1)
            return distanceSquared(poly.vertex(0), original_object);;
        for (uint04 i = 0; i < poly.vertexCount() - 1; i++)
        {
            const t_type current = distanceSquared(poly.segment(i), original_object);
            if (current < min)
            {
                min = current;
            }
        }
        return min;
    }
    template<uint01 t_dims, class t_type, class t_other_type>
    t_type distanceSquared(const Polyline<t_dims, t_type>& poly, const t_other_type& original_object)
    {
        return distanceSquared(original_object, poly);
    }
    template<class t_precision, uint01 t_dims, class t_type, class t_vertex>
    t_precision distance(const LineSegment<t_dims, t_type, t_vertex>& left, const LineSegment<t_dims, t_type, t_vertex>& right, t_precision epsilon = cast<t_precision>(0))
    {
        return sqrt(cast<t_precision>(distanceSquared(left, right, epsilon)));
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const Plane<t_dims, t_type>& plane, const Vertex<t_dims, t_vertex>& vertex)
    {
        return abs(dot(plane.normal, vertex) + plane.d);
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const Vertex<t_dims, t_vertex>& vertex, const Plane<t_dims, t_type>& plane)
    {
        return abs(dot(plane.normal, vertex) + plane.d);
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Plane<t_dims, t_type>& plane, const Vertex<t_dims, t_vertex>& vertex)
    {
        t_type d = distance(plane, vertex);
        return d * d;
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Vertex<t_dims, t_vertex>& vertex, const Plane<t_dims, t_type>& plane)
    {
        t_type d = distance(plane, vertex);
        return d * d;
    }
    template<class t_precision, uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const LineSegment<t_dims, t_type>& line, const Vector<t_dims, t_type>& vertex)
    {
        return sqrt(cast<t_precision>(distanceSquared(line, vertex)));
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const Plane<t_dims, t_type>& plane, const Bounds<t_dims, t_vertex>& bounds)
    {
        const Vertex<t_dims, t_type> c = (bounds[MIN] + bounds[MAX]) * t_type(0.5);
        const Vector<t_dims, t_type> e = (bounds[MAX] - bounds[MIN]) * t_type(0.5);
 
        const t_type s = dot(plane.normal, c) + plane.d;
        const t_type r = dot(abs(plane.normal), e);
 
        return getMax(t_type(0), abs(s) - r);
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distance(const Bounds<t_dims, t_vertex>& bounds, const Plane<t_dims, t_type>& plane)
    {
        return distance(plane, bounds);
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Plane<t_dims, t_type>& plane, const Bounds<t_dims, t_vertex>& bounds)
    {
        auto d = distance(plane, bounds);;
        return d * d;
    }
 
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const Bounds<t_dims, t_vertex>& bounds, const Plane<t_dims, t_type>& plane)
    {
        auto d = distance(plane, bounds);;
        return d * d;
    }
}