How to (roughly) build a KDTree for mesh-ray intersection calculations

fd_tree_pseudocode.cpp

// https://en.wikipedia.org/wiki/K-d_tree
// On building fast kd-Trees for Ray Tracing, and on doing that in O(N log N)
//     http://www.irisa.fr/prive/kadi/Sujets_CTR/kadi/Kadi_sujet2_article_Kdtree.pdf
// https://axom.readthedocs.io/en/develop/index.html#
// https://web.archive.org/web/20090803054252/http://tog.acm.org/resources/GraphicsGems/gems/RayBox.c


#include <math.h>
#include <memory>
#include <vector>
#include <algorithm>


///////////////
// Types
///////////////

enum AxisPlane {
    X=0, Y=1, Z=2
};

struct AxisAlignedPlane {
    AxisPlane axis;
    double position;
};

enum PlanarVoxelSide {
    LEFT,
    RIGHT
};

typedef Point std::array<double, 3>;
typedef Triangle std::array<Point, 3>;
typedef Polygon std::vector<Point>;

struct Voxel {
    int depth = 0;
    Point min_extent;
    Point max_extent;
};

enum EventType {
    END = 0,
    PLANAR = 1,
    START = 2
};

enum PlaneSide {
    LEFT,
    RIGHT,
    BOTH
};

struct Event {
    double position;
    AxisPlane axis;
    EventType type;
    int triangle_id;
    PlaneSide side;
};

struct BoundingBox {
    int n_points = 0;
    Point min_extent;
    Point max_extent;
    void addPoint(Point p) {
        if(n_points == 0) {
            for(unsigned int i=0; i<3; ++i) {
                min_extent[i] = p[i];
                max_extent[i] = p[i];
            }
        } else {
            for(unsigned int i=0; i<3; ++i) {
                min_extent[i] = std::min(min_extent[i], p[i]);
                max_extent[i] = std::max(max_extent[i], p[i]);
            }
        }
        n_points += 1;
    }
};

bool BBoxContainsBBox(BoundingBox const & a, BoundingBox const & b) {
    return  a.min_extent[0] <= b.min_extent[0] and a.max_extent[0] >= b.max_extent[0]
        and a.min_extent[1] <= b.min_extent[1] and a.max_extent[1] >= b.max_extent[1]
        and a.min_extent[2] <= b.min_extent[2] and a.max_extent[2] >= b.max_extent[2];
}

///////////////
// Find plane
///////////////

double CostLambda(int NTrianglesLeft, int NTrianglesRight) {
    double cost = 1.0;
    if(NTrianglesLeft == 0 or NTrianglesRight == 0) {
        return 0.8;
    } else {
        return 1.0;
    }
}

double Cost(double probability_left, double probability_right, int num_left, int num_right, double traversal_cost, double intersection_cost) {
    return CostLambda(NTrianglesLeft, NTrianglesRight) * (
        traversal_cost + intersection_cost
        * (probability_left * num_left + probability_right * num_right)
        );
}

double SurfaceArea(Voxel & V) {
    double area = 0.0;
    Point length;
    for(unsigned int i=0; i<3; ++i) {
        length[i] = std::abs(V.max_extent[i] - V.min_extent[i]);
    }
    return 2.0 * (Point[0] * (Point[1] + Point[2]) + Point[1] * Point[2]);
}

void SplitBox(Voxel const & V, Plane const & p, Voxel & VL, Voxel & VR) {
    int axis = int(p.axis);
    VL = V;
    VL.depth += 1;
    VR = VL;
    VL.max_extent[axis] = p.position;
    VR.min_extent[axis] = p.position;
}

std::tuple<double, PlanarVoxelSide> SAHCost(Plane const & p, Voxel const & V, int num_left, int num_right, int num_planar, double traversal_cost, double intersection_cost) {
    Voxel VL, VR;
    SplitBox(V, p, VL, VR); // Make two voxels from one
    double SA_tot = SurfaceArea(V);
    double SA_L = SurfaceArea(VL);
    double SA_R = SurfaceArea(VR);
    double PL = SA_L / SA_tot; // Probability of hitting voxel is proportional to surface area
    double PR = SA_R / SA_tot;

    // Check what is more expensive: planar events on left side, or planar events on right side
    double CpL = Cost(PL, PR, num_left + num_planar, num_right, traversal_cost, intersection_cost);
    double CpR = Cost(PL, PR, num_left, num_right + num_planar, traversal_cost, intersection_cost);
    if(CpL < CpR) {
        return std::tuple<double, PlanarVoxelSide>(CpL, LEFT);
    } else {
        return std::tuple<double, PlanarVoxelSide>(CpR, RIGHT);
    }
}

