//=============================================================================
// AtiRayTriMoller.cpp -- Optimized ray/triangle intersect routines. Sushi-ified
// Found at: http://www.ce.chalmers.se/staff/tomasm/code/tribox2.txt
// 
// Original comments:
//
// AABB-triangle overlap test code
// by Tomas Akenine-Möller
// Function: int triBoxOverlap(float boxcenter[3],
//          float boxhalfsize[3],float triverts[3][3]);
// History:
//   2001-03-05: released the code in its first version
//   2001-06-18: changed the order of the tests, faster
//
// Acknowledgement: Many thanks to Pierre Terdiman for
// suggestions and discussions on how to optimize code.
// Thanks to David Hunt for finding a ">="-bug!
//=============================================================================
// $File: //depot/3darg/Demos/Sushi/Math/AtiTriBoxMoller.cpp $ $Revision: #1 $ $Author: gosselin $
//=============================================================================
// (C) 2003 ATI Research, Inc., All rights reserved.
//=============================================================================

#include <math.h>
#include <stdio.h>

#include "AtiTriBoxMoller.h"

#define X 0
#define Y 1
#define Z 2

#define CROSS(dest,v1,v2) \
          dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
          dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
          dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2) \
          dest[0]=v1[0]-v2[0]; \
          dest[1]=v1[1]-v2[1]; \
          dest[2]=v1[2]-v2[2];

#define FINDMINMAX(x0,x1,x2,min,max) \
  min = max = x0;   \
  if(x1<min) min=x1;\
  if(x1>max) max=x1;\
  if(x2<min) min=x2;\
  if(x2>max) max=x2;

///////////////////////////////////////////////////////////////////////////////
//
///////////////////////////////////////////////////////////////////////////////
static inline bool8
AtiPlaneBoxOverlap (TBM_FLOAT normal[3], TBM_FLOAT d, TBM_FLOAT maxbox[3])
{
   TBM_FLOAT vmin[3];
   TBM_FLOAT vmax[3];
   for (int32 q = X; q <= Z; q++)
   {
      if (normal[q] > TBM_ZERO)
      {
         vmin[q] = -maxbox[q];
         vmax[q] = maxbox[q];
      }
      else
      {
         vmin[q] = maxbox[q];
         vmax[q] = -maxbox[q];
      }
   }
   if (DOT (normal, vmin) + d > TBM_ZERO)
   {
      return FALSE;
   }
   if (DOT (normal, vmax) + d >= TBM_ZERO)
   {
      return TRUE;
   }
   return FALSE;
}

// ======================== X-tests ========================
#define AXISTEST_X01(a, b, fa, fb)             \
    p0 = a*v0[Y] - b*v0[Z];                    \
    p2 = a*v2[Y] - b*v2[Z];                    \
        if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
    rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return FALSE;

#define AXISTEST_X2(a, b, fa, fb)              \
    p0 = a*v0[Y] - b*v0[Z];                    \
    p1 = a*v1[Y] - b*v1[Z];                    \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return FALSE;

// ======================== Y-tests ========================
#define AXISTEST_Y02(a, b, fa, fb)             \
    p0 = -a*v0[X] + b*v0[Z];                   \
    p2 = -a*v2[X] + b*v2[Z];                       \
        if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return FALSE;

#define AXISTEST_Y1(a, b, fa, fb)              \
    p0 = -a*v0[X] + b*v0[Z];                   \
    p1 = -a*v1[X] + b*v1[Z];                       \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return FALSE;

// ======================== Z-tests ========================

#define AXISTEST_Z12(a, b, fa, fb)             \
    p1 = a*v1[X] - b*v1[Y];                    \
    p2 = a*v2[X] - b*v2[Y];                    \
        if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
    if(min>rad || max<-rad) return FALSE;

#define AXISTEST_Z0(a, b, fa, fb)              \
    p0 = a*v0[X] - b*v0[Y];                \
    p1 = a*v1[X] - b*v1[Y];                    \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
    if(min>rad || max<-rad) return FALSE;

