Dario Sassi
ddade325c4
- standardizzate le funzione Collision Detection sia per trimesh sia per Zmap (ex Avoid...) - nelle funzioni Cde ora se arrivano geometrie errate si ritorna collisione (maggior sicurezza).
228 lines
9.3 KiB
C++
228 lines
9.3 KiB
C++
//----------------------------------------------------------------------------
|
|
// 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 ;
|
|
}
|