EgtGeomKernel/CDeBoxTria.cpp

//----------------------------------------------------------------------------
//  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 "/EgtDev/Include/EGkCDeBoxTria.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
CDeBoxTria( const BBox3d& b3Box, const Triangle3d& trTria)
{
  // vedi Ericson, Real-Time Collision Detection, pag. 172 + Akenine-Moller

  // 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 = trTria.GetP( 0) - ( ptCen - ORIG) ;
   Point3d ptV1 = trTria.GetP( 1) - ( ptCen - ORIG) ;
   Point3d ptV2 = trTria.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( trTria.GetP( 0), trTria.GetN()) ;
   return CDeBoxPlane( b3Box, Plane) ;
}