std::tuple<AxisAlignedPlane, PlanarVoxelSide, double> FindPlane(int Num_tris, Voxel const & voxel, std::vector<Event> const & events, double traversal_cost, double intersection_cost) {
    // Find the lowest cost plane across all 3 dimensions
    // Relevant plane positions are when a triangle swaps sides from one voxel to another
    // Assume events vector is already sorted
    std::array<int, 3> NL = {0, 0, 0}; // Number of triangles left of the plane
    std::array<int, 3> NP = {0, 0, 0}; // Number of triangles in the plane
    std::array<int, 3> NR = {Num_tris, Num_tris, Num_tris}; // Number of triangles right of the plane
    AxisAlignedPlane best_plane;
    PlanarVoxelSide best_side;
    double best_cost = 0;
    bool have_cost = false;
    // Sweep across the possible planes, keeping track of how many triangles are in each voxel
    // Loop does 3 dimensions simultaneously ==> Triangle counts are sepearate for each dimension
    for(unsigned int i=0; i<events.size(); ++i) {
        AxisAlignedPlane plane;
        plane.axis = events[i].axis;
        plane.position = events[i].position;
        int dim = int(axis);
        int p_start = 0;
        int p_end = 0;
        int p_planar = 0;
        while(i < events.size() and events[i].axis == plane.axis and events[i].position == plane.position and events[i].type == EventType::END) {
            ++p_end; ++i;
        }
        while(i < events.size() and events[i].axis == plane.axis and events[i].position == plane.position and events[i].type == EventType::PLANAR) {
            ++p_planar; ++i;
        }
        while(i < events.size() and events[i].axis == plane.axis and events[i].position == plane.position and events[i].type == EventType::START) {
            ++p_start; ++i;
        }
        
        NP[dim] = p_planar;
        NR[dim] -= p_planar + p_end;
        
        std::tuple<double, PlanarVoxelSide> cost_side = 
            SAHCost(plane, voxel, NL[dim], NR[dim], NP[dim], traversal_cost, intersection_cost);
        if((not have_cost) or std::get<0>(cost_side) < best_cost) {
            have_cost = true;
            best_cost = std::get<0>(cost_side);
            best_side = std::get<1>(cost_side);
            best_plane = plane;
        }

        NL[dim] += p_planar + p_start;
        NP[dim] = 0;
    }
    return std::tuple<AxisAlignedPlane, PlanarVoxelSide, double>(best_plane, best_side, best_cost);
}

///////////////
// Fast Box-Triangle intersection check
///////////////

/* this version of SIGN3 shows some numerical instability, and is improved
 * by using the uncommented macro that follows, and a different test with it */
#ifdef OLD_TEST
    #define SIGN3( A ) (((A).x<0)?4:0 | ((A).y<0)?2:0 | ((A).z<0)?1:0)
#else
    #define EPS 10e-5
    #define SIGN3( A ) \
      (((A).x < EPS) ? 4 : 0 | ((A).x > -EPS) ? 32 : 0 | \
       ((A).y < EPS) ? 2 : 0 | ((A).y > -EPS) ? 16 : 0 | \
       ((A).z < EPS) ? 1 : 0 | ((A).z > -EPS) ? 8 : 0)
#endif

#define CROSS( A, B, C ) { \
  (C).x =  (A).y * (B).z - (A).z * (B).y; \
  (C).y = -(A).x * (B).z + (A).z * (B).x; \
  (C).z =  (A).x * (B).y - (A).y * (B).x; \
   }
#define SUB( A, B, C ) { \
  (C).x =  (A).x - (B).x; \
  (C).y =  (A).y - (B).y; \
  (C).z =  (A).z - (B).z; \
   }
#define LERP( A, B, C) ((B)+(A)*((C)-(B)))
#define MIN3(a,b,c) ((((a)<(b))&&((a)<(c))) ? (a) : (((b)<(c)) ? (b) : (c)))
#define MAX3(a,b,c) ((((a)>(b))&&((a)>(c))) ? (a) : (((b)>(c)) ? (b) : (c)))
#define INSIDE 0
#define OUTSIDE 1

typedef struct {
      float           x;
      float           y;
      float           z;
      } Point3;

typedef struct{
   Point3 v1;                 /* Vertex1 */
   Point3 v2;                 /* Vertex2 */
   Point3 v3;                 /* Vertex3 */
   } Triangle3; 
   
/*___________________________________________________________________________*/

/* Which of the six face-plane(s) is point P outside of? */

