Triangle.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: Triangle
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/BaseValues.h>
#include <NDEVR/Vertex.h>
#include <NDEVR/LineSegment.h>
#include <NDEVR/Angle.h>
#include <NDEVR/AngleDefinitions.h>
 
namespace NDEVR
{
    enum TriangleLocation
    {
          outside_tri
        , edge_ab
        , edge_bc
        , edge_ca
        , vertex_a
        , vertex_b
        , vertex_c
        , angle_a
        , angle_b
        , angle_c
        , inside_tri
        , mixed_location_tri
        , tri_location_nan
    };

    class TriangleBase
    {
    public:
        static constexpr TriangleLocation oppositeLocation(TriangleLocation location)
        {
            switch (location)
            {
            case vertex_a:   return edge_bc;
            case vertex_b:   return edge_ca;
            case vertex_c:   return edge_ab;
            case edge_bc:    return vertex_a;
            case edge_ca:    return vertex_b;
            case edge_ab:    return vertex_c;
            default:
                lib_assert(false, "Segment does not have oposite value");
                break;
            }
            return tri_location_nan;
        }

        static constexpr TriangleLocation NextVertexCCW(TriangleLocation location)
        {
            switch (location)
            {
            case vertex_a: return vertex_b;
            case vertex_b: return vertex_c;
            case vertex_c: return vertex_a;
            default:
                lib_assert(false, "Not a valid vertex request");
                return tri_location_nan;
            }
        }

        static constexpr TriangleLocation NextVertexCW(TriangleLocation location)
        {
            switch (location)
            {
            case vertex_a: return vertex_c;
            case vertex_b: return vertex_a;
            case vertex_c: return vertex_b;
            default:
                lib_assert(false, "Not a valid vertex request");
                return tri_location_nan;
            }
        }
    };

    template<uint01 t_dims, class t_type, class t_vertex = Vertex<t_dims, t_type>>
    class Triangle : public Vector<3, t_vertex>, public TriangleBase
    {
    public:
        constexpr Triangle()
        {}
        explicit constexpr Triangle(const t_type& a)
            : Vector<3, t_vertex>(t_vertex(a), t_vertex(a), t_vertex(a))
        {}
        constexpr Triangle(const t_type& a, const t_type& b, const t_type& c)
            : Vector<3, t_vertex>(t_vertex(a), t_vertex(b), t_vertex(c))
        {}
        explicit constexpr Triangle(const t_vertex& vector)
            : Vector<3, t_vertex>(vector)
        {}
        constexpr Triangle(const t_vertex& a, const t_vertex& b, const t_vertex& c)
            : Vector<3, t_vertex>(a, b, c)
        {}

 
        [[nodiscard]] constexpr t_vertex& vertex(TriangleLocation triangle_node)
        {
            switch (triangle_node)
            {
            case vertex_a: return vertex(A);
            case vertex_b: return vertex(B);
            case vertex_c: return vertex(C);
            default: lib_assert(false, "Not a valid vertex request"); return vertex(A);
            }
        }

 
        [[nodiscard]] constexpr const t_vertex& vertex(TriangleLocation triangle_node) const
        {
            switch (triangle_node)
            {
            case vertex_a: return vertex(A);
            case vertex_b: return vertex(B);
            case vertex_c: return vertex(C);
            default: lib_assert(false, "Not a valid vertex request"); return vertex(A);
            }
        }

 
        [[nodiscard]] constexpr t_vertex& vertex(uint01 triangle_node)
        {
            lib_assert(triangle_node <= 2, "Tried to access bad Triangle Node");
            return (*this)[triangle_node];
        }

 
        [[nodiscard]] constexpr const t_vertex& vertex(uint01 triangle_node) const
        {
            lib_assert(triangle_node <= 2, "Tried to access bad Triangle Node");
            return (*this)[triangle_node];
        }

 
        [[nodiscard]] constexpr LineSegment<t_dims, t_type, t_vertex> edge(uint01 triangle_node_a, uint01 triangle_node_b) const
        {
            lib_assert(triangle_node_a <= 2 && triangle_node_b <= 2, "Tried to access bad Triangle Node");
            return LineSegment<t_dims, t_type, t_vertex>((*this)[triangle_node_a], (*this)[triangle_node_b]);
        }

 
        [[nodiscard]] constexpr LineSegment<t_dims, t_type, t_vertex> edge(TriangleLocation location) const
        {
            switch (location)
            {
            case edge_ab: return LineSegment<t_dims, t_type, t_vertex>(vertex(A), vertex(B));
            case edge_bc: return LineSegment<t_dims, t_type, t_vertex>(vertex(B), vertex(C));
            case edge_ca: return LineSegment<t_dims, t_type, t_vertex>(vertex(C), vertex(A));
            default: lib_assert(false, "Not a valid line segment request"); return LineSegment<t_dims, t_type>();
            }
        }

