Line.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: Line
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/BaseValues.h>
#include <NDEVR/Vertex.h>
#include <NDEVR/VectorFunctions.h>
namespace NDEVR
{

    template<uint01 t_dims, class t_type, class t_vertex = Vertex<t_dims, t_type>>
    class LineSegment : public Vector<2, t_vertex>
    {
    public: 
        constexpr LineSegment()
        {}
        constexpr LineSegment(const t_vertex& p1, const t_vertex& p2)
            : Vector<2, t_vertex>(p1, p2)
        {}
        explicit constexpr LineSegment(const Vector<2, t_vertex>& line)
            : Vector<2, t_vertex>(line)
        {}

        template<uint01 t_new_dims, class t_new_type, class t_new_vertex = Vertex<t_new_dims, t_new_type>>
        constexpr LineSegment<t_new_dims, t_new_type, t_new_vertex> as() const
        {
            return LineSegment<t_new_dims, t_new_type, t_new_vertex>(t_new_vertex(vertex(A).template as<t_new_dims, t_new_type>()), t_new_vertex(vertex(B).template as<t_new_dims, t_new_type>()));
        }

        template<class t_inter_type>
        constexpr inline t_vertex pointAt(t_inter_type index) const
        {
            return vertex(A) + (ray().template as<t_dims, t_inter_type>() * index).template as<t_dims, t_type>();
        }

 
        constexpr inline t_vertex midpoint() const
        {
            t_vertex avg = (vertex(A) + vertex(B)) / cast<t_type>(2);
            avg = clip(avg, getMin(vertex(A), vertex(B)), getMax(vertex(A), vertex(B)));
            return avg;
        }

 
        constexpr inline t_vertex ray() const
        {
            return vertex(B) - vertex(A);
        }

 
        constexpr inline t_vertex center() const
        {
            return midpoint();
        }

        constexpr inline const t_vertex& vertex(uint01 index) const
        {
            return Vector<2, t_vertex>::operator[](index);
        }

        constexpr inline t_vertex& vertex(uint01 index)
        {
            return Vector<2, t_vertex>::operator[](index);
        }

        template<class t_precision = t_type>
        constexpr bool isParallel(const LineSegment<t_dims, t_type, t_vertex>& line, t_precision epsilon = 0) const
        {
            const Vector<t_dims, t_precision> ray_1(     ray().template as<t_dims, t_type>().template normalized<t_precision>());
            const Vector<t_dims, t_precision> ray_2(line.ray().template as<t_dims, t_type>().template normalized<t_precision>());
            return (equals(ray_1, ray_2, epsilon) || equals(ray_1, -ray_2, epsilon));
        }

        template<class t_precision = t_type>
        constexpr bool isCollinear(const LineSegment<t_dims, t_type>& line, t_precision epsilon = 0) const
        {
            return isParallel<t_precision>(line, epsilon) && distanceSquared<t_precision>(line, epsilon) <= epsilon * epsilon;
        }

        template<class t_precision = t_type>
        constexpr bool isCollinear(const t_vertex& vert, t_precision epsilon = t_precision(1e-10)) const
        {
            const Vector<t_dims, t_precision> ray_1(vertex(B) - vertex(A));
            const Vector<t_dims, t_precision> ray_2(vert      - vertex(A));
            const Vector<t_dims, t_precision> c = cross(ray_1, ray_2);
            return c.magnitudeSquared() <= epsilon * ray_1.magnitudeSquared();
        }
    

