//----------------------------------------------------------------------------
//  EgalTech 2016-2018
//----------------------------------------------------------------------------
// File : CDeBoxTria.cpp   Data : 28.04.18     Versione : 1.9e1
// Contenuto : Implementazione della verifica di collisione tra 
//             BoundingBox e Triangle3d.
//
//
// Modifiche : 05.10.16 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeBoxTria.h"
#include "CDeSpheTria.h"
#include "CDeCylTria.h"
#include "CDeCapsTria.h"
#include "/EgtDev/Include/EGkPlane3d.h"

using namespace std ;

//--------------------------------------------------------------------------------
static bool
CDeBoxPlane( const BBox3d& Box, const Plane3d& Plane)
{
   // vedi Ericson, Real-Time Collision Detection, pag. 164

   // Compute box center and extents
   Point3d  ptCen ;
   Vector3d vtExt ;
   if ( ! Box.GetCenterExtent( ptCen, vtExt))
      return false ;

  // Compute the projection interval radius of b onto L(t) = ptCen + t * Plane.vtN
   double dR = vtExt.x * abs( Plane.GetVersN().x) +
               vtExt.y * abs( Plane.GetVersN().y) +
               vtExt.z * abs( Plane.GetVersN().z) ;

  // Compute distance of box center from plane
   double dS = ( ptCen - ORIG) * Plane.GetVersN() - Plane.GetDist() ;

  // Intersection occurs when distance dS falls within [-dR,+dR] interval
   return ( abs( dS) < dR + EPS_SMALL) ;
}

//-------------------- Macro for X-tests -------------------------------------
#define AXISTEST_X01( a, b, fa, fb)    \
   p0 = a * ptV0.y - b * ptV0.z ;    \
   p2 = a * ptV2.y - b * ptV2.z ;    \
   if ( p0 < p2) { dMin = p0 ; dMax = p2 ; } else { dMin = p2 ; dMax = p0 ; } \
   rad = fa * vtExt.y + fb * vtExt.z ;   \
   if ( dMin > rad + EPS_SMALL  ||  dMax < - rad - EPS_SMALL) return false ;

#define AXISTEST_X2( a, b, fa, fb)     \
   p0 = a * ptV0.y - b * ptV0.z ;    \
   p1 = a * ptV1.y - b * ptV1.z ;    \
   if ( p0 < p1) { dMin = p0 ; dMax = p1 ; } else { dMin = p1 ; dMax = p0 ; } \
   rad = fa * vtExt.y + fb * vtExt.z ;   \
   if ( dMin > rad + EPS_SMALL ||  dMax < -rad - EPS_SMALL) return false ;

//-------------------- Macro for Y-tests -------------------------------------
#define AXISTEST_Y02( a, b, fa, fb)    \
   p0 = - a * ptV0.x + b * ptV0.z ;    \
   p2 = - a * ptV2.x + b * ptV2.z ;    \
   if ( p0 < p2) { dMin = p0 ; dMax = p2 ; } else { dMin = p2 ; dMax = p0 ; } \
   rad = fa * vtExt.x + fb * vtExt.z ;   \
   if ( dMin > rad + EPS_SMALL  ||  dMax < -rad - EPS_SMALL) return false ;

#define AXISTEST_Y1( a, b, fa, fb)     \
   p0 = - a * ptV0.x + b * ptV0.z ;    \
   p1 = - a * ptV1.x + b * ptV1.z ;    \
   if ( p0 < p1) { dMin = p0 ; dMax = p1 ; } else { dMin = p1 ; dMax = p0 ; } \
   rad = fa * vtExt.x + fb * vtExt.z ;   \
   if ( dMin > rad + EPS_SMALL  ||  dMax < -rad - EPS_SMALL) return false ;