        template<class t_angle_type>
        [[nodiscard]] constexpr Angle< t_angle_type> angle(uint01 triangle_node) const
        {
            lib_assert(triangle_node <= 2, "Tried to access bad Triangle Node");
            const uint01 left  = triangle_node > 0 ? triangle_node - 1 : 2;
            const uint01 right = triangle_node < 2 ? triangle_node + 1 : 0;
            return AngleDefinitions::Rotation<t_angle_type>(vertex(left), vertex(triangle_node), vertex(right));
        }
 
        [[nodiscard]] constexpr fltp08 tan(uint01 triangle_node) const
        {
            lib_assert(triangle_node <= 2, "Tried to access bad Triangle Node");
            uint01 left = triangle_node > 0 ? triangle_node - 1 : 2;
            uint01 right = triangle_node < 2 ? triangle_node + 1 : 0;
 
            const Vector<t_dims, t_type> v_left(vertex(left) - vertex(triangle_node));
            const Vector<t_dims, t_type> v_right(vertex(triangle_node) - vertex(right));
            fltp08 x = dot(v_left, v_right);
            fltp08 y = cast<fltp08>(sqrt(v_left.magnitudeSquared() * v_right.magnitudeSquared()));
            
 
            return (y * sqrt(1.0 - ((x * x) / (y * y)))) / x;
        }

        template<class t_angle_type>
        [[nodiscard]] constexpr Angle<t_angle_type> angle(TriangleLocation location) const
        {
            switch (location)
            {
            case angle_a: return angle<t_angle_type>(A);
            case angle_b: return angle<t_angle_type>(B);
            case angle_c: return angle<t_angle_type>(C);
            default: lib_assert(false, "Not a valid vertex request"); return Angle<t_angle_type>(RADIANS, 0.0f);
            }
        }
 

        template<class t_area_type>
        [[nodiscard]] constexpr inline t_area_type area() const
        {
            return cast<t_area_type>(cast<t_area_type>(.5) * sqrt(cross((vertex(B) - vertex(A)), (vertex(C) - vertex(A))).magnitudeSquared()));
        }

 
        [[nodiscard]] constexpr t_vertex centroid() const
        {
            return ((cast<t_type>(2) * edge(B, C).midpoint()) + vertex(A)) / cast<t_type>(3);
        }

 
        [[nodiscard]] constexpr t_vertex center() const
        {
            t_vertex avg = (vertex(A) + vertex(B) + vertex(C)) / cast<t_type>(3);
            avg = clip(avg, getMin(vertex(A), vertex(B), vertex(C)), getMax(vertex(A), vertex(B), vertex(C)));
            return avg;
        }
        

        template<uint01 t_new_dims, class t_new_type>
        [[nodiscard]] constexpr Triangle<t_new_dims, t_new_type> as() const
        {
            return Triangle<t_new_dims, t_new_type>(vertex(A).template as<t_new_dims, t_new_type>(), vertex(B).template as<t_new_dims, t_new_type>(), vertex(C).template as<t_new_dims, t_new_type>());
        }
        