long face_plane(Point3 p)
{
long outcode;

   outcode = 0;
   if (p.x >  .5) outcode |= 0x01;
   if (p.x < -.5) outcode |= 0x02;
   if (p.y >  .5) outcode |= 0x04;
   if (p.y < -.5) outcode |= 0x08;
   if (p.z >  .5) outcode |= 0x10;
   if (p.z < -.5) outcode |= 0x20;
   return(outcode);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Which of the twelve edge plane(s) is point P outside of? */

long bevel_2d(Point3 p)
{
long outcode;

   outcode = 0;
   if ( p.x + p.y > 1.0) outcode |= 0x001;
   if ( p.x - p.y > 1.0) outcode |= 0x002;
   if (-p.x + p.y > 1.0) outcode |= 0x004;
   if (-p.x - p.y > 1.0) outcode |= 0x008;
   if ( p.x + p.z > 1.0) outcode |= 0x010;
   if ( p.x - p.z > 1.0) outcode |= 0x020;
   if (-p.x + p.z > 1.0) outcode |= 0x040;
   if (-p.x - p.z > 1.0) outcode |= 0x080;
   if ( p.y + p.z > 1.0) outcode |= 0x100;
   if ( p.y - p.z > 1.0) outcode |= 0x200;
   if (-p.y + p.z > 1.0) outcode |= 0x400;
   if (-p.y - p.z > 1.0) outcode |= 0x800;
   return(outcode);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Which of the eight corner plane(s) is point P outside of? */

long bevel_3d(Point3 p)
{
long outcode;

   outcode = 0;
   if (( p.x + p.y + p.z) > 1.5) outcode |= 0x01;
   if (( p.x + p.y - p.z) > 1.5) outcode |= 0x02;
   if (( p.x - p.y + p.z) > 1.5) outcode |= 0x04;
   if (( p.x - p.y - p.z) > 1.5) outcode |= 0x08;
   if ((-p.x + p.y + p.z) > 1.5) outcode |= 0x10;
   if ((-p.x + p.y - p.z) > 1.5) outcode |= 0x20;
   if ((-p.x - p.y + p.z) > 1.5) outcode |= 0x40;
   if ((-p.x - p.y - p.z) > 1.5) outcode |= 0x80;
   return(outcode);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Test the point "alpha" of the way from P1 to P2 */
/* See if it is on a face of the cube              */
/* Consider only faces in "mask"                   */

long check_point(Point3 p1, Point3 p2, float alpha, long mask)
{
Point3 plane_point;

   plane_point.x = LERP(alpha, p1.x, p2.x);
   plane_point.y = LERP(alpha, p1.y, p2.y);
   plane_point.z = LERP(alpha, p1.z, p2.z);
   return(face_plane(plane_point) & mask);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Compute intersection of P1 --> P2 line segment with face planes */
/* Then test intersection point to see if it is on cube face       */
/* Consider only face planes in "outcode_diff"                     */
/* Note: Zero bits in "outcode_diff" means face line is outside of */

long check_line(Point3 p1, Point3 p2, long outcode_diff)
{

   if ((0x01 & outcode_diff) != 0)
      if (check_point(p1,p2,( 0.5f-p1.x)/(p2.x-p1.x),0x3e) == INSIDE) return(INSIDE);
   if ((0x02 & outcode_diff) != 0)
      if (check_point(p1,p2,(-0.5f-p1.x)/(p2.x-p1.x),0x3d) == INSIDE) return(INSIDE);
   if ((0x04 & outcode_diff) != 0) 
      if (check_point(p1,p2,( 0.5f-p1.y)/(p2.y-p1.y),0x3b) == INSIDE) return(INSIDE);
   if ((0x08 & outcode_diff) != 0) 
      if (check_point(p1,p2,(-0.5f-p1.y)/(p2.y-p1.y),0x37) == INSIDE) return(INSIDE);
   if ((0x10 & outcode_diff) != 0) 
      if (check_point(p1,p2,( 0.5f-p1.z)/(p2.z-p1.z),0x2f) == INSIDE) return(INSIDE);
   if ((0x20 & outcode_diff) != 0) 
      if (check_point(p1,p2,(-0.5f-p1.z)/(p2.z-p1.z),0x1f) == INSIDE) return(INSIDE);
   return(OUTSIDE);
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/* Test if 3D point is inside 3D triangle */

long point_triangle_intersection(Point3 p, Triangle3 t)
{
long sign12,sign23,sign31;
Point3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
Point3 cross12_1p,cross23_2p,cross31_3p;

/* First, a quick bounding-box test:                               */
/* If P is outside triangle bbox, there cannot be an intersection. */

   if (p.x > MAX3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);  
   if (p.y > MAX3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z > MAX3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);
   if (p.x < MIN3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);
   if (p.y < MIN3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z < MIN3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);

/* For each triangle side, make a vector out of it by subtracting vertexes; */
/* make another vector from one vertex to point P.                          */
/* The crossproduct of these two vectors is orthogonal to both and the      */
/* signs of its X,Y,Z components indicate whether P was to the inside or    */
/* to the outside of this triangle side.                                    */

   SUB(t.v1, t.v2, vect12)
   SUB(t.v1,    p, vect1h);
   CROSS(vect12, vect1h, cross12_1p)
   sign12 = SIGN3(cross12_1p);      /* Extract X,Y,Z signs as 0..7 or 0...63 integer */

   SUB(t.v2, t.v3, vect23)
   SUB(t.v2,    p, vect2h);
   CROSS(vect23, vect2h, cross23_2p)
   sign23 = SIGN3(cross23_2p);

   SUB(t.v3, t.v1, vect31)
   SUB(t.v3,    p, vect3h);
   CROSS(vect31, vect3h, cross31_3p)
   sign31 = SIGN3(cross31_3p);

/* If all three crossproduct vectors agree in their component signs,  */
/* then the point must be inside all three.                           */
/* P cannot be OUTSIDE all three sides simultaneously.                */

   /* this is the old test; with the revised SIGN3() macro, the test
    * needs to be revised. */
#ifdef OLD_TEST
   if ((sign12 == sign23) && (sign23 == sign31))
      return(INSIDE);
   else
      return(OUTSIDE);
#else
   return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE;
#endif
}

/*. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . */

/**********************************************/
/* This is the main algorithm procedure.      */
/* Triangle t is compared with a unit cube,   */
/* centered on the origin.                    */
/* It returns INSIDE (0) or OUTSIDE(1) if t   */
/* intersects or does not intersect the cube. */
/**********************************************/

long t_c_intersection(Triangle3 t)
{
long v1_test,v2_test,v3_test;
float d,denom;
Point3 vect12,vect13,norm;
Point3 hitpp,hitpn,hitnp,hitnn;

/* First compare all three vertexes with all six face-planes */
/* If any vertex is inside the cube, return immediately!     */

   if ((v1_test = face_plane(t.v1)) == INSIDE) return(INSIDE);
   if ((v2_test = face_plane(t.v2)) == INSIDE) return(INSIDE);
   if ((v3_test = face_plane(t.v3)) == INSIDE) return(INSIDE);

/* If all three vertexes were outside of one or more face-planes, */
/* return immediately with a trivial rejection!                   */

   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);

/* Now do the same trivial rejection test for the 12 edge planes */

   v1_test |= bevel_2d(t.v1) << 8; 
   v2_test |= bevel_2d(t.v2) << 8; 
   v3_test |= bevel_2d(t.v3) << 8;
   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);  

/* Now do the same trivial rejection test for the 8 corner planes */

   v1_test |= bevel_3d(t.v1) << 24; 
   v2_test |= bevel_3d(t.v2) << 24; 
   v3_test |= bevel_3d(t.v3) << 24; 
   if ((v1_test & v2_test & v3_test) != 0) return(OUTSIDE);   

/* If vertex 1 and 2, as a pair, cannot be trivially rejected */
/* by the above tests, then see if the v1-->v2 triangle edge  */
/* intersects the cube.  Do the same for v1-->v3 and v2-->v3. */
/* Pass to the intersection algorithm the "OR" of the outcode */
/* bits, so that only those cube faces which are spanned by   */
/* each triangle edge need be tested.                         */

   if ((v1_test & v2_test) == 0)
      if (check_line(t.v1,t.v2,v1_test|v2_test) == INSIDE) return(INSIDE);
   if ((v1_test & v3_test) == 0)
      if (check_line(t.v1,t.v3,v1_test|v3_test) == INSIDE) return(INSIDE);
   if ((v2_test & v3_test) == 0)
      if (check_line(t.v2,t.v3,v2_test|v3_test) == INSIDE) return(INSIDE);

/* By now, we know that the triangle is not off to any side,     */
/* and that its sides do not penetrate the cube.  We must now    */
/* test for the cube intersecting the interior of the triangle.  */
/* We do this by looking for intersections between the cube      */
/* diagonals and the triangle...first finding the intersection   */
/* of the four diagonals with the plane of the triangle, and     */
/* then if that intersection is inside the cube, pursuing        */
/* whether the intersection point is inside the triangle itself. */

/* To find plane of the triangle, first perform crossproduct on  */
/* two triangle side vectors to compute the normal vector.       */  
                                
   SUB(t.v1,t.v2,vect12);
   SUB(t.v1,t.v3,vect13);
   CROSS(vect12,vect13,norm)

/* The normal vector "norm" X,Y,Z components are the coefficients */
/* of the triangles AX + BY + CZ + D = 0 plane equation.  If we   */
/* solve the plane equation for X=Y=Z (a diagonal), we get        */
/* -D/(A+B+C) as a metric of the distance from cube center to the */
/* diagonal/plane intersection.  If this is between -0.5 and 0.5, */
/* the intersection is inside the cube.  If so, we continue by    */
/* doing a point/triangle intersection.                           */
/* Do this for all four diagonals.                                */

   d = norm.x * t.v1.x + norm.y * t.v1.y + norm.z * t.v1.z;

   /* if one of the diagonals is parallel to the plane, the other will intersect the plane */
   if(fabs(denom=(norm.x + norm.y + norm.z))>EPS)
   /* skip parallel diagonals to the plane; division by 0 can occur */
   {
      hitpp.x = hitpp.y = hitpp.z = d / denom;
      if (fabs(hitpp.x) <= 0.5)
         if (point_triangle_intersection(hitpp,t) == INSIDE) return(INSIDE);
   }
   if(fabs(denom=(norm.x + norm.y - norm.z))>EPS)
   {
      hitpn.z = -(hitpn.x = hitpn.y = d / denom);
      if (fabs(hitpn.x) <= 0.5)
         if (point_triangle_intersection(hitpn,t) == INSIDE) return(INSIDE);
   }       
   if(fabs(denom=(norm.x - norm.y + norm.z))>EPS)
   {       
      hitnp.y = -(hitnp.x = hitnp.z = d / denom);
      if (fabs(hitnp.x) <= 0.5)
         if (point_triangle_intersection(hitnp,t) == INSIDE) return(INSIDE);
   }
   if(fabs(denom=(norm.x - norm.y - norm.z))>EPS)
   {
      hitnn.y = hitnn.z = -(hitnn.x = d / denom);
      if (fabs(hitnn.x) <= 0.5)
         if (point_triangle_intersection(hitnn,t) == INSIDE) return(INSIDE);
   }
   
/* No edge touched the cube; no cube diagonal touched the triangle. */
/* We're done...there was no intersection.                          */

   return(OUTSIDE);

}

///////////////
// Clip triangle to voxel
///////////////

double dot(Point const & a, Point const & b) {
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

Point mul(Point const & a, double x) {
    Point ret = a;
    ret[0] *= x;
    ret[1] *= x;
    ret[2] *= x;
    return ret;
}

Point add(Point const & a, Point, const & b) {
    Point ret = a;
    ret[0] += b[0];
    ret[1] += b[1];
    ret[2] += b[2];
    return ret
}

Point subtract(Point const & a, Point, const & b) {
    Point ret = a;
    ret[0] -= b[0];
    ret[1] -= b[1];
    ret[2] -= b[2];
    return ret
}

template <typename T>
inline void swap(T& a, T& b) {
    T tmp = a;
    a = b;
    b = tmp;
}

enum OrientationResult {
    ON_BOUNDARY,      /*!< primitive is on the boundary of a primitive      */
    ON_POSITIVE_SIDE, /*!< primitive is on the positive side of a primitive */
    ON_NEGATIVE_SIDE  /*!< primitive is on the negative side of a primitive */
};

bool isEven(int i) {
    return i % 2 == 0;
}

int classifyPointAxisPlane(Point const & pt,
                           int index,
                           double val,
                           const double eps = 1e-8) {
    // Note: we are exploiting the fact that the planes are axis aligned
    // So the dot product is +/- the given coordinate.
    // In general, we would need to call distance(pt, plane) here
    double dist = isEven(index) ? val - pt[index / 2] : pt[index / 2] - val;

    if(dist > eps) {
        return ON_POSITIVE_SIDE;
    }
    if(dist < -eps) {
        return ON_NEGATIVE_SIDE;
    }

    return ON_BOUNDARY;
}


Point findIntersectionPoint(Point const & a,
                                      Point const & b,
                                      int index,
                                      double val)
{
    // Need to find a parameter t for the point pt, such that,
    // * 0 <= t <= 1
    // * pt = a + t * (b-a)
    // * pt[ index/2]  == val

    double t = (val - a[index / 2]) / (b[index / 2] - a[index / 2]);

    Point ret = add(a, mul(subtract(b, a), t));

    return ret;
}

void clipAxisPlane(Polygon const * prevPoly,
                   Polygon * currentPoly,
                   int index,
                   T val)
{

    currentPoly->clear();
    int numVerts = prevPoly->size();
    if(numVerts == 0) {
        return;
    }
    // Initialize point a with the last vertex of the polygon
    const Point* a = &(*prevPoly)[numVerts - 1];
    int aSide = classifyPointAxisPlane(*a, index, val);
    for(int i = 0; i < numVerts; ++i) {
        const Point* b = &(*prevPoly)[i];
        int bSide = classifyPointAxisPlane(*b, index, val);
        switch(bSide) {
            case ON_POSITIVE_SIDE:
                if(aSide == ON_NEGATIVE_SIDE) {
                    currentPoly->push_back(findIntersectionPoint(*a, *b, index, val));
                }
                break;
            case ON_BOUNDARY:
                if(aSide == ON_NEGATIVE_SIDE) {
                    currentPoly->push_back(*b);
                }
                break;
            case ON_NEGATIVE_SIDE:
                switch(aSide) {
                  case ON_POSITIVE_SIDE:
                      currentPoly->push_back(findIntersectionPoint(*a, *b, index, val));
                      currentPoly->push_back(*b);
                      break;
                  case ON_BOUNDARY:
                      currentPoly->push_back(*a);
                      currentPoly->push_back(*b);
                      break;
                  case ON_NEGATIVE_SIDE:
                      currentPoly->push_back(*b);
                      break;
                }
                break;
        }
        // swap a and b
        a = b;
        aSide = bSide;
    }
}

bool TriangleIntersectsAABB(Triangle tri, BoundingBox const & box) {
    Point lengths;
    lengths[0] = box.max_extent[0] - box.min_extent[0];
    lengths[1] = box.max_extent[1] - box.min_extent[1];
    lengths[2] = box.max_extent[2] - box.min_extent[2];

    for(unsigned int p_i=0; p_i<3; ++p_i) {
        for(unsigned int dim=0; dim<3; ++dim) {
            tri[p_i][dim] -= box.min_extent[dim];
            tri[p_i][dim] /= lengths[dim];
        }
    }

    Point3 v1, v2, v3;
    v1.x = tri[0][0];
    v1.y = tri[0][1];
    v1.z = tri[0][2];
    v2.x = tri[1][0];
    v2.y = tri[1][1];
    v3.z = tri[1][2];
    v3.x = tri[2][0];
    v3.y = tri[2][1];
    v3.z = tri[2][2];
    Triangle3 new_tri;
    new_tri.v1 = v1;
    new_tri.v2 = v2;
    new_tri.v3 = v3;
    return t_c_intersection(new_tri) == INSIDE;
}

bool TriangleVoxelIntersects(Triangle const & tri, Voxel const & voxel) {
    BoundingBox box;
    box.min_extent = voxel.min_extent;
    box.min_extent = voxel.min_extent;
    return TriangleIntersectsAABB(tri, box);
}

Polygon clip(Triangle const & tri, BoundingBox const & bbox) {

    // Use two polygons with pointers for 'back-buffer'-like swapping
    const int MAX_VERTS = 6;
    Polygon poly[2] = {Polygon(MAX_VERTS), Polygon(MAX_VERTS)};
    Polygon* currentPoly = &poly[0];
    Polygon* prevPoly = &poly[1];

    // First check if the triangle is contained in the bbox, if not we are empty
    std::vector<Point> triBox;
    BoundingBox triBox;
    triBox.addPoint(tri[0]);
    triBox.addPoint(tri[1]);
    triBox.addPoint(tri[2]);

    if(not TriangleIntersectsAABB(triBox, bbox)) {
        return *currentPoly;  // note: currentPoly is empty
    }

    // Set up the initial polygon
    currentPoly->push_back(tri[0]);
    currentPoly->push_back(tri[1]);
    currentPoly->push_back(tri[2]);

    // If all the vertices are contained, we have no work to do
    if(BBoxContainsBBox(bbox, triBox)) {
        return *currentPoly;  // Note current poly has the same verts as tri
    }

    // Loop through the planes of the bbox and clip the vertices
    for(int dim = 0; dim < 3; ++dim) {
        // Optimization note: we should be able to save some work based on
        // the clipping plane and the triangle's bounding box

        if(triBox.max_extent[dim] > bbox.min_extent[dim]) {
            swap(prevPoly, currentPoly);
            clipAxisPlane(prevPoly, currentPoly, 2 * dim + 0, bbox.min_extent[dim]);
        }
        if(triBox.min_extent[dim] < bbox.max_extent[dim]) {
            swap(prevPoly, currentPoly);
            clipAxisPlane(prevPoly, currentPoly, 2 * dim + 1, bbox.max_extent[dim]);
        }
    }

    return *currentPoly;
}

///////////////
// Split events
///////////////

void ClassifyEventLeftRightBoth(std::vector<Event> & E, Plane const & p, PlanarVoxelSide side) {
    for(unsigned int i=0; i<E.size(); ++i) {
        Event & e = E[i];
        E[i].side = PlaneSide::BOTH;
    }
    PlaneSide default_planar_side;
    if(side == PlanarVoxelSide::LEFT) {
        default_planar_side = PlaneSide::LEFT;
    } else {
        default_planar_side = PlaneSide::RIGHT;
    }
    for(unsigned int i=0; i<E.size(); ++i) {
        Event & e = E[i];
        if(e.type == EventType::END
            and e.axis == p.axis
            and e.position <= p.position) {
            e.side = PlaneSide::LEFT;
        } else if (e.type == EventType::START
            and e.axis == p.axis
            and e.position >= p.position) {
            e.side = PlaneSide::RIGHT
        } else if (e.type == EventType::PLANAR
            and e.axis == p.axis) {
            if(e.position < p.position) {
                e.side = PlaneSide::LEFT;
            } else if(e.position > p.position) {
                e.side = PlaneSide::RIGHT;
            } else if(e.position == p.position) {
                e.side = default_planar_side;
            }
        }
    }
}

void AddPlanarEvent(std::vector<Event> & events, BoundingBox const & tri_box, PlaneAxis axis, int tri_id) {
    Event e;
    int dim = int(axis);
    e.position = tri_box.min_extent[dim];
    e.axis = axis;
    e.type = EventType::PLANAR;
    e.triangle_id = tri_id;
    e.side = PlanarVoxelSide::BOTH;
    events.push_back(e);
}

void AddStartEndEvents(std::vector<Event> & events, BoundingBox const & tri_box, PlaneAxis axis, int tri_id) {
    Event e;
    int dim = int(axis);
    e.position = tri_box.min_extent[dim];
    e.axis = axis;
    e.type = EventType::START;
    e.triangle_id = tri_id;
    e.side = PlanarVoxelSide::BOTH;
    events.push_back(e);
    Event e_end;
    e.position = tri_box.max_extent[dim];
    e.type = EventType::END;
    events.push_back(e);
}

void GenerateNonClippedTriangleVoxelEvents(std::vector<Event> & events, Triangle const & tri, int tri_id) {
    BoundingBox box;
    for(unsigned int point_i=0; point_i<3; ++point_i) {
        box.addPoint(tri[point_i]);
    }

    for(unsigned int dim_i=0; dim_i<3; ++dim_i) {
        if(box.min_extent[dim_i] == box.max_extent[dim_i]) {
            // Planar event
            AddPlanarEvent(events, box, PlaneAxis(dim_i), tri_id);
        } else {
            // Start and End events
            AddStartEndEvents(events, box, PlaneAxis(dim_i), tri_id);
        }
    }
}

void GenerateClippedTriangleVoxelEvents(std::vector<Event> & events, Triangle const & tri, int tri_id, BoundingBox const & voxel_box) {
    Polygon p = clip(tri, voxel_box);

    BoundingBox box;
    for(unsigned int point_i=0; point_i<p.size(); ++point_i) {
        box.addPoint(p[point_i]);
    }

    for(unsigned int dim_i=0; dim_i<3; ++dim_i) {
        if(box.min_extent[dim_i] == box.max_extent[dim_i]) {
            // Planar event
            AddPlanarEvent(events, box, PlaneAxis(dim_i), tri_id);
        } else {
            // Start and End events
            AddStartEndEvents(events, box, PlaneAxis(dim_i), tri_id);
        }
    }
}

void GeneratePlaneEvents(std::vector<Event> & events_L, std::vector<Event> & events_R, std::vector<Triangle> const & tiangles, std::vector<int> const & intersecting_tris, Voxel const & voxel, Plane const & plane) {
    Voxel VL;
    Voxel VR;
    SplitBox(voxel, plane, VL, VR);
    BoundingBox BBL, BBR;
    BBL.min_extent = VL.min_extent;
    BBL.max_extent = VL.max_extent;
    BBR.min_extent = VR.min_extent;
    BBR.max_extent = VR.max_extent;

    for(unsigned int i=0; i<triangles.size()) {
        GenerateClippedTriangleVoxelEvents(events_L, triangles[intersecting_tris[i]], intersecting_tris[i], BBL);
        GenerateClippedTriangleVoxelEvents(events_R, triangles[intersecting_tris[i]], intersecting_tris[i], BBR);
    }
}

int TauEventType(EventType etype) {
    return int(etype);
}

bool EventCompare(Event const & a, Event const & b) {
    // Is a < b
    return (a.position < b.position)
        or (a.position == b.position and TauEventType(a) < TauEventType(b));
}

void SplitEventsByPlane(std::vector<Event> const & events,
    std::vector<Triangle> const & triangles,
    Voxel const & voxel,
    AxisAlignedPlane const & plane,
    std::vector<Event> & EL,
    std::vector<Event> & ER,
    PlanarVoxelSide const & side) {
    std::vector<Event> ELtemp;
    std::vector<Event> ERtemp;
    std::vector<Event> EBLtemp;
    std::vector<Event> EBRtemp;
    ClassifyEventLeftRightBoth(events, plane, side);
    std::vector<int> intersecting_tris;
    for(unsigned int i=0; i<events.size(); ++i) {
        Event & e = events[i];
        if(e.side == PlaneSide::BOTH) {
            intersecting_tris.push_back(e.triangle_id);
        } else if (e.side == PlaneSide::LEFT) {
            ELtemp.push_back(e);
        } else if (e.side == PlaneSide::RIGHT) {
            ERtemp.push_back(e);
        }
    }
    GeneratePlaneEvents(EBLtemp, EBRtemp, triangles, intersecting_tris, voxel, plane);
    std::sort(EBLtemp.begin(), EBLtemp.end(), EventCompare);
    std::sort(EBRtemp.begin(), EBRtemp.end(), EventCompare);
    std::merge(ELtemp.begin(), ELtemp.end(), EBLtemp.begin(), EBLtemp.end(), EL.begin(), EventCompare);
    std::merge(ERtemp.begin(), ERtemp.end(), EBRtemp.begin(), EBRtemp.end(), ER.begin(), EventCompare);
}

///////////////
// Build KD Tree
///////////////

struct KDNode {
    bool terminal = false;
    Voxel V;
    std::vector<int> T;
    std::shared_ptr<KDNode> left;
    std::shared_ptr<KDNode> right;
    KDNode(Voxel const & voxel, std::vector<int> const & tri_idx) :
        terminal(true), V(voxel), T(tri_idx) {};
    KDNode(Voxel const & voxel, std::vector<int> const & tri_idx, std::shared_ptr<KDNode> l, std::shared_ptr<KDNode> r) :
        terminal(false), V(voxel), left(l), right(r) {};
};

std::shared_ptr<KDNode> RecBuild(std::vector<Triangle> const & triangles, std::vector<int> const & T, Voxel & V, std::vector<Event> const & events, double traversal_cost, double intersection_cost) {
    std::tuple<AxisAlignedPlane, PlanarVoxelSide, double> plane_side_cost = FindPlane(T.size(), V, events, traversal_cost, intersection_cost);
    double cost = std::get<2>();
    double termination_cost = intersection_cost * T.size();

    if(cost > termination_cost) {
        return std::make_shared<KDNode>(V, T);
    }

    AxisAlignedPlane const & plane = std::get<0>();
    PlanarVoxelSide const & side = std::get<1>();

    std::vector<Event> EL, ER;
    SplitEventsByPlane(events, triangles, V, plane, EL, ER, side);
    Voxel VL, VR;
    SplitBox(V, plane, VL, VR);
    return std::make_shared<KDNode>(V, RecBuild(triangles, TL, VL, EL, traversal_cost, intersection_cost), RecBuild(triangles, TR, VR, ER, traversal_cost, intersection_cost));
}

std::shared_ptr<KDNode> BuildKDTree(std::vector<Triangle> const & triangles, double traversal_cost, double intersection_cost) {
    // Generate all events and compute world bounding box
    BoundingBox box;
    std::vector<Event> events;
    for(unsigned int i=0; i<triangles.size(); ++i) {
        GenerateNonClippedTriangleVoxelEvents(events, triangles[i], i);
        box.addPoint(triangles[i][0]);
        box.addPoint(triangles[i][1]);
        box.addPoint(triangles[i][2]);
    }

    // Sort events once
    std::sort(std::begin(events), std::end(events), EventCompare);

    // Make the world voxel
    Voxel V;
    V.min_extent = box.min_extent;
    V.max_extent = box.max_extent;
    V.depth = 0;

    // Start with all triangle IDs
    std::vector<int> T(triangles.size());
    std::iota(std::begin(T), std::end(T), 0); // Fill T with 0, 1, ..., triangles.size()

    // Recursively build the tree
    return RecBuild(triangles, T, V, events, traversal_cost, intersection_cost);
}