EgtGeomKernel/CDeConeTria.cpp

//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : CDeConTria.cpp   Data : 27.10.20     Versione : 2.2k1
// Contenuto : Implementazione della verifica di collisione tra 
//             Cono e Triangle3d.
//
//
// Modifiche : 27.10.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeConeTria.h"
#include "CDeSpheTria.h"
#include "CDeConvexTorusTria.h"
#include "CDeConeFrustumTria.h"
#include "CDeUtility.h"
#include "IntersLineSurfStd.h"
#include "/EgtDev/Include/EGkPlane3d.h"
#include "/EgtDev/Include/EGkIntersLineTria.h"
#include "/EgtDev/Include/EGkIntersPlanePlane.h"
#include "/EgtDev/Include/EgtNumUtils.h"

using namespace std ;

//----------------------------------------------------------------------------
// Il sistema di riferimento ha asse Z coincidente con l'asse del cono e origine nel vertice del cono (punto più basso).
//----------------------------------------------------------------------------
bool
CDeSimpleConeTria( const Frame3d& frCone, double dRad, double dHeight, const Triangle3d& trTria)
{
  // Porto il triangolo nel sistema di riferimento del tronco di cono
   Triangle3d trMyTria = trTria ;
   trMyTria.ToLoc( frCone) ;

  // Se almeno un vertice collide ho finito
   for ( int nV = 0 ; nV < 3 ; ++ nV) {
      Point3d ptVert = trMyTria.GetP( nV) ;
      double dLenZ = ptVert.z ;
      double dLenXY = sqrt( ptVert.x * ptVert.x + ptVert.y * ptVert.y) ;
      double dRadAtHeight = Clamp( dRad * dLenZ / dHeight, 0., dRad) ;
      if ( dLenZ > - EPS_SMALL  &&  dLenZ < dHeight + EPS_SMALL  &&  dLenXY < dRadAtHeight + EPS_SMALL)
         return true ;
   }

  // Se almeno un segmento collide ho finito
   for ( int nS = 0 ; nS < 3 ; ++ nS) {
      Point3d ptSegSt = trMyTria.GetP( nS) ;
      Point3d ptSegEn = trMyTria.GetP( ( nS + 1) % 3) ;
      Vector3d vtDir = ptSegEn - ptSegSt ;
      double dSegLen = vtDir.Len() ;
      vtDir /= dSegLen ;
      double dU1, dU2 ;
     // Collisione con la superficie laterale
      int nIndex = SegmentCone( ptSegSt, vtDir, dSegLen, ORIG, Z_AX, dRad, dHeight, dU1, dU2) ;
      if ( nIndex != CC_ERROR_INT  &&  nIndex != CC_NO_INTERS)
         return true ;
     // Collisione con la base
      nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, Point3d( 0., 0., dHeight), Z_AX, dRad, dU1, dU2) ;
      if ( nIndex != D_ERROR_INT  &&  nIndex != D_NO_INTERS)
         return true ;
   }

  // Contatti con l'interno del triangolo
   Point3d ptP = trMyTria.GetP( 0) ;
   Vector3d vtN = trMyTria.GetN() ;
  // Se il segmento congiungente i centri delle basi interseca il triangolo ho finito
   if ( abs( vtN.z) > EPS_ZERO) {
      Point3d ptInt( 0., 0., ( ( ptP - ORIG) * vtN) / vtN.z) ;
      if ( ptInt.z > - EPS_SMALL  &&  ptInt.z < dHeight + EPS_SMALL  &&
         IsPointInsideTriangle( ptInt, trMyTria, TriangleType::CLOSED))
         return true ;
   }
  // Intersezione basi / interno triangolo
   Point3d ptLine ;
   Vector3d vtLine ;
  // Piano triangolo
   Plane3d plPlaneTria ;
   plPlaneTria.Set( ptP, vtN) ;
  // Base
   Plane3d plBase ;
   plBase.Set( Point3d( 0., 0., dHeight), Z_AX) ;
   int nBaseTriaIndex = IntersPlanePlane( plPlaneTria, plBase, ptLine, vtLine) ;
   if ( nBaseTriaIndex == IPPT_OVERLAPS) {
      if ( CoplanarDiscTriangleInterference( plBase.GetPoint(), dRad, trMyTria, TriangleType::CLOSED))
         return true ;
      return false ;
   }
   else if ( nBaseTriaIndex == IPPT_YES) {
      double dU1, dU2;
      if ( LineDisc( ptLine, vtLine, Point3d( 0., 0., dHeight), Z_AX, dRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
         Point3d ptInt, ptInt2 ;
         int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, ptInt, ptInt2) ;
         if ( nLineTriaIndex != ILTT_NO  &&  nLineTriaIndex != ILTT_IN)
            return true ;
      }
   }
  // Triangolo tangente al cono
   Vector3d vtTriaNormXY( vtN.x, vtN.y, 0.) ;
   if ( ! vtTriaNormXY.Normalize())
      return false ;
   Point3d ptContactSt( 0., 0., 0.) ;
   Point3d ptContactEn( dRad * vtTriaNormXY.x, dRad * vtTriaNormXY.y, dHeight) ;
   Point3d ptInt, ptInt2 ;
   int nContactIndex = IntersLineTria( ptContactSt, ptContactEn, trMyTria, ptInt, ptInt2) ;
   return ! ( nContactIndex == ILTT_NO  ||  nContactIndex == ILTT_IN) ;
}

//----------------------------------------------------------------------------
bool
CDeConeTria( const Frame3d& frCone, double dRad, double dHeight, const Triangle3d& trTria, double dSafeDist)
{
  // Verifico validità del cono
   if ( dRad < EPS_SMALL  ||  dHeight < EPS_SMALL)
      return false ;

  // Se distanza di sicurezza nulla
   if ( dSafeDist < EPS_SMALL)
      return CDeSimpleConeTria( frCone, dRad, dHeight, trTria) ;

  // Verifica preliminare con cono esteso
   double dDeltaVert = dSafeDist * sqrt( 1 + ( dHeight * dHeight / ( dRad * dRad))) ;
   double dHeightExt = dHeight + dSafeDist + dDeltaVert ;
   double dRadExt = dRad * dHeightExt / dHeight ;
   Frame3d frTmp = frCone ; frTmp.Translate( -dDeltaVert * frCone.VersZ()) ;
   if ( ! CDeSimpleConeTria( frTmp, dRadExt, dHeightExt, trTria))
      return false ;

  // Sfera nel vertice in basso
   if ( CDeSimpleSpheTria(  frCone.Orig() + dHeight * frCone.VersZ(), dSafeDist, trTria))
      return true ;

  // Tronco di cono intermedio
   double dHypo = sqrt( dRad * dRad + dHeight * dHeight ) ;
   double dDeltaH = dSafeDist * dRad / dHypo ;
   double dDeltaR = dSafeDist * dHeight / dHypo ;
   frTmp = frCone ; frTmp.Translate( -dDeltaH * frCone.VersZ()) ;
   if ( CDeSimpleConeFrustumTria( frTmp, dDeltaR, dRad + dDeltaR, dHeight, trTria))
      return true ;

  // Toro in alto
   frTmp = frCone ; frTmp.Translate( dHeight * frCone.VersZ()) ;
   if ( CDeSimpleConvexTorusTria( frTmp, dRad, dSafeDist, CT_TOT, trTria))
      return true ;

   return false ;
}