//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : CDeTriaTria.cpp   Data : 05.11.20     Versione : 2.2k1
// Contenuto : Implementazione della verifica di collisione tra  due Triangle3d.
//
//
//
// Modifiche : 05.11.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "CDeTriaTria.h"
#include "/EgtDev/Include/EGkDistLineLine.h"
#include "/EgtDev/Include/EGkIntersPlanePlane.h"
#include <array>

using namespace std ;

//----------------------------------------------------------------------------
void
GetTwoGreater( int nVal0, int nVal1, int nVal2, int& nFirstMaxPos, int& nSecondMaxPos)
{
   if ( nVal0 > nVal1) {
      if ( nVal0 > nVal2) {
         nFirstMaxPos = 0 ;
         if ( nVal1 > nVal2)
            nSecondMaxPos = 1 ;
         else
            nSecondMaxPos = 2 ;
      }
      else {
         nFirstMaxPos = 2 ;
         nSecondMaxPos = 0 ;
      }
   }
   else if ( nVal0 > nVal2) {
      nFirstMaxPos = 1 ;
      nSecondMaxPos = 0 ;
   }
   else if ( nVal1 > nVal2) {
      nFirstMaxPos = 1 ;
      nSecondMaxPos = 2 ;
   }
   else {
      nFirstMaxPos = 2 ;
      nSecondMaxPos = 1 ;
   }
}