        template<bool is_normalized, class t_normal_type = t_type>
        [[nodiscard]] constexpr Vector<t_dims, t_normal_type> normal() const
        {
            const Vector<t_dims, t_normal_type> edge1 = vertex(B).template as<t_dims, t_normal_type>() - vertex(A).template as<t_dims, t_normal_type>();
            const Vector<t_dims, t_normal_type> edge2 = vertex(C).template as<t_dims, t_normal_type>() - vertex(A).template as<t_dims, t_normal_type>();
            Vector<t_dims, t_normal_type> cross_product = cross(edge1, edge2);
            if (is_normalized) cross_product = cross_product.template normalized<t_normal_type>();
            return cross_product;
        }

 
        [[nodiscard]] constexpr inline bool contains(const Vector<t_dims - 1, t_type>& p) const
        {
            if((p[X] - vertex(B)[X]) * (vertex(A)[Y] - vertex(B)[Y]) - (vertex(A)[X] - vertex(B)[X]) * (p[Y] - vertex(B)[Y]) < cast<t_type>(0))
            {
                if ((p[X] - vertex(C)[X]) * (vertex(B)[Y] - vertex(C)[Y]) - (vertex(B)[X] - vertex(C)[X]) * (p[Y] - vertex(C)[Y]) >= cast<t_type>(0))
                    return false;
                if ((p[X] - vertex(A)[X]) * (vertex(C)[Y] - vertex(A)[Y]) - (vertex(C)[X] - vertex(A)[X]) * (p[Y] - vertex(A)[Y]) >= cast<t_type>(0))
                    return false;
            }
            else
            {
                if ((p[X] - vertex(C)[X]) * (vertex(B)[Y] - vertex(C)[Y]) - (vertex(B)[X] - vertex(C)[X]) * (p[Y] - vertex(C)[Y]) < cast<t_type>(0))
                    return false;
                if ((p[X] - vertex(A)[X]) * (vertex(C)[Y] - vertex(A)[Y]) - (vertex(C)[X] - vertex(A)[X]) * (p[Y] - vertex(A)[Y]) < cast<t_type>(0))
                    return false;
            }
            return true;
        }

 
        [[nodiscard]] constexpr bool contains(const t_vertex& vertex_point, t_type epsilon = t_type(0.0001)) const
        {
            //Check bounds first, as rest of code may take time
            for(uint01 dim = 0; dim < t_dims; dim++)
                if(    getMax(vertex(A)[dim], vertex(B)[dim], vertex(C)[dim]) < vertex_point[dim]
                    || getMin(vertex(A)[dim], vertex(B)[dim], vertex(C)[dim]) > vertex_point[dim])
                    return false;
            // Compute vectors        
            const t_vertex dca = vertex(C) - vertex(A);
            const t_vertex dba = vertex(B) - vertex(A);
            const t_vertex dbp = vertex_point - vertex(A);
 
            // Compute dot products
            t_type dotca   = dot(dca, dca);
            t_type dotcaba = dot(dca, dba);
            t_type dotcabp = dot(dca, dbp);
            t_type dotba   = dot(dba, dba);
            t_type dotbabp = dot(dba, dbp);
 
            // Compute barycentric coordinates
            t_type invDenom = (dotca * dotba - dotcaba * dotcaba);
            t_type u        = ((dotba * dotcabp) - (dotcaba * dotbabp)) / invDenom;
            t_type v        = ((dotca * dotbabp) - (dotcaba * dotcabp)) / invDenom;
 
            // Check if point is in triangle
            if ((u >= 0) && (v >= 0) && (u + v < 1))
            {
                return true;
            }
            else
            {
                for (uint01 location = edge_ab; location <= edge_ca; location++)
                    if (distanceSquared(edge(cast<TriangleLocation>(location)), vertex_point) <= epsilon)
                        return true;
            }
            return false;
        }

 
        [[nodiscard]] constexpr TriangleLocation sharesObject(TriangleLocation object_to_check, const Triangle<t_dims, t_type, t_vertex>& tri, t_type epsilon = 0) const
        {
            switch (object_to_check)
            {
            case edge_ab:
            case edge_bc:
            case edge_ca:
            {
                const LineSegment<t_dims, t_type> edge_a = edge(object_to_check);
                for (uint01 i = edge_ab; i <= edge_ca; i++)
                {
                    LineSegment<t_dims, t_type> edge_b = tri.edge(cast<TriangleLocation>(i));
                    if (equals(edge_a, edge_b, epsilon))
                        return cast<TriangleLocation>(i);
                }
                return tri_location_nan;
            }
            case vertex_a:
            case vertex_b:
            case vertex_c:
            {
                const t_vertex vertex_1 = vertex(object_to_check);
                for (uint01 i = vertex_a; i <= vertex_c; i++)
                {
                    t_vertex vertex_2 = tri.vertex(cast<TriangleLocation>(i));
                    if (equals(vertex_1, vertex_2, epsilon))
                        return cast<TriangleLocation>(i);
                }
                return tri_location_nan;
            }
            case angle_a:
            case angle_b:
            case angle_c:
                return tri_location_nan;
            case inside_tri:
            case outside_tri:
            {
                const t_vertex vertex_a = vertex(object_to_check);
                for (uint01 i = 0; i <= 2; i++)
                {
                    t_vertex vertex_b = tri.vertex(i);
                    if (equals(vertex_a, vertex_b, epsilon))
                        return cast<TriangleLocation>(i);
                }
                return tri_location_nan;
            }
            default:
                return tri_location_nan;
            }
        }

