EgtGeomKernel :

- migliorie varie a Collision Avoidance di Tools.

CAvToolTriangle.cpp

@@ -291,29 +291,25 @@ CAvToolTriangle( const Tool& tlTool, const Point3d& ptToolOrig,  const Vector3d&
 double
 CAvSphereTriangle( const Point3d& ptSpheCen, double dSpheRad, const Triangle3d& trTria, const Vector3d& vtMove)
 {
-   // Verifica preliminare che la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione
+  // Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata
+   // Posizione del centro sfera rispetto al piano del triangolo
+   Vector3d vtPCen = ptSpheCen - trTria.GetP( 0) ;
+   if ( vtPCen * trTria.GetN() > dSpheRad - EPS_SMALL) 
+      return 0. ;
+
+   // Se la fetta di spazio delimitata dal piano sotto alla sfera rispetto alla direzione
    // di allontanamento non sia già sopra al triangolo (tutti i vertici del triangolo sono sotto).
     Point3d ptBase = ptSpheCen - dSpheRad * vtMove ;
     double dMaxDistV = PointPlaneSignedDist( trTria.GetP( 0), ptBase, vtMove) ;
     dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 1), ptBase, vtMove)) ;
     dMaxDistV = max( dMaxDistV, PointPlaneSignedDist( trTria.GetP( 2), ptBase, vtMove)) ;
-    if ( dMaxDistV < 0.)
+    if ( dMaxDistV < EPS_SMALL)
        return 0. ;
 
-  // Se la sfera sta già tutta dalla parte esterna del triangolo, non va ulteriormente allontanata
-   // Posizione del centro sfera rispetto ai vertici del triangolo
-   Vector3d vtVe0Cen = ptSpheCen - trTria.GetP( 0) ;
-   Vector3d vtVe1Cen = ptSpheCen - trTria.GetP( 1) ;
-   Vector3d vtVe2Cen = ptSpheCen - trTria.GetP( 2) ;
-   if ( vtVe0Cen * trTria.GetN() > dSpheRad - EPS_SMALL  &&
-        vtVe1Cen * trTria.GetN() > dSpheRad - EPS_SMALL  &&
-        vtVe2Cen * trTria.GetN() > dSpheRad - EPS_SMALL) 
-      return 0. ;
-
   // Valuto Tangenza col piano
    double dPlaneLeakDist = SpherePlaneLeakDist( ptSpheCen, dSpheRad, trTria.GetP( 0), trTria.GetN(), vtMove) ;
   // Se il punto di tangenza esiste, verifico se è interno al triangolo