//----------------------------------------------------------------------------
bool
CDeTriaTria( const Triangle3d& trTriaA, const Triangle3d& trTriaB)
{
   // Piano del triangolo A
   Plane3d plPlaneTriaA ;
   plPlaneTriaA.Set( trTriaA.GetCentroid(), trTriaA.GetN()) ;
   // Calcolo le distanze dei vertici del triangolo B dal piano del triangolo A
   array< double, 3> vDistBA ;
   for ( int i = 0 ; i < 3 ; ++ i)
      vDistBA[i] = DistPointPlane( trTriaB.GetP( i), plPlaneTriaA) ;
   // Verifico posizione del triangolo B rispetto al piano del triangolo A
   int nVertPosB = 0 ;
   int nVertNegB = 0 ;
   for ( const auto& dDist : vDistBA) {
      if ( dDist > EPS_SMALL)
         ++ nVertPosB ;
      else if ( dDist < - EPS_SMALL)
         ++ nVertNegB ;
   }
   // Se il triangolo giace tutto da una parte del piano, nessuna intersezione
   if ( nVertPosB == 3  ||  nVertNegB == 3)
      return false ;
   // Piano del triangolo B
   Plane3d plPlaneTriB ;
   plPlaneTriB.Set( trTriaB.GetCentroid(), trTriaB.GetN()) ;
   // Calcolo le distanze dei vertici del triangolo A dal piano del triangolo B
   array< double, 3> vDistAB ;
   for ( int i = 0 ; i < 3 ; ++ i)
      vDistAB[i] = DistPointPlane( trTriaA.GetP(i) , plPlaneTriB) ;
   // Verifico posizione del triangolo A rispetto al piano del triangolo B
   int nVertPosA = 0 ;
   int nVertNegA = 0 ;
   for ( const auto& dDist : vDistAB) {
      if ( dDist > EPS_SMALL)
         ++ nVertPosA ;
      else if ( dDist < - EPS_SMALL)
         ++ nVertNegA ;
   }
   // Se il triangolo giace tutto da una parte del piano, nessuna intersezione
   if ( nVertPosA == 3  ||  nVertNegA == 3)
      return false ;
   // Interseco i piani
   Point3d ptLineP ;
   Vector3d vtLineV ;
   int nResPP = IntersPlanePlane( plPlaneTriaA, plPlaneTriB, ptLineP, vtLineV) ;
   if ( nResPP == IPPT_NO)
      return false ;
   else if ( nResPP == IPPT_OVERLAPS) {
      for ( int nA = 0 ; nA < 3 ; ++ nA) {
         Point3d ptStA = trTriaA.GetP( nA) ;
         Point3d PtEnA = trTriaA.GetP( ( nA + 1) % 3);
         for ( int nB = 0 ; nB < 3 ; ++ nB) {
            Point3d ptStB = trTriaB.GetP( nB) ;
            Point3d PtEnB = trTriaB.GetP( ( nB + 1) % 3) ;
            DistLineLine LineLineDistCalc( ptStA, PtEnA, ptStB, PtEnB) ;
            double dSqSegSegDist ;
            LineLineDistCalc.GetSqDist( dSqSegSegDist) ;
            if ( dSqSegSegDist < SQ_EPS_SMALL)
               return true ;
         }
      }
      return ( IsPointInsideTriangle( trTriaA.GetP( 0), trTriaB, TriangleType::CLOSED)  ||
               IsPointInsideTriangle( trTriaA.GetP( 1), trTriaB, TriangleType::CLOSED)  ||
               IsPointInsideTriangle( trTriaA.GetP( 2), trTriaB, TriangleType::CLOSED)  ||
               IsPointInsideTriangle( trTriaB.GetP( 0), trTriaA, TriangleType::CLOSED)  ||
               IsPointInsideTriangle( trTriaB.GetP( 1), trTriaA, TriangleType::CLOSED)  ||
               IsPointInsideTriangle( trTriaB.GetP( 2), trTriaA, TriangleType::CLOSED)) ;
   }
   // Interseco la retta col triangolo A
   // Cerco i due segmenti del triangolo A che limitano la retta di intersezione fra i piani
   int nSegExtrDistA[3] = { -1, -1, -1 } ;
   for ( int nV = 0 ; nV < 3 ; ++ nV) {
      int nStateSt = ( vDistAB[nV] > EPS_SMALL ? 1 : ( vDistAB[nV] > - EPS_SMALL ? 0 : - 1)) ;
      int nStateEn = ( vDistAB[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistAB[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ;
      nSegExtrDistA[nV] = abs( nStateSt - nStateEn) ;
   }
   int nFirstMaxPosA, nSecondMaxPosA ;
   GetTwoGreater( nSegExtrDistA[0], nSegExtrDistA[1], nSegExtrDistA[2], nFirstMaxPosA, nSecondMaxPosA) ;
   // Limito la retta col primo segmento trovato di A
   Vector3d vtSegFirstA = trTriaA.GetP( ( nFirstMaxPosA + 1) % 3) - trTriaA.GetP( nFirstMaxPosA) ;
   double dSegLenFirstA = vtSegFirstA.Len() ;
   vtSegFirstA /= dSegLenFirstA ;
   DistLineLine LineLineDistCalcFirstA( ptLineP, vtLineV, 100., trTriaA.GetP( nFirstMaxPosA), vtSegFirstA, dSegLenFirstA, false) ;
   double dIntParStA, dOtherParFirstA ;
   LineLineDistCalcFirstA.GetPositionsAtMinDistPoints( dIntParStA, dOtherParFirstA) ;
   // Limito la retta col secondo segmento trovato di A
   Vector3d vtSegSecondA = trTriaA.GetP( ( nSecondMaxPosA + 1) % 3) - trTriaA.GetP( nSecondMaxPosA) ;
   double dSegLenSecondA = vtSegSecondA.Len() ;
   vtSegSecondA /= dSegLenSecondA ;
   DistLineLine LineLineDistCalcSecondA( ptLineP, vtLineV, 100., trTriaA.GetP( nSecondMaxPosA), vtSegSecondA, dSegLenSecondA, false) ;
   double dIntParEnA, dOtherParSecondA ;
   LineLineDistCalcSecondA.GetPositionsAtMinDistPoints( dIntParEnA, dOtherParSecondA) ;
   // Ordino i parametri lungo la retta di intersezione fra i piani
   if ( dIntParStA > dIntParEnA) {
      swap( dIntParStA, dIntParEnA) ;
   }
   // Interseco la retta col triangolo B
   int nSegExtrDistB[3] = { -1, -1, -1 } ;
   for (int nV = 0; nV < 3; ++nV) {
      int nStateSt = ( vDistBA[nV] > EPS_SMALL ? 1 : ( vDistBA[nV] > - EPS_SMALL ? 0 : - 1)) ;
      int nStateEn = ( vDistBA[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistBA[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ;
      nSegExtrDistB[nV] = abs( nStateSt - nStateEn) ;
   }
   int nFirstMaxPosB, nSecondMaxPosB ;
   GetTwoGreater( nSegExtrDistB[0], nSegExtrDistB[1], nSegExtrDistB[2], nFirstMaxPosB, nSecondMaxPosB) ;
   // Limito la retta col primo segmento trovato di B
   Vector3d vtSegFirstB = trTriaB.GetP( ( nFirstMaxPosB + 1) % 3) - trTriaB.GetP( nFirstMaxPosB) ;
   double dSegLenFirstB = vtSegFirstB.Len() ;
   vtSegFirstB /= dSegLenFirstB ;
   DistLineLine LineLineDistCalcFirstB( ptLineP, vtLineV, 100., trTriaB.GetP( nFirstMaxPosB), vtSegFirstB, dSegLenFirstB, false) ;
   double dIntParStB, dOtherParFirstB ;
   LineLineDistCalcFirstB.GetPositionsAtMinDistPoints( dIntParStB, dOtherParFirstB) ;
   // Limito la retta col secondo segmento trovato di B
   Vector3d vtSegSecondB = trTriaB.GetP( ( nSecondMaxPosB + 1) % 3) - trTriaB.GetP( nSecondMaxPosB) ;
   double dSegLenSecondB = vtSegSecondB.Len() ;
   vtSegSecondB /= dSegLenSecondB ;
   DistLineLine LineLineDistCalcSecondB( ptLineP, vtLineV, 100., trTriaB.GetP( nSecondMaxPosB), vtSegSecondB, dSegLenSecondB, false) ;
   double dIntParEnB, dOtherParSecondB ;
   LineLineDistCalcSecondB.GetPositionsAtMinDistPoints( dIntParEnB, dOtherParSecondB) ;
   // Ordino i parametri lungo la retta di intersezione fra i piani
   if ( dIntParStB > dIntParEnB) {
      swap( dIntParStB, dIntParEnB) ;
   }
   // Se gli intervalli si sovrappongono c'è collisione, altrimenti non c'è
   if ( dIntParEnB < dIntParStA - EPS_SMALL)
      return false ;
   else if ( dIntParStB < dIntParEnA + EPS_SMALL)
      return true ;
   else
      return false ;
}