        [[nodiscard]] constexpr uint01 vertexIndex(const t_vertex& p) const
        {
            for (uint01 i = A; i <= C; ++i)
                if (vertex(i) == p)
                    return i;
            return Constant<uint01>::Invalid;
        }
        [[nodiscard]] constexpr TriangleLocation getIdentityOf(const t_vertex& p) const
        {
            for (uint01 i = A; i <= C; ++i)
                if (vertex(i) == p)
                    return cast<TriangleLocation>(vertex_a + i);
            return tri_location_nan;
        }
        [[nodiscard]] constexpr TriangleLocation getIdentityOf(const t_vertex& p, t_type epsilon) const
        {
            lib_assert(IsValid(p), "Cannot get triangle identity of Invalid type");
            for (uint01 i = A; i <= C; ++i)
                if (equals(vertex(i), p, epsilon))
                    return cast<TriangleLocation>(vertex_a + i);
            return tri_location_nan;
        }
        [[nodiscard]] constexpr TriangleLocation getIdentityOf(const LineSegment<t_dims, t_type, t_vertex>& line, t_type epsilon) const
        {
            switch (getIdentityOf(line[A], epsilon))
            {
            case vertex_a:
                switch (getIdentityOf(line[B], epsilon))
                {
                case vertex_a: return vertex_a;
                case vertex_b: return edge_ab;
                case vertex_c: return edge_ca;
                default: return tri_location_nan;
                }
            case vertex_b:
                switch (getIdentityOf(line[B], epsilon))
                {
                case vertex_a: return edge_ab;
                case vertex_b: return vertex_b;
                case vertex_c: return edge_bc;
                default: return tri_location_nan;
                }
            case vertex_c:
                switch (getIdentityOf(line[B], epsilon))
                {
                case vertex_a: return edge_ca;
                case vertex_b: return edge_bc;
                case vertex_c: return vertex_c;
                default: return tri_location_nan;
                }
            default: return tri_location_nan;
            }
        }
        [[nodiscard]] constexpr TriangleLocation getIdentityOf(const LineSegment<t_dims, t_type, t_vertex>& line) const
        {
            switch (getIdentityOf(line[A]))
            {
            case vertex_a:
                switch (getIdentityOf(line[B]))
                {
                case vertex_a: return vertex_a;
                case vertex_b: return edge_ab;
                case vertex_c: return edge_ca;
                default: return tri_location_nan;
                }
            case vertex_b:
                switch (getIdentityOf(line[B]))
                {
                case vertex_a: return edge_ab;
                case vertex_b: return vertex_b;
                case vertex_c: return edge_bc;
                default: return tri_location_nan;
                }
            case vertex_c:
                switch (getIdentityOf(line[B]))
                {
                case vertex_a: return edge_ca;
                case vertex_b: return edge_bc;
                case vertex_c: return vertex_c;
                default: return tri_location_nan;
                }
            default: return tri_location_nan;
            }
        }
        [[nodiscard]] constexpr TriangleLocation getLocation(const t_vertex& p, t_type epsilon = cast<t_type>(0)) const
        {
            lib_assert(IsValid(p), "Cannot get triangle location of Invalid type");
            for (uint01 i = A; i <= C; ++i)
                if (equals(vertex(i), p, epsilon))
                    return cast<TriangleLocation>(i);
            for(uint01 location = edge_ab; location <= edge_ca; location++)
                if(distanceSquared(edge(cast<TriangleLocation>(location)), p) <= epsilon)
                    return cast<TriangleLocation>(location);
 
            const t_vertex normal_abp = Triangle(vertex(A), vertex(B), p).normal<true>();
            const t_vertex normal_bcp = Triangle(vertex(B), vertex(C), p).normal<true>();
 
            const t_type dot_1 = dot(normal_abp, normal_bcp);
            if(dot_1 >= epsilon + 1 || dot_1 <= -epsilon + 1)
                return outside_tri;
 
            const t_vertex normal_cap = Triangle(vertex(C), vertex(A), p).normal<true>();
            const t_type dot_2 = dot(normal_bcp, normal_cap);
            if((dot_2 >= epsilon + 1 && dot_2 <= -epsilon + 1))
                return outside_tri;
            return inside_tri;
        }
 