-   if ( dPlaneLeakDist >= 0) {
+   if ( dPlaneLeakDist >= 0.) {
       Point3d ptTan = ptSpheCen + dPlaneLeakDist * vtMove - dSpheRad * trTria.GetN() ;
      // Se il punto è interno abbiamo finito
       if ( IsPointInsideTriangle( ptTan, trTria)) 
@@ -340,8 +336,9 @@ double
 SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad,
                      const Point3d& ptPlane, const Vector3d& vtPlaneN, const Vector3d& vtMove)
 {
+   double dCosPM = vtPlaneN * vtMove ;
   // Se la direzione di allontanamento sta nel piano
-   if ( abs( vtPlaneN * vtMove) < EPS_ZERO) {
+   if ( abs( dCosPM) < EPS_ZERO) {
       if ( abs( ( ptSpheCen - ptPlane) * vtPlaneN) > dSpheRad - EPS_SMALL)
          return 0. ;
       else 
@@ -349,7 +346,7 @@ SpherePlaneLeakDist( const Point3d& ptSpheCen, double dSpheRad,
    }
   // altrimenti ...
    else {
-      double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / ( vtMove * vtPlaneN) ;
+      double dLeakDist = ( dSpheRad - ( ptSpheCen - ptPlane) * vtPlaneN) / dCosPM ;
       return max( dLeakDist, 0.) ;
    }
 }
@@ -395,7 +392,7 @@ SpherePointLeakDist( const Point3d& ptSpheCen, double dSpheRad, const Point3d pt
    if ( dSqOROrtLen > dSqSphRad + 2 * dSpheRad * EPS_SMALL)
       return 0. ;
    else
-      return max( dDotORMot + sqrt( max( dSpheRad * dSpheRad - dSqOROrtLen, 0.)), 0.) ;
+      return max( dDotORMot + sqrt( max( dSqSphRad - dSqOROrtLen, 0.)), 0.) ;
 }
 
 //----------------------------------------------------------------------------
@@ -469,9 +466,9 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
       }
       return dMaxDistVS ;
    }
+
   // Movimento perpendicolare all'asse del cilindro
    else if ( nMotionType == 3) {
-     
       double dLeakDist = 0. ;
       // INTERNO : se distanza di allontanamento da interno è positiva, abbiamo finito
       Point3d ptTopCont, ptBotCont ;
@@ -519,6 +516,7 @@ CAvCylinderTriangle( const Point3d& ptCylOrig, const Vector3d& vtCylAx, double d
       }   
       return dLeakDist ;
    }
+
   // Movimento generico
    else
       return -1. ;
@@ -1931,25 +1929,25 @@ IsPointInsideTriangle( const Point3d& ptP, const Triangle3d& trTria)
    if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL)
       return false ;
 
-  // Vettori dei lati normalizzati
+  // Verifico se il punto è a destra del primo lato
    Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ;
    vtV0.Normalize() ;
-   Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
-   vtV1.Normalize() ;
-   Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
-   vtV2.Normalize() ;
-
-  // E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori
    double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ;
    if ( dProd0 < - EPS_SMALL)
       return false ;
+  // Verifico se il punto è a destra del secondo lato
+   Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
+   vtV1.Normalize() ;
    double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ;
    if ( dProd1 < - EPS_SMALL)
       return false ;
+  // Verifico se il punto è a destra del terzo lato
+   Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
+   vtV2.Normalize() ;
    double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ;
    if ( dProd2 < - EPS_SMALL)
       return false ;
-
+  // Punto a sinistra di tutti i lati, quindi interno
    return true ;
 }
 
@@ -1969,25 +1967,25 @@ IsPointInsideOpenTriangle( const Point3d& ptP, const Triangle3d& trTria)
    if ( abs( ( ptP - trTria.GetP( 0)) * trTria.GetN()) > EPS_SMALL)
       return false ;
 
-  // Vettori dei lati normalizzati
+  // Verifico se il punto è a destra del primo lato
    Vector3d vtV0 = trTria.GetP( 1) - trTria.GetP( 0) ;
    vtV0.Normalize() ;
-   Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
-   vtV1.Normalize() ;
-   Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
-   vtV2.Normalize() ;
-
-  // E' sufficiente che il segno di un prodotto misto sia negativo per essere fuori
    double dProd0 = ( vtV0 ^ ( ptP - trTria.GetP( 0))) * trTria.GetN() ;
    if ( dProd0 < EPS_SMALL)
       return false ;
+  // Verifico se il punto è a destra del secondo lato
+   Vector3d vtV1 = trTria.GetP( 2) - trTria.GetP( 1) ;
+   vtV1.Normalize() ;
    double dProd1 = ( vtV1 ^ ( ptP - trTria.GetP( 1))) * trTria.GetN() ;
    if ( dProd1 < EPS_SMALL)
       return false ;
+  // Verifico se il punto è a destra del terzo lato
+   Vector3d vtV2 = trTria.GetP( 0) - trTria.GetP( 2) ;
+   vtV2.Normalize() ;
    double dProd2 = ( vtV2 ^ ( ptP - trTria.GetP( 2))) * trTria.GetN() ;
    if ( dProd2 < EPS_SMALL)
       return false ;
-
+  // Punto a sinistra di tutti i lati, quindi interno
    return true ;
 }