        template<class t_precision, class t_other_vertex>
        constexpr LineSegment<t_dims, t_precision> closestPoints(const LineSegment<t_dims, t_type, t_other_vertex>& l2, t_precision epsilon = 0) const
        {
            const Vector<t_dims, t_precision> u(   ray());
            const Vector<t_dims, t_precision> v(l2.ray());
            const Vector<t_dims, t_precision> w(vertex(A) - l2.vertex(A));
 
            const t_precision a(dot(u, u));
            const t_precision b(dot(u, v));
            const t_precision c(dot(v, v));
 
            const t_precision d(dot(u, w));
            const t_precision e(dot(v, w));
 
            const t_precision D((a * c) - (b * b));
            t_precision sD = D;
            t_precision tD = D;
 
            t_precision sN;
            t_precision tN;
            // compute the line parameters of the two closest points
            if (D > epsilon) // the lines are almost parallel
            {
                sN = (b*e - c*d);
                tN = (a*e - b*d);
                if (sN < static_cast<t_precision>(0))  // sc < 0 => the s=0 edge is visible     
                {
                    sN = static_cast<t_precision>(0);
                    tN = e;
                    tD = c;
                }
                else if (sN > sD) // sc > 1 => the s=1 edge is visible
                {
                    sN = sD;
                    tN = e + b;
                    tD = c;
                }
            }
            else//Consider the lines parallel
            {
                sN = static_cast<t_precision>(0);       // force using point P0 on segment S1
                sD = static_cast<t_precision>(1);       // to prevent possible division by 0.0 later
                tN = e;
                tD = c;
            }
 
            if (tN < static_cast<t_precision>(0)) // tc < 0 => the t=0 edge is visible
            {
                tN = static_cast<t_precision>(0);
                //recompute sc for this edge
                if (-d < 0)
                    sN = static_cast<t_precision>(0);
                else if (-d > a)
                    sN = sD;
                else
                {
                    sN = -d;
                    sD = a;
                }
            }
            else if (tN > tD)       //tc > 1 => the t=1 edge is visible
            {
                tN = tD;
                // recompute sc for this edge
                if ((-d + b) < static_cast<t_precision>(0))
                    sN = static_cast<t_precision>(0);
                else if ((-d + b) > a)
                    sN = sD;
                else
                {
                    sN = (-d + b);
                    sD = a;
                }
            }
 
            //finally do the division to get sc and tc
            t_precision sc = abs(sN) <= epsilon ? static_cast<t_precision>(0) : sN / sD;
            t_precision tc = abs(sN) <= epsilon ? static_cast<t_precision>(0) : tN / tD;
 
            //get the difference of the two closest points
            //Vector<t_dims, t_type>dP = w + (u * sc) - (v * tc);
            //get the difference of the two closest points
            return LineSegment<t_dims, t_precision>(
                  (u * sc) + vertex(A).template as<t_dims, t_precision>()
                , (v * tc) + l2.vertex(A).template as<t_dims, t_precision>());
        }

        template<bool t_clip = true>
        constexpr t_type closestPos(const t_vertex& p) const
        {
            const Vector<t_dims, t_type> ray_1 = ray();
            const Vector<t_dims, t_type> ray_2 = p - vertex(A);
            
            t_type sum = (ray_1 * ray_2).sum();
            
            const t_type mag_squared = ray_1.magnitudeSquared();
            if (mag_squared != 0)
                sum /= mag_squared;
            if constexpr (t_clip)
                return clip(sum, cast<t_type>(0), cast<t_type>(1));
            else
                return sum;
        }

 
        constexpr t_vertex closestValue(const t_vertex& p) const
        {
            return pointAt(closestPos(p));
        }
        template<class t_precision = t_type>
        t_precision distanceSquared(const LineSegment<t_dims, t_type, t_vertex>& right, const t_precision& epsilon = cast<t_precision>(0)) const
        {
            return closestPoints<t_precision>(right, epsilon).lengthSquared();
        }


        template<class t_inter_type>
        constexpr LineSegment<t_dims, t_type> scale(const t_inter_type& scale) const
        {
            return LineSegment<t_dims, t_type>(pointAt(-scale / cast<t_inter_type>(2)), pointAt(scale / cast<t_inter_type>(2) + cast<t_inter_type>(1)));
        }
        template<class t_inter_type>
        constexpr LineSegment<t_dims, t_type> scale(const t_inter_type& a_scale, const t_inter_type& b_scale) const
        {
            return LineSegment<t_dims, t_type>(pointAt(-a_scale), pointAt(b_scale + cast<t_inter_type>(1)));
        }
 
        constexpr LineSegment<t_dims, t_type> extend(const t_type& extension) const
        {
            t_vertex dx = (extension / cast<t_type>(2)) * ray().template normalized<t_type>();
            return LineSegment<t_dims, t_type>(vertex(A) - dx, vertex(B) + dx);
        }
        constexpr LineSegment<t_dims, t_type> extend(const t_type& a_extension, const t_type& b_extension) const
        {
            t_vertex dx = ray().template normalized<t_type>();
            return LineSegment<t_dims, t_type>(vertex(A) - (dx * a_extension), vertex(B) + (dx * b_extension));
        }
        template<class t_precision = t_type>
        constexpr t_precision length() const
        {
            return ray().template magnitude<t_precision>();
        }

 
        constexpr t_type lengthSquared() const
        {
            return ray().magnitudeSquared();
        }
 
    

