//#####################################################################
// Copyright 2002-2006, Robert Bridson, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Neil Molino, Avi Robinson-Mosher, Andrew Selle, Eftychios Sifakis, Jonathan Su, Joseph Teran.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class TRIANGLE_3D
//##################################################################### 
#include <Core/Basic_Geometry/TRIANGLE_3D.h>
#include <Core/Arrays/ARRAY.h>
#include <Core/Arrays/INDIRECT_ARRAY.h>
#include <Core/Basic_Geometry/RAY.h>
#include <Core/Basic_Geometry/RAY_HIT.h>
#include <Core/Interpolation/MULTILINEAR_INTERPOLATION.h>
#include <Core/Level_Sets/LEVELSET_UTILITIES.h>
#include <Core/Math_Tools/INTERVAL.h>
#include <Core/Matrices/FRAME.h>
#include <Core/Matrices/MATRIX.h>
#include <Core/Nonlinear_Equations/ITERATIVE_SOLVER.h>
#include <Core/Polynomials/CUBIC.h>
#include <Core/Random_Numbers/RANDOM_NUMBERS.h>
namespace PhysBAM{
//#####################################################################
// Function Fill_Uniform
//#####################################################################
template<class T> void TRIANGLE_3D<T>::
Fill_Uniform(RANDOM_NUMBERS<T>& rand,const RANGE<TV>& box)
{
    for(auto&a:X) rand.Fill_Uniform(a,box);
}
//#####################################################################
// Function Change_Size
//#####################################################################
// Enlarges the triangle by pushing out the triangle edges by distance 'delta' orthogonally to the edges.
// This keeps the incenter fixed.  If the triangle is degenerate, it will not be changed.
template<class T> void TRIANGLE_3D<T>::
Change_Size(const T delta)
{
    TV edge_lengths((X.z-X.y).Magnitude(),(X.x-X.z).Magnitude(),(X.y-X.x).Magnitude());
    T perimeter=edge_lengths.Sum(),area=Area();
    if(!perimeter || !area) return; // don't know which direction to enlarge a degenerate triangle, so do nothing
    T scale=1+delta*(T).5*perimeter/area;
    TV incenter=Point_From_Barycentric_Coordinates(edge_lengths/perimeter);
    X.x=incenter+(X.x-incenter)*scale;X.y=incenter+(X.y-incenter)*scale;X.z=incenter+(X.z-incenter)*scale;
}
//#####################################################################
// Function Inside
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Inside(const TV& point,const T thickness_over_two) const
{
    return Point_Inside_Triangle(point,thickness_over_two);
}
//#####################################################################
// Function Point_Inside_Triangle
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Point_Inside_Triangle(const TV& point,const T thickness_over_2) const
{
    return Plane().Boundary(point,thickness_over_2) && Planar_Point_Inside_Triangle(point,thickness_over_2);
}
//#####################################################################
// Function Planar_Point_Inside_Triangle
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Planar_Point_Inside_Triangle(const TV& point,const T thickness_over_2) const
{
    TV normal=Normal();
    PLANE<T> edge_plane((X.y-X.x).Cross(normal).Normalized(),X.x);
    if(edge_plane.Outside(point,thickness_over_2)) return false;
    edge_plane.normal=(X.x-X.z).Cross(normal).Normalized();
    if(edge_plane.Outside(point,thickness_over_2)) return false;
    edge_plane.normal=(X.z-X.y).Cross(normal).Normalized();
    edge_plane.x0=X.y;
    if(edge_plane.Outside(point,thickness_over_2)) return false;
    return true;
}
//#####################################################################
// Function Lazy_Planar_Point_Inside_Triangle
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Lazy_Planar_Point_Inside_Triangle(const TV& point) const
{
    TV normal=Normal();
    TV edge_normal_1=(X.y-X.x).Cross(normal),point_minus_x1=point-X.x;
    if(edge_normal_1.Dot(point_minus_x1) > 0) return false;
    TV edge_normal_2=(X.x-X.z).Cross(normal);
    if(edge_normal_2.Dot(point_minus_x1) > 0) return false;
    edge_normal_1+=edge_normal_2;
    if(edge_normal_1.Dot(point-X.y) < 0) return false; // this equals X.y-X.z (== -edge_normal_3)
    return true;
}
//#####################################################################
// Function Minimum_Edge_Length
//#####################################################################
template<class T> T TRIANGLE_3D<T>::
Minimum_Edge_Length() const
{
    return min((X.y-X.x).Magnitude(),(X.z-X.y).Magnitude(),(X.x-X.z).Magnitude());
}
//#####################################################################
// Function Maximum_Edge_Length
//#####################################################################
template<class T> T TRIANGLE_3D<T>::
Maximum_Edge_Length() const
{      
    return max((X.y-X.x).Magnitude(),(X.z-X.y).Magnitude(),(X.x-X.z).Magnitude());
}
//#####################################################################
// Function Region
//#####################################################################
// returns the region one is near in priority of 1=vertex, 2=edge, 3=face based on distance, region_id differentiates which point or edge
template<class T> int TRIANGLE_3D<T>::
Region(const TV& location,int& region_id,const T distance) const
{
    SEGMENT_3D<T> segment(X.x,X.y);
    T d1=segment.Distance_From_Point_To_Segment(location);
    segment.X.x=X.z;
    T d2=segment.Distance_From_Point_To_Segment(location);
    segment.X.y=X.x;
    T d3=segment.Distance_From_Point_To_Segment(location);
    if(d1 > distance && d2 > distance && d3 > distance) return 2; // face is closest
    else if(d1 <= distance){
        if(d2 <= distance){region_id=2;return 0;} // vertex 2
        else if(d3 <= distance){region_id=1;return 0;} // vertex 1
        else{region_id=1;return 1;}} // edge 1
    else if(d2 <= distance){
        if(d3 <= distance){region_id=3;return 0;} // vertex 3
        else{region_id=2;return 1;}} // edge 2
    else{region_id=3;return 1;} // edge 3
}
//#####################################################################
// Function Closest_Point
//#####################################################################
template<class T> VECTOR<T,3> TRIANGLE_3D<T>::
Closest_Point_Weight(const TV& location) const
{
    TV w=Barycentric_Coordinates(location);
    auto frac=[](const TV& x0,const TV& x1,const TV& Y)
    {
        TV v=x1-x0;
        return v.Dot(Y-x0)/v.Dot(v);
    };
    if(w.x<0)
    {
        T f=frac(X(1),X(2),location);
        if(f<0)
        {
            if(w.z<0)
            {
                T g=frac(X(0),X(1),location);
                if(g<0) return {1,0,0};
                if(g>1) return {0,1,0};
                return {1-g,g,0};
            }
            return {0,1,0};
        }
        if(f>1)
        {
            if(w.y<0)
            {
                T g=frac(X(0),X(2),location);
                if(g<0) return {1,0,0};
                if(g>1) return {0,0,1};
                return {1-g,0,g};
            }
            return {0,0,1};
        }
        return {0,1-f,f};
    }
    if(w.y<0)
    {
        T f=frac(X(2),X(0),location);
        if(f<0) return {0,0,1};
        if(f>1)
        {
            if(w.z<0)
            {
                T g=frac(X(1),X(0),location);
                if(g<0) return {0,1,0};
                if(g>1) return {1,0,0};
                return {g,1-g,0};
            }
            return {1,0,0};
        }
        return {f,0,1-f};
    }
    if(w.z<0)
    {
        T f=frac(X(0),X(1),location);
        if(f<0) return {1,0,0};
        if(f>1) return {0,1,0};
        return {1-f,f,0};
    }
    return w;
}
//#####################################################################
// Function Closest_Point
//#####################################################################
template<class T> auto TRIANGLE_3D<T>::
Diff_Closest_Point_Weight(const TV& location) const
    -> std::pair<TV,MATRIX<T,TV::m>>
{
    typedef MATRIX<T,TV::m> TM;
    auto [w,dw]=Diff_Barycentric_Coordinates(location,X(0),X(1),X(2));
    auto frac=[](const TV& x0,const TV& x1,const TV& Y)
    {
        TV v=x1-x0,w=v/v.Dot(v);
        return std::make_pair(w.Dot(Y-x0),w);
    };
    if(w.x<0)
    {
        auto [f,df]=frac(X(1),X(2),location);
        if(f<0)
        {
            if(w.z<0)
            {
                auto [g,dg]=frac(X(0),X(1),location);
                if(g<0) return {{1,0,0},{}};
                if(g>1) return {{0,1,0},{}};
                return {{1-g,g,0},TM{-dg,dg,TV{}}.Transposed()};
            }
            return {{0,1,0},{}};
        }
        if(f>1)
        {
            if(w.y<0)
            {
                auto [g,dg]=frac(X(0),X(2),location);
                if(g<0) return {{1,0,0},{}};
                if(g>1) return {{0,0,1},{}};
                return {{1-g,0,g},TM{-dg,TV{},dg}.Transposed()};
            }
            return {{0,0,1},{}};
        }
        return {{0,1-f,f},TM{TV{},-df,df}.Transposed()};
    }
    if(w.y<0)
    {
        auto [f,df]=frac(X(2),X(0),location);
        if(f<0) return {{0,0,1},{}};
        if(f>1)
        {
            if(w.z<0)
            {
                auto [g,dg]=frac(X(0),X(1),location);
                if(g<0) return {{1,0,0},{}};
                if(g>1) return {{0,1,0},{}};
                return {{1-g,g,0},TM{-dg,dg,TV{}}.Transposed()};
            }
            return {{1,0,0},{}};
        }
        return {{f,0,1-f},TM{df,TV{},-df}.Transposed()};
    }
    if(w.z<0)
    {
        auto [f,df]=frac(X(0),X(1),location);
        if(f<0) return {{1,0,0},{}};
        if(f>1) return {{0,1,0},{}};
        return {{1-f,f,0},TM{-df,df,TV{}}.Transposed()};
    }
    return {w,dw};
}
//#####################################################################
// Function Diff_Closest_Point
//#####################################################################
template<class T> auto TRIANGLE_3D<T>::
Diff_Closest_Point(const TV& location,TV& weights,MATRIX<T,TV::m>& diff_weights) const
    -> std::pair<TV,MATRIX<T,TV::m>>
{
    auto [w,dw]=Diff_Closest_Point_Weight(location);
    weights=w;
    diff_weights=dw;
    MATRIX<T,TV::m> dpt(X(0),X(1),X(2));
    return {dpt*w,dpt*dw};
}
//#####################################################################
// Function Closest_Point
//#####################################################################
template<class T> VECTOR<T,3> TRIANGLE_3D<T>::
Closest_Point(const TV& location,TV& weights) const
{
    weights=Closest_Point_Weight(location);
    return weights.x*X.x+weights.y*X.y+weights.z*X.z;
}
//#####################################################################
// Function Distance_To_Triangle
//#####################################################################
template<class T> T TRIANGLE_3D<T>::
Distance_To_Triangle(const TV& location) const
{   
    TV weights,projected_point=Closest_Point(location,weights);
    return (location-projected_point).Magnitude();
}
//#####################################################################
// Function Distance_To_Triangle
//#####################################################################
template<class T> auto TRIANGLE_3D<T>::
Diff_Distance_To_Triangle(const TV& location) const -> std::pair<T,TV>
{
    TV w;
    MATRIX<T,TV::m> dw;
    auto [p,dp]=Diff_Closest_Point(location,w,dw);
    TV diff=p-location;
    T dist=diff.Normalize();
    return {dist,(dp-1).Transpose_Times(diff)};
}
//#####################################################################
// Function Minimum_Angle
//#####################################################################
template<class T> T TRIANGLE_3D<T>::
Minimum_Angle() const
{
    TV s1=(X.x-X.y).Normalized(),s2=(X.y-X.z).Normalized(),s3=(X.z-X.x).Normalized();
    return acos(max(s1.Dot(-s2),-s1.Dot(s3),s2.Dot(-s3)));
}
//#####################################################################
// Function Maximum_Angle
//#####################################################################
template<class T> T TRIANGLE_3D<T>::
Maximum_Angle() const
{
    TV s1=(X.x-X.y).Normalized(),s2=(X.y-X.z).Normalized(),s3=(X.z-X.x).Normalized();
    return acos(min(s1.Dot(-s2),-s1.Dot(s3),s2.Dot(-s3)));
}
//#####################################################################
// Function Signed_Solid_Angle
//#####################################################################
// positive for normals that point away from the center - not reliable if center is too close to the triangle face
template<class T> T TRIANGLE_3D<T>::
Signed_Solid_Angle(const TV& center) const
{
    VECTOR<TV,3> Y=X-center;
    T t=Triple_Product(Y.x,Y.y,Y.z);
    T a=Y.x.Magnitude(),b=Y.y.Magnitude(),c=Y.z.Magnitude();
    T ab=Y.x.Dot(Y.y),bc=Y.y.Dot(Y.z),ca=Y.z.Dot(Y.x);
    return 2*atan2(t,a*b*c+a*bc+b*ca+c*ab);
}
//#####################################################################
// Function Point_Face_Interaction
//#####################################################################
// outputs unsigned distance
template<class T> bool TRIANGLE_3D<T>::
Point_Face_Interaction(const TV& x,const T interaction_distance,const bool allow_negative_weights,T& distance) const                       
{
    distance=Signed_Distance(x);
    return abs(distance)<=interaction_distance && Planar_Point_Inside_Triangle(x,allow_negative_weights?interaction_distance:0);
}
//#####################################################################
// Function Point_Face_Interaction_Data
//#####################################################################
template<class T> void TRIANGLE_3D<T>::
Point_Face_Interaction_Data(const TV& x,T& distance,TV& interaction_normal,VECTOR<T,TV::m+1>& weights,const bool perform_attractions) const                       
{
    interaction_normal=Normal();
    weights=Barycentric_Coordinates(x).Insert(-1,0);
    if(!perform_attractions && distance<0){distance*=-1;interaction_normal*=-1;} // distance > 0, interaction_normal points from the triangle to the point
}
//#####################################################################
// Function Point_Face_Interaction
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Point_Face_Interaction(const TV& x,const TV& v,const TV& v1,const TV& v2,const TV& v3,const T interaction_distance,T& distance,
    TV& interaction_normal,VECTOR<T,TV::m+1>& weights,const bool allow_negative_weights,const bool exit_early) const
{      
    if(!Point_Face_Interaction(x,interaction_distance,allow_negative_weights,distance)) return false;
    if(!exit_early) Point_Face_Interaction_Data(x,distance,interaction_normal,weights,false); // relative speed is in the normal direction
    return true;
}
//#####################################################################
// Function Robust_Point_Triangle_Collision
//#####################################################################
template<class T> POINT_SIMPLEX_COLLISION_TYPE TRIANGLE_3D<T>::
Robust_Point_Face_Collision(const TRIANGLE_3D<T>& initial_triangle,const TRIANGLE_3D<T>& final_triangle,const TV& x,const TV& final_x,const T dt,
    const T collision_thickness,T& collision_time,TV& normal,VECTOR<T,TV::m+1>& weights)
{
    if(final_triangle.Point_Inside_Triangle(final_x,collision_thickness)){
        collision_time=dt;
        weights=final_triangle.Barycentric_Coordinates(final_x).Insert(-1,0);
        normal=final_triangle.Normal();
        return POINT_SIMPLEX_COLLISION_ENDS_INSIDE;}
    if(initial_triangle.Point_Inside_Triangle(x,collision_thickness)){
        collision_time=0;
        weights=initial_triangle.Barycentric_Coordinates(x).Insert(-1,0);
        normal=initial_triangle.Normal();
        return POINT_SIMPLEX_COLLISION_ENDS_OUTSIDE;}
    TV v1=(final_triangle.X.x-initial_triangle.X.x)/dt,v2=(final_triangle.X.y-initial_triangle.X.y)/dt,v3=(final_triangle.X.z-initial_triangle.X.z)/dt,v=(final_x-x)/dt;
    if(initial_triangle.Point_Face_Collision(x,v,v1,v2,v3,dt,collision_thickness,collision_time,normal,weights,false))
        return POINT_SIMPLEX_COLLISION_ENDS_OUTSIDE;
    return POINT_SIMPLEX_NO_COLLISION;
}
//#####################################################################
// Function Point_Triangle_Collision
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Point_Face_Collision(const TV& x,const TV& v,const TV& v1,const TV& v2,const TV& v3,const T dt,const T collision_thickness,
    T& collision_time,TV& normal,VECTOR<T,TV::m+1>& weights,const bool exit_early) const
{
    typedef conditional_t<is_same<T,float>::value,double,T> D;
    // find cubic and compute the roots as possible collision times
    TV ABo=X.y-X.x,ABv=dt*(v2-v1),ACo=X.z-X.x,ACv=dt*(v3-v1);
    TV No=ABo.Cross(ACo),Nv=ABo.Cross(ACv)+ABv.Cross(ACo),Na=ABv.Cross(ACv);
    TV APo=x-X.x,APv=dt*(v-v1);

    CUBIC<D> cubic((D)Na.Dot(APv),(D)Nv.Dot(APv)+Na.Dot(APo),
                                       (D)No.Dot(APv)+Nv.Dot(APo),(D)No.Dot(APo));
    T xmin=0,xmax=1.000001;
    int num_intervals=0;
    INTERVAL<D> intervals[3];
    cubic.Compute_Intervals(xmin,xmax,num_intervals,intervals);
    if(!num_intervals) return false;
  
    // find and check roots
    T distance;
    for(int k=0;k<num_intervals;k++){
        collision_time=dt*(T)Bisection_Secant_Root<D>(cubic,intervals[k].min_corner,intervals[k].max_corner);
        TRIANGLE_3D<T> triangle(X.x+collision_time*v1,X.y+collision_time*v2,X.z+collision_time*v3);
        if(triangle.Point_Face_Interaction(x+collision_time*v,v,v1,v2,v3,collision_thickness,distance,normal,weights,true,exit_early)) return true;}

    return false;
}
//#####################################################################
// Function Clip_To_Box
//#####################################################################
template<class T> void TRIANGLE_3D<T>::
Clip_To_Box(const RANGE<TV>& box,ARRAY<TRIANGLE_3D<T> >& clipped_simplices) const
{
    // cut with all sides of box
    clipped_simplices.Remove_All();
    clipped_simplices.Append(*this);
    for(int axis=0;axis<TV::m;axis++){
        for(int i=clipped_simplices.m-1;i>=0;i--){
            Cut_With_Hyperplane_And_Discard_Outside_Simplices(clipped_simplices(i),PLANE<T>(-TV::Axis_Vector(axis),box.min_corner),clipped_simplices);
            clipped_simplices.Remove_Index_Lazy(i);}
        for(int i=clipped_simplices.m-1;i>=0;i--){
            // TODO: make this more efficient by not removing a triangle that is fully inside
            Cut_With_Hyperplane_And_Discard_Outside_Simplices(clipped_simplices(i),PLANE<T>(TV::Axis_Vector(axis),box.max_corner),clipped_simplices);
            clipped_simplices.Remove_Index_Lazy(i);}}
}
//#####################################################################
// Function Cut_With_Hyperplane_And_Discard_Outside_Simplices
//#####################################################################
template<class T> void TRIANGLE_3D<T>::
Cut_With_Hyperplane_And_Discard_Outside_Simplices(const TRIANGLE_3D<T>& triangle,const PLANE<T>& cutting_plane,ARRAY<TRIANGLE_3D<T> >& negative_triangles)
{
    TV phi_nodes;
    VECTOR<TV,3> X_nodes;
    X_nodes(0)=triangle.X.x;
    X_nodes(1)=triangle.X.y;
    X_nodes(2)=triangle.X.z;
    for(int i=0;i<3;i++){phi_nodes(i)=cutting_plane.Signed_Distance(X_nodes(i));}

    // left simplices are in the negative halfspace, right simplices in the positive halfspace
    int positive_count=0,single_node_sign;
    for(int i=0;i<3;i++) if(phi_nodes(i)>0) positive_count++;
    switch(positive_count){
        case 0: // in negative halfspace
            negative_triangles.Append(triangle);break;
        case 1:
        case 2:
            single_node_sign=positive_count==1?1:-1;
            // draw positive triangle. has correct positive/negative area based on whether triangle is backwards or not
            for(int i=0;i<3;i++)if(LEVELSET_UTILITIES<T>::Sign(phi_nodes(i))==single_node_sign){
                VECTOR<TV,2> interface_locations;int index=(i+1)%3;
                VECTOR<int,2> other_locations;
                for(int j=0;j<2;j++,index=(index+1)%3){
                    other_locations(j)=index;
                    interface_locations(j)=Linear(X_nodes(i),X_nodes(index),LEVELSET_UTILITIES<T>::Theta(phi_nodes(i),phi_nodes(index)));}
                if(positive_count==1){ // add two triangles to negative triangles
                    negative_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(0)),X_nodes(other_locations(1))));
                    negative_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(1)),interface_locations(1)));}
                else // add triangle to negative_triangles
                    negative_triangles.Append(TRIANGLE_3D<T>(X_nodes(i),interface_locations(0),interface_locations(1)));
                return;}
        case 3: // in positive halfspace
            break;}
}
//#####################################################################
// Function Cut_With_Hyperplane
//#####################################################################
template<class T> void TRIANGLE_3D<T>::
Cut_With_Hyperplane(const TRIANGLE_3D<T>& triangle,const PLANE<T>& cutting_plane,ARRAY<TRIANGLE_3D<T> >& negative_triangles,ARRAY<TRIANGLE_3D<T> >& positive_triangles,T tol)
{
    TV phi_nodes;
    VECTOR<TV,3> X_nodes;
    X_nodes(0)=triangle.X.x;
    X_nodes(1)=triangle.X.y;
    X_nodes(2)=triangle.X.z;
    for(int i=0;i<3;i++) phi_nodes(i)=cutting_plane.Signed_Distance(X_nodes(i));

    int positive_count=0,single_node_sign;
    for(int i=0;i<3;i++) if(phi_nodes(i)>0) positive_count++;
    switch(positive_count){
        case 0: // in negative halfspace
            negative_triangles.Append(triangle);break;
        case 1:
        case 2:
            single_node_sign=positive_count==1?1:-1;
            // draw positive triangle. has correct positive/negative area based on whether triangle is backwards or not
            for(int i=0;i<3;i++)if(LEVELSET_UTILITIES<T>::Sign(phi_nodes(i))==single_node_sign){
                VECTOR<TV,2> interface_locations;int index=(i+1)%3;
                VECTOR<T,2> theta;
                VECTOR<int,2> other_locations;
                for(int j=0;j<2;j++,index=(index+1)%3){
                    other_locations(j)=index;
                    theta(j)=LEVELSET_UTILITIES<T>::Theta(phi_nodes(i),phi_nodes(index));
                    interface_locations(j)=Linear(X_nodes(i),X_nodes(index),theta(j));}
                if(positive_count==1){ // add 2 negative and 1 positive
                    if(1-theta(0)>tol) negative_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(0)),X_nodes(other_locations(1))));
                    if(1-theta(1)>tol) negative_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(1)),interface_locations(1)));
                    if(theta(0)>tol && theta(1)>tol) positive_triangles.Append(TRIANGLE_3D<T>(X_nodes(i),interface_locations(0),interface_locations(1)));}
                else{ // add 2 positive and 1 negative
                    if(1-theta(0)>tol) positive_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(0)),X_nodes(other_locations(1))));
                    if(1-theta(1)>tol) positive_triangles.Append(TRIANGLE_3D<T>(interface_locations(0),X_nodes(other_locations(1)),interface_locations(1)));
                    if(theta(0)>tol && theta(1)>tol) negative_triangles.Append(TRIANGLE_3D<T>(X_nodes(i),interface_locations(0),interface_locations(1)));}
                return;}
        case 3: // in positive halfspace
            positive_triangles.Append(triangle);break;
            break;}
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T> bool TRIANGLE_3D<T>::
Intersects(const TRIANGLE_3D<T>& triangle,T theta_tol,INTERSECTS_HELPER* ih) const
{
    INTERSECTS_HELPER lh[2],*h=ih?ih:lh; // h[0].w is a z, h[1].w is a y.
    VECTOR<TV,3> Y[2]={X,triangle.X};
    for(int i=0;i<2;i++) h[i].n=(Y[i][0]-Y[i][2]).Cross(Y[i][1]-Y[i][2]);
    TV r=h[0].n.Cross(h[1].n);
    for(int k=0;k<2;k++){
        h[k].neg=0;
        h[k].pos=0;
        for(int i=0;i<3;i++){
            h[k].x(i)=r.Dot(Y[k][i]);
            h[k].w(i)=h[1-k].n.Dot(Y[k][i]-Y[1-k][0]);
            h[k].neg|=(h[k].w(i)<0)<<i;
            h[k].pos|=(h[k].w(i)>0)<<i;}}
    for(int k=0;k<2;k++) if(!h[k].neg || !h[k].pos) return false;
    for(int k=0;k<2;k++){
        for(h[k].is=0;h[k].is<3;h[k].is++) if(h[k].pos==(1<<h[k].is) || h[k].neg==(1<<h[k].is)) break;
        h[k].i[0]=h[k].is+1;
        if(h[k].i[0]>2) h[k].i[0]=0;
        h[k].i[1]=3-h[k].is-h[k].i[0];
        for(int j=0;j<2;j++){
            h[k].th[j]=h[k].w(h[k].is)/(h[k].w(h[k].is)-h[k].w(h[k].i[j]));
            h[k].t[j]=h[k].x(h[k].is)+h[k].th[j]*(h[k].x(h[k].i[j])-h[k].x(h[k].is));}}

    if(theta_tol>0){
        for(int k=0;k<2;k++)
            for(int j=0;j<2;j++)
                if(h[k].th[j]<theta_tol){
                    h[k].th[j]=0;
                    h[k].th[1-j]=0;
                    h[k].neg&=~(1<<h[k].is);
                    h[k].pos&=~(1<<h[k].is);}
        for(int k=0;k<2;k++)
            for(int j=0;j<2;j++)
                if(1-h[k].th[j]<theta_tol){
                    h[k].th[j]=1;
                    h[k].neg&=~(1<<h[k].i[j]);
                    h[k].pos&=~(1<<h[k].i[j]);}
        for(int k=0;k<2;k++) if(!h[k].neg || !h[k].pos) return false;}

    for(int k=0;k<2;k++) if(min(h[k].t[0],h[k].t[1])>=max(h[1-k].t[0],h[1-k].t[1])) return false;
    return true;
}
//#####################################################################
// Function Barycentric_Coordinates
//#####################################################################
template<class T> VECTOR<T,3> TRIANGLE_3D<T>::
Barycentric_Coordinates(const TV& location,const TV& x0,const TV& x1,const TV& x2) // clockwise vertices
{
    TV u=x1-x0,v=x2-x0,w=location-x0,n=u.Cross(v),vn=v.Cross(n),nu=n.Cross(u);
    T d=n.Dot(n);
    if(!d) return TV((T)1/3,(T)1/3,(T)1/3);
    T b=w.Dot(vn)/d,c=w.Dot(nu)/d,a=1-b-c;
    return TV(a,b,c);
}
//#####################################################################
// Function Diff_Barycentric_Coordinates
//#####################################################################
template<class T> auto TRIANGLE_3D<T>::
Diff_Barycentric_Coordinates(const TV& location,const TV& x0,const TV& x1,const TV& x2)
    -> std::pair<TV,MATRIX<T,TV::m>>
{
    typedef MATRIX<T,TV::m> TM;
    TV u=x1-x0,v=x2-x0,w=location-x0,n=u.Cross(v),vn=v.Cross(n),nu=n.Cross(u);
    T d=n.Dot(n);
    if(!d) return {TV((T)1/3,(T)1/3,(T)1/3),{}};
    T b=w.Dot(vn)/d,c=w.Dot(nu)/d,a=1-b-c;
    TV db=vn/d,dc=nu/d,da=-db-dc;
    return {TV(a,b,c),TM{da,db,dc}.Transposed()};
}
//#####################################################################
// Function Clamped_Barycentric_Coordinates
//#####################################################################
template<class T> VECTOR<T,3> TRIANGLE_3D<T>::
Clamped_Barycentric_Coordinates(const TV& location,const TV& x0,const TV& x1,const TV& x2,const T tolerance) // clockwise vertices
{
    TV u=x1-x0,v=x2-x0,w=location-x0;
    T u_dot_u=u.Dot(u),v_dot_v=v.Dot(v),u_dot_v=u.Dot(v),
       u_dot_w=u.Dot(w),v_dot_w=v.Dot(w);
    if(abs(u_dot_u)<tolerance){
        if(abs(v_dot_v)<tolerance) return TV(((T)1/3),((T)1/3),((T)1/3)); // single point
        T c=clamp(v_dot_w/v_dot_v,(T)0,(T)1);T a_and_b=(T).5*(1-c);return TV(a_and_b,a_and_b,c);} // x0 and x1 are a single point
    else if(abs(v_dot_v)<tolerance){
        T b=clamp(u_dot_w/u_dot_u,(T)0,(T)1);T a_and_c=(T).5*(1-b);return TV(a_and_c,b,a_and_c);} // x0 and x2 are a single point
    else{
        T denominator=u_dot_u*v_dot_v-sqr(u_dot_v); 
        if(abs(denominator)<tolerance){
            if(u_dot_v>0){ // u and v point in the same direction
                if(u_dot_u>u_dot_v){T b=clamp(u_dot_w/u_dot_u,(T)0,(T)1);return TV(1-b,b,0);}
                else{T c=clamp(v_dot_w/v_dot_v,(T)0,(T)1);return TV(1-c,0,c);}}
            else if(u_dot_w>0){T b=clamp(u_dot_w/u_dot_u,(T)0,(T)1);return TV(1-b,b,0);} // u and v point in opposite directions, and w is on the u segment
            else{T c=clamp(v_dot_w/v_dot_v,(T)0,(T)1);return TV(1-c,0,c);}} // u and v point in opposite directions, and w is on the v segment
        T one_over_denominator=1/denominator;
        T a=clamp((v_dot_v*u_dot_w-u_dot_v*v_dot_w)*one_over_denominator,(T)0,(T)1),b=clamp((u_dot_u*v_dot_w-u_dot_v*u_dot_w)*one_over_denominator,(T)0,(T)1);
        return TV(1-a-b,a,b);}
}
//#####################################################################
// Function Scaled
//#####################################################################
template<class T,class TV> TRIANGLE_3D<T>
Scaled(const TRIANGLE_3D<T>& b,const TV& center,T scale)
{
    return TRIANGLE_3D<T>(VECTOR<TV,3>((b.X-center)*scale+center));
}
//#####################################################################
// Function Transformed
//#####################################################################
template<class T,class TV> TRIANGLE_3D<T>
Transformed(const TRIANGLE_3D<T>& b,const FRAME<TV>& f)
{
    return TRIANGLE_3D<T>(f*b.X(0),f*b.X(1),f*b.X(2));
}
//#####################################################################
// Function Intersects
//#####################################################################
template<class T,class TV> RAY_HIT<T>
Intersects(const RAY<TV>& ray,const TRIANGLE_3D<T>& triangle,const T thickness_over_two)
{
    T thickness=2*thickness_over_two;

    // first check the plane of the triangle's face
    TV normal=triangle.Normal();
    auto hit=Intersects(ray,PLANE<T>(normal,triangle.X.x),thickness_over_two);
    if(!hit.Hit()) return {}; // otherwise intersects the plane
    TV plane_point(ray.Point(hit.dist));
    PLANE<T> edge_plane_12((triangle.X.y-triangle.X.x).Cross(normal).Normalized(),triangle.X.x);
    PLANE<T> edge_plane_23((triangle.X.z-triangle.X.y).Cross(normal).Normalized(),triangle.X.y);
    PLANE<T> edge_plane_31((triangle.X.x-triangle.X.z).Cross(normal).Normalized(),triangle.X.z);
    if(!edge_plane_12.Outside(plane_point,thickness) && !edge_plane_23.Outside(plane_point,thickness) && !edge_plane_31.Outside(plane_point,thickness))
        return hit; // intersects face of triangle wedge 

    PLANE<T> triangle_plane(normal,triangle.X.x);
    // check for intersection with the sides of the wedge
    if(edge_plane_12.Outside(plane_point,thickness_over_two)){
        auto hit=Intersects(ray,edge_plane_12,thickness_over_two);
        if(hit.Hit()){
            TV edge_point(ray.Point(hit.dist));
            if(triangle_plane.Boundary(edge_point,thickness) && !edge_plane_23.Outside(edge_point,thickness) && !edge_plane_31.Outside(edge_point,thickness)){
                return hit;}}}
    if(edge_plane_23.Outside(plane_point,thickness_over_two)){
        auto hit=Intersects(ray,edge_plane_23,thickness_over_two);
        if(hit.Hit()){
            TV edge_point(ray.Point(hit.dist));
            if(triangle_plane.Boundary(edge_point,thickness) && !edge_plane_12.Outside(edge_point,thickness) && !edge_plane_31.Outside(edge_point,thickness)){
                return hit;}}}
    if(edge_plane_31.Outside(plane_point,thickness_over_two)){
        auto hit=Intersects(ray,edge_plane_31,thickness_over_two);
        if(hit.Hit()){
            TV edge_point(ray.Point(hit.dist));
            if(triangle_plane.Boundary(edge_point,thickness) && !edge_plane_12.Outside(edge_point,thickness) && !edge_plane_23.Outside(edge_point,thickness)){
                return hit;}}}
    return {};
}
//#####################################################################
// Function Lazy_Intersects
//#####################################################################
template<class T,class TV> RAY_HIT<T> Lazy_Intersects(const RAY<TV>& ray,const TRIANGLE_3D<T>& triangle,const T thickness_over_two)
{
    PLANE<T> triangle_plane(triangle.Normal(),triangle.X.x);
    auto hit=Intersects(ray,triangle_plane);
    if(!hit.Hit()) return {}; // otherwise intersects the plane
    if(triangle.Lazy_Planar_Point_Inside_Triangle(ray.Point(hit.dist))) return hit; // intersects the face of the triangle 
    return {};
}
//#####################################################################
// Function Closest_Non_Intersecting_Point
//#####################################################################
template<class T,class TV> RAY_HIT<T> Closest_Non_Intersecting_Point(const RAY<TV>& ray,const TRIANGLE_3D<T>& triangle,const T thickness_over_two)
{
    auto hit=Intersects(ray,triangle,thickness_over_two);
    if(!hit.Hit()) return {};
    if(hit.Hit(thickness_over_two)) return RAY_HIT<T>{0};

    TV normal=triangle.Normal();
    // TODO: Save having to re-generate all the planes...
    T thickness=2*thickness_over_two;
    TV normal_times_thickness=normal*thickness;
    TRIANGLE_3D<T> top_triangle(triangle.X+normal_times_thickness);
    TRIANGLE_3D<T> bottom_triangle(triangle.X-normal_times_thickness);
    PLANE<T> top_plane(normal,triangle.X.x);
    PLANE<T> bottom_plane(normal,triangle.X.x);
    PLANE<T> edge_plane_12((triangle.X.y-triangle.X.x).Cross(normal).Normalized(),triangle.X.x);
    edge_plane_12.x0+=edge_plane_12.normal*thickness;
    PLANE<T> edge_plane_23((triangle.X.z-triangle.X.y).Cross(normal).Normalized(),triangle.X.y);
    edge_plane_23.x0+=edge_plane_23.normal*thickness;
    PLANE<T> edge_plane_31((triangle.X.x-triangle.X.z).Cross(normal).Normalized(),triangle.X.z);
    edge_plane_31.x0+=edge_plane_31.normal*thickness;
    hit=Intersects(ray,top_triangle,thickness_over_two);
    hit=hit.Best(Intersects(ray,bottom_triangle,thickness_over_two));
    hit=hit.Best(Rectangle_Intersects(ray,edge_plane_12,top_plane,bottom_plane,edge_plane_23,edge_plane_31,thickness_over_two));
    hit=hit.Best(Rectangle_Intersects(ray,edge_plane_23,top_plane,bottom_plane,edge_plane_12,edge_plane_31,thickness_over_two));
    hit=hit.Best(Rectangle_Intersects(ray,edge_plane_31,top_plane,bottom_plane,edge_plane_12,edge_plane_23,thickness_over_two));
    return hit;
}
//#####################################################################
// Function Name
//#####################################################################
template<class T> std::string TRIANGLE_3D<T>::
Name()
{
    return "TRIANGLE_3D<T>";
}
//#####################################################################
template class TRIANGLE_3D<float>;
template class TRIANGLE_3D<double>;
template TRIANGLE_3D<double> Scaled<double,VECTOR<double,3> >(TRIANGLE_3D<double> const&,VECTOR<double,3> const&,double);
template TRIANGLE_3D<double> Transformed<double,VECTOR<double,3> >(TRIANGLE_3D<double> const&,FRAME<VECTOR<double,3> > const&);
template TRIANGLE_3D<float> Scaled<float,VECTOR<float,3> >(TRIANGLE_3D<float> const&,VECTOR<float,3> const&,float);
template TRIANGLE_3D<float> Transformed<float,VECTOR<float,3> >(TRIANGLE_3D<float> const&,FRAME<VECTOR<float,3> > const&);
template RAY_HIT<float> Intersects(const RAY<VECTOR<float,3> >&,const TRIANGLE_3D<float>&,const float);
template RAY_HIT<float> Lazy_Intersects(const RAY<VECTOR<float,3> >&,const TRIANGLE_3D<float>&,const float);
template RAY_HIT<float> Closest_Non_Intersecting_Point(const RAY<VECTOR<float,3> >&,const TRIANGLE_3D<float>&, const float);
template RAY_HIT<double> Intersects(const RAY<VECTOR<double,3> >&,const TRIANGLE_3D<double>&,const double);
template RAY_HIT<double> Lazy_Intersects(const RAY<VECTOR<double,3> >&,const TRIANGLE_3D<double>&,const double);
template RAY_HIT<double> Closest_Non_Intersecting_Point(const RAY<VECTOR<double,3> >&,const TRIANGLE_3D<double>&, const double);
}