///////////////////////////////////////////////////////////////////////////////
//
///////////////////////////////////////////////////////////////////////////////
bool8
AtiTriBoxMoller (TBM_FLOAT boxcenter[3], TBM_FLOAT boxhalfsize[3],
                 TBM_FLOAT triVert0[3], TBM_FLOAT triVert1[3],
                 TBM_FLOAT triVert2[3])
{
   // use separating axis theorem to test overlap between triangle and box
   // need to test for overlap in these directions:
   // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
   //    we do not even need to test these)
   // 2) normal of the triangle
   // 3) crossproduct(edge from tri, {x,y,z}-directin)
   //    this gives 3x3=9 more tests
   TBM_FLOAT v0[3], v1[3], v2[3];
   TBM_FLOAT min, max, d, p0, p1, p2, rad, fex, fey, fez;
   TBM_FLOAT normal[3], e0[3], e1[3], e2[3];

   // This is the fastest branch on Sun
   // move everything so that the boxcenter is in (0,0,0)
   SUB (v0, triVert0, boxcenter);
   SUB (v1, triVert1, boxcenter);
   SUB (v2, triVert2, boxcenter);

   // compute triangle edges
   SUB (e0, v1, v0);      // tri edge 0
   SUB (e1, v2, v1);      // tri edge 1
   SUB (e2, v0, v2);      // tri edge 2

   // Bullet 3:
   //  test the 9 tests first (this was faster)
   fex = (TBM_FLOAT)fabs (e0[X]);
   fey = (TBM_FLOAT)fabs (e0[Y]);
   fez = (TBM_FLOAT)fabs (e0[Z]);
   AXISTEST_X01 (e0[Z], e0[Y], fez, fey);
   AXISTEST_Y02 (e0[Z], e0[X], fez, fex);
   AXISTEST_Z12 (e0[Y], e0[X], fey, fex);

   fex = (TBM_FLOAT)fabs (e1[X]);
   fey = (TBM_FLOAT)fabs (e1[Y]);
   fez = (TBM_FLOAT)fabs (e1[Z]);
   AXISTEST_X01 (e1[Z], e1[Y], fez, fey);
   AXISTEST_Y02 (e1[Z], e1[X], fez, fex);
   AXISTEST_Z0 (e1[Y], e1[X], fey, fex);

   fex = (TBM_FLOAT)fabs (e2[X]);
   fey = (TBM_FLOAT)fabs (e2[Y]);
   fez = (TBM_FLOAT)fabs (e2[Z]);
   AXISTEST_X2 (e2[Z], e2[Y], fez, fey);
   AXISTEST_Y1 (e2[Z], e2[X], fez, fex);
   AXISTEST_Z12 (e2[Y], e2[X], fey, fex);

   // Bullet 1:
   //  first test overlap in the {x,y,z}-directions
   //  find min, max of the triangle each direction, and test for overlap in
   //  that direction -- this is equivalent to testing a minimal AABB around
   //  the triangle against the AABB

   // test in X-direction
   FINDMINMAX (v0[X], v1[X], v2[X], min, max);
   if (min > boxhalfsize[X] || max < -boxhalfsize[X])
   {
      return FALSE;
   }

   // test in Y-direction
   FINDMINMAX (v0[Y], v1[Y], v2[Y], min, max);
   if (min > boxhalfsize[Y] || max < -boxhalfsize[Y])
   {
      return FALSE;
   }

   // test in Z-direction
   FINDMINMAX (v0[Z], v1[Z], v2[Z], min, max);
   if (min > boxhalfsize[Z] || max < -boxhalfsize[Z])
   {
      return FALSE;
   }

   // Bullet 2:
   //  test if the box intersects the plane of the triangle
   //  compute plane equation of triangle: normal*x+d=0
   CROSS (normal, e0, e1);
   d = -DOT (normal, v0);  // plane eq: normal.x+d=0
   return AtiPlaneBoxOverlap (normal, d, boxhalfsize);
}