        [[nodiscard]] constexpr TriangleLocation getLocation(const LineSegment<t_dims, t_type, t_vertex>& line, t_type epsilon = cast<t_type>(0)) const
        {
            switch (getLocation(line[A], epsilon))
            {
            case edge_ab:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_a:
                case vertex_b:
                case edge_ab:    return edge_ab;
                case vertex_c:
                case edge_ca:
                case edge_bc:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case edge_bc:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_b:
                case vertex_c:
                case edge_bc:    return edge_bc;
                case vertex_a:
                case edge_ca:
                case edge_ab:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case edge_ca:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_c:
                case vertex_a:
                case edge_ca:    return edge_ca;
                case vertex_b:
                case edge_ab:
                case edge_bc:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case vertex_a:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_c:   return edge_ca;
                case vertex_b:   return edge_ab;
                case vertex_a:   return vertex_a;
                case edge_ca:    return edge_ca;
                case edge_ab:    return edge_ab;
                case edge_bc:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case vertex_b:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_b:   return vertex_b;
                case vertex_c:   return edge_bc;
                case vertex_a:   return edge_ab;
                case edge_bc:    return edge_bc;
                case edge_ab:    return edge_ab;
                case edge_ca:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case vertex_c:
                switch (getLocation(line[B], epsilon))
                {
                case vertex_c:   return vertex_c;
                case vertex_b:   return edge_bc;
                case vertex_a:   return edge_ca;
                case edge_bc:    return edge_bc;
                case edge_ca:    return edge_ca;
                case edge_ab:
                case inside_tri: return inside_tri;
                default: return mixed_location_tri;
                }
            case inside_tri:
                if (getLocation(line[B], epsilon) == outside_tri)
                    return mixed_location_tri;
                else
                    return inside_tri;
            case outside_tri:
                if (getLocation(line[B], epsilon) == outside_tri)
                    return outside_tri;
                else
                    return mixed_location_tri;
            default:
                lib_assert(false, "Should never arrive here in triangle identity line");
            }
            lib_assert(false, "Should never arrive here in triangle identity line");
            return tri_location_nan;
        }
        
 
        constexpr void invert()
        {
            std::swap(vertex(C), vertex(B));
        }
 
        

        constexpr const t_vertex& nextVertexCCW(const t_vertex& point) const
        {
            return vertex(NextVertexCCW(getIdentityOf(point)));
        }