//-------------------- Macro for Z-tests -------------------------------------
#define AXISTEST_Z12( a, b, fa, fb)    \
   p1 = a * ptV1.x - b * ptV1.y ;    \
   p2 = a * ptV2.x - b * ptV2.y ;    \
   if ( p2 < p1) { dMin = p2 ; dMax = p1 ; } else { dMin = p1 ; dMax = p2 ; } \
   rad = fa * vtExt.x + fb * vtExt.y ;   \
   if ( dMin > rad + EPS_SMALL  ||  dMax < -rad - EPS_SMALL) return false ;

#define AXISTEST_Z0( a, b, fa, fb)     \
   p0 = a * ptV0.x - b * ptV0.y ;    \
   p1 = a * ptV1.x - b * ptV1.y ;    \
   if ( p0 < p1) { dMin = p0 ; dMax = p1 ; } else { dMin = p1 ; dMax = p0 ; } \
   rad = fa * vtExt.x + fb * vtExt.y ;   \
   if ( dMin > rad + EPS_SMALL  ||  dMax < -rad - EPS_SMALL) return false ;

//----------------------------------------------------------------------------
bool
CDeSimpleBoxTria( const Frame3d& frBox, const Vector3d& vtDiag, const Triangle3d& trTria)
{
  // vedi Ericson, Real-Time Collision Detection, pag. 172 + Akenine-Moller

  // Porto il triangolo nel riferimento del box
   Triangle3d trTriaL = trTria ;
   trTriaL.ToLoc( frBox) ;

  // Calcolo il box locale del triangolo
   BBox3d b3TriaL ;
   trTriaL.GetLocalBBox( b3TriaL) ;

  // Calcolo il box come tale
   BBox3d b3Box( ORIG, ORIG + vtDiag) ;

  // Se i BBox non interferiscono, non c'è collisione
   if ( ! b3Box.Overlaps( b3TriaL))
      return false ;

  // Compute box center and extents
   Point3d  ptCen ;
   Vector3d vtExt ;
   if ( ! b3Box.GetCenterExtent( ptCen, vtExt))
      return false ;

  // Translate triangle as conceptually moving AABB to origin
   Point3d ptV0 = trTriaL.GetP( 0) - ( ptCen - ORIG) ;
   Point3d ptV1 = trTriaL.GetP( 1) - ( ptCen - ORIG) ;
   Point3d ptV2 = trTriaL.GetP( 2) - ( ptCen - ORIG) ;

  // Compute edge vectors for triangle
   Vector3d vtE0 = ptV1 - ptV0 ;
   Vector3d vtE1 = ptV2 - ptV1 ;
   Vector3d vtE2 = ptV0 - ptV2 ;

  // Test axes a00..a22 (category 3)
   double dMin, dMax, p0, p1, p2, rad, fex, fey, fez ;

   fex = abs( vtE0.x) ;
   fey = abs( vtE0.y) ;
   fez = abs( vtE0.z) ;
   AXISTEST_X01( vtE0.z, vtE0.y, fez, fey)
   AXISTEST_Y02( vtE0.z, vtE0.x, fez, fex)
   AXISTEST_Z12( vtE0.y, vtE0.x, fey, fex)

   fex = abs( vtE1.x) ;
   fey = abs( vtE1.y) ;
   fez = abs( vtE1.z) ;
   AXISTEST_X01( vtE1.z, vtE1.y, fez, fey)
   AXISTEST_Y02( vtE1.z, vtE1.x, fez, fex)
   AXISTEST_Z0( vtE1.y, vtE1.x, fey, fex)

   fex = abs( vtE2.x) ;
   fey = abs( vtE2.y) ;
   fez = abs( vtE2.z) ;
   AXISTEST_X2( vtE2.z, vtE2.y, fez, fey)
   AXISTEST_Y1( vtE2.z, vtE2.x, fez, fex)
   AXISTEST_Z12( vtE2.y, vtE2.x, fey, fex)

  // Test the three axes corresponding to the face normals of AABB Box (category 1).
   // Exit if...
   // ... [-e0, e0] and [min(v0.x,v1.x,v2.x), max(v0.x,v1.x,v2.x)] do not overlap
   if ( max( ptV0.x, max( ptV1.x, ptV2.x)) < - vtExt.x || min( ptV0.x, min( ptV1.x, ptV2.x)) > vtExt.x)
      return false ;
   // ... [-e1, e1] and [min(v0.y,v1.y,v2.y), max(v0.y,v1.y,v2.y)] do not overlap
   if ( max( ptV0.y, max( ptV1.y, ptV2.y)) < - vtExt.y || min( ptV0.y, min( ptV1.y, ptV2.y)) > vtExt.y)
      return false ;
   // ... [-e2, e2] and [min(v0.z,v1.z,v2.z), max(v0.z,v1.z,v2.z)] do not overlap
   if ( max( ptV0.z, max( ptV1.z, ptV2.z)) < - vtExt.z || min( ptV0.z, min( ptV1.z, ptV2.z)) > vtExt.z)
      return false ;

  // Test separating axis corresponding to triangle face normal (category 2)
   Plane3d Plane ;
   Plane.Set( trTriaL.GetP( 0), trTriaL.GetN()) ;
   return CDeBoxPlane( b3Box, Plane) ;
}