        template<class t_precision = t_type>
        constexpr Vector<t_dims, t_precision> intersection(const LineSegment& r, t_precision epsilon = 0) const
        {
            LineSegment<t_dims, t_precision> line = closestPoints<t_precision>(r);
            if(abs(line.vertex(B) - line.vertex(A)) <= epsilon)
                return line.vertex(B);
            else
                return Vector<t_dims, t_type>(Constant<t_type>::Invalid);
        }

        template<class t_precision = t_type>
        constexpr t_precision intersectionPosition(const LineSegment<t_dims, t_type>& segment) const
        {
            Vector<t_dims, t_precision> intersection_location(intersection<t_precision>(segment));
            if(IsValid(intersection_location))
                return closestPos(intersection_location);
            return Constant<t_precision>::Invalid;
        }

        template<class t_precision = t_type>
        [[nodiscard]] constexpr bool intersects(const LineSegment<3, t_type>& segment, t_precision epsilon = cast<t_precision>(1e-9)) const
        {
            return closestPoints<t_precision>(segment).lengthSquared() <= epsilon;
        }
        template<class t_precision = t_type, class t_vertex_type>
        [[nodiscard]] constexpr bool intersects(const LineSegment<2, t_type, t_vertex_type>& a, t_precision epsilon = cast<t_precision>(1e-9)) const
        {
            const Vector<2, t_precision> p = a[A].template as<2, t_precision>();
            const Vector<2, t_precision> r = a[B].template as<2, t_precision>() - p;
            const Vector<2, t_precision> q = (*this)[A].template as<2, t_precision>();
            const Vector<2, t_precision> s = (*this)[B].template as<2, t_precision>() - q;
 
            const t_precision rxs = cross<t_type>(r, s);
            const Vector<2, t_precision> qp = q - p;
            const t_precision qpxr = cross<t_type>(qp, r);
 
            const t_precision zero = t_precision(0);
 
            // If r x s == 0 and (q - p) x r == 0, then the lines are colinear
            if (abs(rxs) <= epsilon && abs(qpxr) <= epsilon)
            {
                const t_precision rr = dot(r, r);
                const t_precision t0 = dot(qp, r) / rr;
                const t_precision t1 = t0 + dot(s, r) / rr;
                const t_precision tmin = getMin(t0, t1);
                const t_precision tmax = getMin(t0, t1);
                return tmax >= zero && tmin <= t_precision(1);
            }
 
            // If r x s == 0 and (q - p) x r != 0, then the lines are parallel and non-intersecting
            if (abs(rxs) <= epsilon)
                return false;
 
            // Otherwise, check if intersection point lies on both segments
            const t_precision t = cross<t_type>(qp, s) / rxs;
            const t_precision u = qpxr / rxs;
 
            return (t >= -epsilon && t <= t_precision(1) + epsilon &&
                u >= -epsilon && u <= t_precision(1) + epsilon);
        }
        template<bool t_clip, class t_precision>
        constexpr t_precision getLocationAt(t_precision value, uint01 dim) const
        {
            if (vertex(A)[dim] == vertex(B)[dim])
                return Constant<t_precision>::Invalid;
            if (t_clip)
            {
                if (vertex(A)[dim] > vertex(B)[dim])
                {
                    if (value >= vertex(A)[dim])
                        return cast<t_precision>(0);
                    else if (value <= vertex(B)[dim])
                        return cast<t_precision>(1);
                }
                else
                {
                    if (value >= vertex(B)[dim])
                        return cast<t_precision>(1);
                    else if (value <= vertex(A)[dim])
                        return cast<t_precision>(0);
                }
            }
            return (vertex(A)[dim] - value) / ((value - vertex(B)[dim]) + (vertex(A)[dim] - value));
        }
 

        template<bool t_clip, class t_precision>
        constexpr t_vertex pointAt(t_precision value, uint01 dim, const t_vertex& nan_return = Constant<t_vertex>::Invalid) const
        {
            if (vertex(A)[dim] == vertex(B)[dim])
                return nan_return;
            if (t_clip)
            {
                if (vertex(A)[dim] > vertex(B)[dim])
                {
                    if (value >= vertex(A)[dim])
                        return vertex(A);
                    else if (value <= vertex(B)[dim])
                        return vertex(B);
                }
                else
                {
                    if (value >= vertex(B)[dim])
                        return vertex(B);
                    else if (value <= vertex(A)[dim])
                        return vertex(A);
                }
            }
            const t_precision dt = vertex(A)[dim] - value;
            const t_precision dy = value - vertex(B)[dim];
            return (vertex(B) * dt + vertex(A) * dy) / (dt + dy);
        }
 
        

