Dario Sassi
a3ae429662
- aggiunta verifica collisione per cono, piramide, relativi tronchi e prismatoidi.
205 lines
8.1 KiB
C++
205 lines
8.1 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2018-2020
|
|
//----------------------------------------------------------------------------
|
|
// File : DistPointSurfTm.cpp Data : 19.12.20 Versione : 2.2l3
|
|
// Contenuto : Implementazione della classe distanza Punto da Trimesh.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 07.12.18 LM Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "/EgtDev/Include/EGkDistPointTria.h"
|
|
#include "/EgtDev/Include/EGkDistPointSurfTm.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la differenza fra i bounding-box A e B.
|
|
// L'insieme differenza non è un bounding-box, ma è esprimibile come unione di al più sei bounding-box.
|
|
// Se l'insieme differenza fra i box non ha misura nulla viene restituito true, false altrimenti.
|
|
// I casi in cui non vengono trovati box di misura positiva sono quelli in cui o il box A è contenuto
|
|
// nel box B; uno di questi si verifica se il box A è vuoto.
|
|
// Nel vettore vBoxDiff vengono restituiti i box la cui unione costituisce la differenza fra A e B.
|
|
bool
|
|
BoundingBoxDifference( const BBox3d& boxA, const BBox3d& boxB, BOXVECTOR& vBoxDiff)
|
|
{
|
|
// Svuoto il risultato
|
|
vBoxDiff.clear() ;
|
|
// Se box A vuoto, risultato vuoto
|
|
if ( boxA.IsEmpty())
|
|
return false ;
|
|
// Se box B vuoto o i box non si intersecano, risultato è ancora A
|
|
BBox3d boxInt ;
|
|
if ( boxB.IsSmall() || ! boxA.FindIntersection( boxB, boxInt)) {
|
|
vBoxDiff.emplace_back( boxA) ;
|
|
return true ;
|
|
}
|
|
// Recupero i punti estremi dei box A e Intersezione
|
|
Point3d ptMinA, ptMaxA ; boxA.GetMinMax( ptMinA, ptMaxA) ;
|
|
Point3d ptMinInt, ptMaxInt ; boxInt.GetMinMax( ptMinInt, ptMaxInt) ;
|
|
// Sotto
|
|
if ( ptMinInt.z - ptMinA.z > EPS_SMALL) {
|
|
BBox3d boxD( ptMinA, Point3d( ptMaxA.x, ptMaxA.y, ptMinInt.z)) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Sopra
|
|
if ( ptMaxA.z - ptMaxInt.z > EPS_SMALL) {
|
|
BBox3d boxD( Point3d( ptMinA.x, ptMinA.y, ptMaxInt.z), ptMaxA) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Davanti
|
|
if ( ptMinInt.y - ptMinA.y > EPS_SMALL) {
|
|
BBox3d boxD( Point3d( ptMinA.x, ptMinA.y, ptMinInt.z), Point3d( ptMaxA.x, ptMinInt.y, ptMaxInt.z)) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Dietro
|
|
if ( ptMaxA.y - ptMaxInt.y > EPS_SMALL) {
|
|
BBox3d boxD( Point3d( ptMinA.x, ptMaxInt.y, ptMinInt.z), Point3d( ptMaxA.x, ptMaxA.y, ptMaxInt.z)) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Sinistra
|
|
if ( ptMinInt.x - ptMinA.x > EPS_SMALL) {
|
|
BBox3d boxD( Point3d( ptMinA.x, ptMinInt.y, ptMinInt.z), Point3d( ptMinInt.x, ptMaxInt.y, ptMaxInt.z)) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Destra
|
|
if ( ptMaxA.y - ptMaxInt.y > EPS_SMALL) {
|
|
BBox3d boxD( Point3d( ptMaxInt.x, ptMinInt.y, ptMinInt.z), Point3d( ptMaxA.x, ptMaxInt.y, ptMaxInt.z)) ;
|
|
vBoxDiff.emplace_back( boxD) ;
|
|
}
|
|
// Risultato
|
|
return ( ! vBoxDiff.empty()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
DistPointSurfTm::DistPointSurfTm( const Point3d& ptP, const ISurfTriMesh& tmSurf)
|
|
: m_dDist( -1), m_nMinDistTriaIndex( 0), m_bIsInside( false)
|
|
{
|
|
// Trimesh non valida
|
|
if ( &tmSurf == nullptr || ! tmSurf.IsValid())
|
|
return ;
|
|
// Calcolo la distanza
|
|
Calculate( ptP, tmSurf) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void
|
|
DistPointSurfTm::Calculate( const Point3d& ptP, const ISurfTriMesh& tmSurf)
|
|
{
|
|
// Inizializzo distanza non calcolata
|
|
m_dDist = - 1. ;
|
|
|
|
// Recupero e verifico il box locale della superficie
|
|
BBox3d b3Stm = tmSurf.GetAllTriaBox() ;
|
|
if ( b3Stm.IsEmpty())
|
|
return ;
|
|
|
|
// Determino i triangoli vicini e fra di essi cerco quello di minima distanza.
|
|
// Considero un box centrato nel punto P; finché all'interno del box non trovo un set di triangoli
|
|
// fra cui quello a distanza minima dal punto P ha distanza minore del minimo semi-lato del box,
|
|
// continuo a ingrandire il box. La condizione di terminazione garantisce di trovare il tiangolo di
|
|
// distanza minima della trimesh intera.
|
|
Point3d ptMin, ptMax ; b3Stm.GetMinMax( ptMin, ptMax) ;
|
|
double dDeltaLen = max( min( min( ptMax.x - ptMin.x, ptMax.y - ptMin.y), ptMax.z - ptMin.z) / 40., 10.) ;
|
|
double dBoxHalfLenX = max( max( ptMin.x - ptP.x, ptP.x - ptMax.x), 0.) + dDeltaLen ;
|
|
double dBoxHalfLenY = max( max( ptMin.y - ptP.y, ptP.y - ptMax.y), 0.) + dDeltaLen ;
|
|
double dBoxHalfLenZ = max( max( ptMin.z - ptP.z, ptP.z - ptMax.z), 0.) + dDeltaLen ;
|
|
// Considero anche il box precedente per poter analizzare solo lo spazio differenza tra i due
|
|
BBox3d boxPPrev( ptP) ;
|
|
BBox3d boxP( ptP, dBoxHalfLenX, dBoxHalfLenY, dBoxHalfLenZ) ;
|
|
// Variabili distanza minima, indice del triangolo di distanza minima, punto di distanza minima
|
|
double dMinSqDist = DBL_MAX ;
|
|
int nMinDistTriaIndex = SVT_NULL ;
|
|
Point3d ptMinDistPoint ;
|
|
bool bContinue = true ;
|
|
// Finché non si verifica la condizione di terminazione ingrandisco il box.
|
|
while ( bContinue) {
|
|
// Calcolo il box differenza con il precedente per non esplorare parti già considerate
|
|
BOXVECTOR vBox ;
|
|
BoundingBoxDifference( boxP, boxPPrev, vBox) ;
|
|
// Ciclo sui box differenza
|
|
bool bCollide = false ;
|
|
for ( const auto& b3Box : vBox) {
|
|
// interseco il box con quello della superficie e ne verifico la distanza minima dal punto
|
|
BBox3d b3Int ;
|
|
if ( ! b3Box.FindIntersection( b3Stm, b3Int) || b3Int.SqDistFromPoint( ptP) > dMinSqDist)
|
|
continue ;
|
|
// ricerca sui triangoli nel box
|
|
bCollide = true ;
|
|
INTVECTOR vnIds ;
|
|
if ( tmSurf.GetAllTriaOverlapBox( b3Int, vnIds)) {
|
|
// Ciclo sui triangoli del sotto-box corrente
|
|
for ( auto nT : vnIds) {
|
|
Triangle3d trCurTria ;
|
|
tmSurf.GetTriangle( nT, trCurTria) ;
|
|
DistPointTriangle distPT( ptP, trCurTria) ;
|
|
double dCurSqDist ;
|
|
// Se la distanza del triangolo è valida e minore di quella attuale aggiorno
|
|
if ( distPT.GetSqDist( dCurSqDist) && dCurSqDist < dMinSqDist) {
|
|
dMinSqDist = dCurSqDist ;
|
|
nMinDistTriaIndex = nT ;
|
|
distPT.GetMinDistPoint( ptMinDistPoint) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// Se si verifica la condizione di terminazione arresto il ciclo altrimenti aggiorno i box
|
|
if ( ! bCollide || dMinSqDist < EPS_SMALL * EPS_SMALL)
|
|
bContinue = false ;
|
|
else {
|
|
boxPPrev = boxP ;
|
|
boxP.Expand( dDeltaLen) ;
|
|
}
|
|
}
|
|
|
|
if ( nMinDistTriaIndex != SVT_NULL) {
|
|
m_dDist = sqrt( max( dMinSqDist, 0.)) ;
|
|
m_nMinDistTriaIndex = nMinDistTriaIndex ;
|
|
m_ptMinDistPoint = ptMinDistPoint ;
|
|
Triangle3d trMinDistTria ;
|
|
tmSurf.GetTriangle( m_nMinDistTriaIndex, trMinDistTria) ;
|
|
trMinDistTria.Validate() ;
|
|
m_bIsInside = ( ( ptP - m_ptMinDistPoint) * trMinDistTria.GetN() < - EPS_SMALL) && tmSurf.IsClosed() ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistPointSurfTm::GetDist( double& dDist)
|
|
{
|
|
// Distanza non valida
|
|
if ( m_dDist < -EPS_ZERO)
|
|
return false ;
|
|
// Distanza valida
|
|
dDist = m_dDist ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistPointSurfTm::GetMinDistPoint( Point3d& ptMinDistPoint)
|
|
{
|
|
// Distanza non valida
|
|
if ( m_dDist < -EPS_ZERO)
|
|
return false ;
|
|
// Distanza valida
|
|
ptMinDistPoint = m_ptMinDistPoint ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistPointSurfTm::GetMinDistTriaIndex( int& nMinDistIndex)
|
|
{
|
|
// Distanza non valida
|
|
if ( m_dDist < -EPS_ZERO)
|
|
return false ;
|
|
// Distanza valida
|
|
nMinDistIndex = m_nMinDistTriaIndex ;
|
|
return true ;
|
|
}
|