//----------------------------------------------------------------------------
bool
CDeBoxTria( const Frame3d& frBox, const Vector3d& vtDiag, const Triangle3d& trTria, double dSafeDist)
{
  // Porto il triangolo nel riferimento del box
   Triangle3d trTriaL = trTria ;
   trTriaL.ToLoc( frBox) ;

  // Se distanza di sicurezza nulla
   if ( dSafeDist < EPS_SMALL)
      return CDeSimpleBoxTria( Frame3d(), vtDiag, trTriaL) ;

  // Verifica preliminare con box esteso
   Frame3d frTmp( Point3d( -dSafeDist, -dSafeDist, -dSafeDist)) ;
   if ( ! CDeSimpleBoxTria( frTmp, vtDiag + 2 * Vector3d( dSafeDist, dSafeDist, dSafeDist), trTriaL))
      return false ;

  // Tre box aumentati con distanza di sicurezza in un sola dimensione
   frTmp.ChangeOrig( Point3d( -dSafeDist, 0, 0)) ;
   if ( CDeSimpleBoxTria( frTmp, vtDiag + 2 * dSafeDist * X_AX, trTriaL))
      return true ;
   frTmp.ChangeOrig( Point3d( 0, -dSafeDist, 0)) ;
   if ( CDeSimpleBoxTria( frTmp, vtDiag + 2 * dSafeDist * Y_AX, trTriaL))
      return true ;
   frTmp.ChangeOrig( Point3d( 0, 0, -dSafeDist)) ;
   if ( CDeSimpleBoxTria( frTmp, vtDiag + 2 * dSafeDist * Z_AX, trTriaL))
      return true ;

  // Capsule centrati sui dodici spigoli
   if ( CDeSimpleCapsTria( Point3d( 0, 0, 0), Point3d( vtDiag.x, 0, 0), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, vtDiag.y, 0), Point3d( vtDiag.x, vtDiag.y, 0), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, 0, 0), Point3d( 0, vtDiag.y, 0), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( vtDiag.x, 0, 0), Point3d( vtDiag.x, vtDiag.y, 0), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, 0, vtDiag.z), Point3d( vtDiag.x, 0, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, vtDiag.y, vtDiag.z), Point3d( vtDiag.x, vtDiag.y, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, 0, vtDiag.z), Point3d( 0, vtDiag.y, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( vtDiag.x, 0, vtDiag.z), Point3d( vtDiag.x, vtDiag.y, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, 0, 0), Point3d( 0, 0, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( vtDiag.x, 0, 0), Point3d( vtDiag.x, 0, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( vtDiag.x, vtDiag.y, 0), Point3d( vtDiag.x, vtDiag.y, vtDiag.z), dSafeDist, trTriaL))
      return true ;
   if ( CDeSimpleCapsTria( Point3d( 0, vtDiag.y, 0), Point3d( 0, vtDiag.y, vtDiag.z), dSafeDist, trTriaL))
      return true ;

   return false ;
}