        constexpr const t_vertex& nextVertexCW(const t_vertex& point) const
        {
            return vertex(NextVertexCW(getIdentityOf(point)));
        }
        bool hasVertex(const t_vertex& point) const
        {
            return point == vertex(A) || point == vertex(B) || point == vertex(C);
        }
    };
    
    
    

    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr Vector<t_dims, t_type> ClosestPoint(const Triangle<t_dims, t_type, t_vertex>& tri, const t_vertex& point)
    {
        const t_vertex edge0 = tri.edge(B, A).ray();
        const t_vertex edge1 = tri.edge(C, A).ray();
 
        const t_type a = dot(edge0, edge0);
        const t_type b = dot(edge0, edge1);
        const t_type c = dot(edge1, edge1);
 
        const t_type det = a * c - b * b;
 
        return ClosestPoint(tri, point, edge0, edge1, a, b, c, det);
    }
    template<uint01 t_dims, class t_type, class t_vertex>
    constexpr Vector<t_dims, t_type> ClosestPoint(const Triangle<t_dims, t_type, t_vertex>& tri, const t_vertex& point, const t_vertex& edge0, const Vector<t_dims, t_type>& edge1, t_type a, t_type b, t_type c, t_type det)
    {
        const t_vertex v0(tri.vertex(A) - point);
        const t_type d = dot(edge0, v0);
        const t_type e = dot(edge1, v0);
 
        t_type s = b * e - c * d;
        t_type t = b * d - a * e;
 
        if (s + t < det)
        {
            if (s < cast<t_type>(0))
            {
                if (t < cast<t_type>(0))
                {
                    if (d < cast<t_type>(0))
                    {
                        s = clip(-d / a, cast<t_type>(0), cast<t_type>(1));
                        t = cast<t_type>(0);
                    }
                    else
                    {
                        s = cast<t_type>(0);
                        t = clip(-e / c, cast<t_type>(0), cast<t_type>(1));
                    }
                }
                else
                {
                    s = cast<t_type>(0);
                    t = clip(-e / c, cast<t_type>(0), cast<t_type>(1));
                }
            }
            else if (t < cast<t_type>(0))
            {
                s = clip(-d / a, cast<t_type>(0), cast<t_type>(1));
                t = cast<t_type>(0);
            }
            else
            {
                t_type invDet = cast<t_type>(1) / det;
                s *= invDet;
                t *= invDet;
            }
        }
        else
        {
            if (s < cast<t_type>(0))
            {
                t_type tmp0 = b + d;
                t_type tmp1 = c + e;
                if (tmp1 > tmp0)
                {
                    t_type numer = tmp1 - tmp0;
                    t_type denom = a - 2 * b + c;
                    s = clip(numer / denom, cast<t_type>(0), cast<t_type>(1));
                    t = 1 - s;
                }
                else
                {
                    t = clip(-e / c, cast<t_type>(0), cast<t_type>(1));
                    s = cast<t_type>(0);
                }
            }
            else if (t < cast<t_type>(0))
            {
                if (a + d > b + e)
                {
                    t_type numer = c + e - b - d;
                    t_type denom = a - 2 * b + c;
                    s = clip(numer / denom, cast<t_type>(0), cast<t_type>(1));
                    t = 1 - s;
                }
                else
                {
                    s = clip(-e / c, cast<t_type>(0), cast<t_type>(1));
                    t = cast<t_type>(0);
                }
            }
            else
            {
                t_type numer = c + e - b - d;
                t_type denom = a - 2 * b + c;
                s = clip(numer / denom, cast<t_type>(0), cast<t_type>(1));
                t = cast<t_type>(1) - s;
            }
        }
 
        return tri.vertex(A) + s * edge0 + t * edge1;
    }

    template<bool t_has_winding, uint01 t_dims, class t_type, class t_vertex>
    constexpr bool Equals(const Triangle<t_dims, t_type, t_vertex>& tri_a, const Triangle<t_dims, t_type, t_vertex>& tri_b, t_type epsilon = 0)
    {
        uint01 dif = 0;
        for (uint01 n = A; n <= C; ++n)
        {
            if(equals(tri_a.vertex(A), tri_b.vertex(n), epsilon))
                break;
            dif++;
        }
        if (tri_a.vertex(B) == tri_b.vertex((B + dif) % 3))
        {
            if (tri_a.vertex(C) == tri_b.vertex((C + dif) % 3))
                return true;
        }
        else if(!t_has_winding && tri_a.vertex(B) == tri_b.vertex((C + dif) % 3))
        {
            if (tri_a.vertex(C) == tri_b.vertex((A + dif) % 3))
                return true;
        }
        return false;
    }
    
 
    
 
    template<uint01 t_dims, class t_type, class t_vector>
    struct Constant<Triangle<t_dims, t_type, t_vector>>
    {
        constexpr const static Triangle<t_dims, t_type, t_vector> Invalid{ Constant<t_vector>::Invalid, Constant<t_vector>::Invalid, Constant<t_vector>::Invalid };
        constexpr const static Triangle<t_dims, t_type, t_vector> Min{ Constant<t_vector>::Min, Constant<t_vector>::Min, Constant<t_vector>::Min };
        constexpr const static Triangle<t_dims, t_type, t_vector> Max{ Constant<t_vector>::Max, Constant<t_vector>::Max, Constant<t_vector>::Max };
    };
 
    template<uint01 t_dims, class t_type>
    static constexpr bool IsInvalid(const Triangle<t_dims, t_type>& value)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            if (IsInvalid(value[dim]))
                return true;
        }
        return false;
    }
    template<uint01 t_dims, class t_type>
    static constexpr bool IsValid(const Triangle<t_dims, t_type>& value)
    {
        for (uint01 dim = 0; dim < t_dims; ++dim)
        {
            if (IsInvalid(value[dim]))
                return false;
        }
        return true;
    }
};