        template<class t_buffer_type>
        constexpr static LineSegment createBestFitLine(const t_buffer_type& vertices, uint01 dim_0, uint01 dim_1)
        {
            if(vertices.size() < 2)
                return Constant<LineSegment>::Invalid;
            if(vertices.size() == 2)
                return LineSegment(vertices[0], vertices[1]);
            
            t_vertex total_vector(0);
            t_type total_volume = 0;
            t_type total_x_sqr = 0; 
            for(uint04 i = 0; i < vertices.size(); i++)
            {
                total_vector += vertices[i];
                total_x_sqr  += vertices[i][dim_0] * vertices[i][dim_0];
                total_volume += vertices[i][dim_0] * vertices[i][dim_1];
            }
            
            const t_type x_sqr = total_vector[dim_0] * total_vector[dim_0];
            const t_type a = (total_vector[dim_1] * total_x_sqr - total_vector[dim_0] * total_volume)   / (vertices.size() * total_x_sqr - x_sqr);
 
            const t_type b = (vertices.size() * total_volume - total_vector[dim_0] * total_vector[dim_1]) / (vertices.size() * total_x_sqr - x_sqr);
 
            const t_vertex p1(vertices[0]);
            const t_vertex p2 = vertices.getLast();
            p1[dim_1] = a + b * p1[dim_0];
            p2[dim_1] = a + b * p2[dim_0];
            
            return LineSegment(p1, p2);
        }
    };

    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquaredOptimized(const LineSegment<t_dims, t_type, t_vertex>& line, const t_vertex& vertex, const t_vertex& ray, const t_type& dot_ray)
    {
        const t_vertex v_t(vertex - line.vertex(A));
        const t_type t = dot(v_t, ray);
        if (t <= 0)
            return v_t.magnitudeSquared();
        else if (t >= dot_ray)
            return  distanceSquared(vertex, line.vertex(B));
        else
        {
            t_vertex vc = (ray * t) / dot_ray + line.vertex(A);
            return distanceSquared(vertex, vc);
        }
    }

    template<uint01 t_dims, class t_type, class t_vertex>
    t_type distanceSquared(const LineSegment<t_dims, t_type, t_vertex>& line, const t_vertex& vertex)
    {
        const t_vertex ray_value(line.ray());
        const t_type dot_ray = dot(ray_value, ray_value);
        return distanceSquaredOptimized(line, vertex, ray_value, dot_ray);
    }
 

    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr t_type distanceSquared(const t_vertex& vertex, const LineSegment<t_dims, t_type, t_vertex>& line)
    {
        return distanceSquared(line, vertex);
    }
 
 

    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr bool equals(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))
    {
        if (equals(left[A], right[A], epsilon))
        {
            return (equals(left[B], right[B], epsilon));
        }
        if (equals(left[B], right[A], epsilon))
        {
            return (equals(left[A], right[B], epsilon));
        }
        return false;
    }
 
    
 
    
 
    template<uint01 t_dims, class t_type>
    static constexpr bool IsInvalid(const LineSegment<t_dims, t_type>& value)
    {
        for (uint01 dim = 0; dim < 2; ++dim)
        {
            if (IsInvalid(value[dim]))
                return true;
        }
        return false;
    }
    template<uint01 t_dims, class t_type>
    static constexpr bool IsValid(const LineSegment<t_dims, t_type>& value)
    {
        for (uint01 dim = 0; dim < 2; ++dim)
        {
            if (IsInvalid(value[dim]))
                return false;
        }
        return true;
    }
    template<uint01 t_dims, class t_type, class t_vector>
    struct Constant<LineSegment<t_dims, t_type, t_vector>>
    {
        constexpr const static LineSegment<t_dims, t_type> Invalid{ Constant<t_vector>::Invalid, Constant<t_vector>::Invalid };
        constexpr const static LineSegment<t_dims, t_type> Min{ Constant<t_vector>::Min, Constant<t_vector>::Min };
        constexpr const static LineSegment<t_dims, t_type> Max{ Constant<t_vector>::Max, Constant<t_vector>::Max };
    };
    
};