From 5368adff540e707ff74db9d9473bce7f9a467730 Mon Sep 17 00:00:00 2001
From: ama <ama@opencascade.com>
Date: Sat, 5 May 2012 17:04:19 +0400
Subject: [PATCH] 0022883: Extrema can not find projection of 3D point on
 surface.

Optimization of process of surface discretization: increase the number of parametric points in case of a complex surface geometry (BSpline and Bezier surfaces).
Small correction process of building subgrid in Extrema_GenExtPS::FindSolution().
Minor corrections (formatting, duplicate statements)
---
 src/Extrema/Extrema_ExtPS.cxx     |  65 +++---
 src/Extrema/Extrema_FuncExtCS.cxx |  13 +-
 src/Extrema/Extrema_FuncExtPS.cxx |  13 ++
 src/Extrema/Extrema_GenExtPS.cdl  |  14 +-
 src/Extrema/Extrema_GenExtPS.cxx  | 355 ++++++++++++++++++++++--------
 src/math/math_FunctionSetRoot.cxx |  20 +-
 6 files changed, 348 insertions(+), 132 deletions(-)

diff --git a/src/Extrema/Extrema_ExtPS.cxx b/src/Extrema/Extrema_ExtPS.cxx
index ed4d30449c..cfcf009a5b 100755
--- a/src/Extrema/Extrema_ExtPS.cxx
+++ b/src/Extrema/Extrema_ExtPS.cxx
@@ -60,41 +60,43 @@ static Standard_Boolean IsoIsDeg  (const Adaptor3d_Surface& S,
     Standard_Real Step,D1NormMax;
     if (IT == GeomAbs_IsoV) 
     {
-      Step = (U2 - U1)/10;
-      if(Step < Precision::PConfusion()) {
-        return Standard_False;
-      }
-      if(Step < Precision::PConfusion()) {
-        return Standard_False;
-      }
-      D1NormMax=0.;
-      for (T=U1;T<=U2;T=T+Step) 
+      if( !Precision::IsInfinite(U1) &&  !Precision::IsInfinite(U2) )
       {
-        S.D1(T,Param,P,D1U,D1V);
-        D1NormMax=Max(D1NormMax,D1U.Magnitude());
+        Step = (U2 - U1)/10;
+        if(Step < Precision::PConfusion()) {
+          return Standard_False;
+        }
+        D1NormMax=0.;
+
+        for (T=U1;T<=U2;T=T+Step) 
+        {
+          S.D1(T,Param,P,D1U,D1V);
+          D1NormMax=Max(D1NormMax,D1U.Magnitude());
+        }
+
+        if (D1NormMax >TolMax || D1NormMax < TolMin ) 
+          Along = Standard_False;
       }
-
-      if (D1NormMax >TolMax || D1NormMax < TolMin ) 
-           Along = Standard_False;
     }
     else 
     {
-      Step = (V2 - V1)/10;
-      if(Step < Precision::PConfusion()) {
-        return Standard_False;
-      }
-      if(Step < Precision::PConfusion()) {
-        return Standard_False;
-      }
-      D1NormMax=0.;
-      for (T=V1;T<=V2;T=T+Step) 
+      if( !Precision::IsInfinite(V1) &&  !Precision::IsInfinite(V2) )
       {
-	S.D1(Param,T,P,D1U,D1V);
-        D1NormMax=Max(D1NormMax,D1V.Magnitude());
+        Step = (V2 - V1)/10;
+        if(Step < Precision::PConfusion()) {
+          return Standard_False;
+        }
+        D1NormMax=0.;
+        for (T=V1;T<=V2;T=T+Step) 
+        {
+          S.D1(Param,T,P,D1U,D1V);
+          D1NormMax=Max(D1NormMax,D1V.Magnitude());
+        }
+
+        if (D1NormMax >TolMax || D1NormMax < TolMin ) 
+          Along = Standard_False;
       }
 
-      if (D1NormMax >TolMax || D1NormMax < TolMin ) 
-           Along = Standard_False;
 
 
     }
@@ -240,14 +242,7 @@ void Extrema_ExtPS::Perform(const gp_Pnt& P)
   myPoints.Clear();
   mySqDist.Clear();
   Standard_Integer i;
-  P11 = myS->Value(myuinf, myvinf);
-  P12 = myS->Value(myuinf, myvsup);
-  P21 = myS->Value(myusup, myvinf);
-  P22 = myS->Value(myusup, myvsup);
-  d11 = P.SquareDistance(P11);
-  d12 = P.SquareDistance(P12);
-  d21 = P.SquareDistance(P21);
-  d22 = P.SquareDistance(P22);
+  
   
   switch(mytype) {
     
diff --git a/src/Extrema/Extrema_FuncExtCS.cxx b/src/Extrema/Extrema_FuncExtCS.cxx
index bb64b83508..ec8764a01e 100755
--- a/src/Extrema/Extrema_FuncExtCS.cxx
+++ b/src/Extrema/Extrema_FuncExtCS.cxx
@@ -23,6 +23,7 @@
 #include <Extrema_FuncExtCS.ixx>
 #include <gp_Vec.hxx>
 #include <Standard_TypeMismatch.hxx>
+#include <Precision.hxx>
 
 /*-----------------------------------------------------------------------------
  Fonction permettant de rechercher une distance extremale entre une courbe C 
@@ -209,7 +210,17 @@ Standard_Integer Extrema_FuncExtCS::GetStateNumber()
   Value(UVSol, Sol);
   cout <<"F(1)= "<<Sol(1)<<" F(2)= "<<Sol(2)<<" F(3)= "<<Sol(3)<<endl;
 #endif
-
+  //comparison of solution with previous solutions
+  Standard_Real tol2d = Precision::PConfusion() * Precision::PConfusion();
+  Standard_Integer i = 1, nbSol = mySqDist.Length();
+  for( ; i <=  nbSol; i++)
+  {
+    Standard_Real aU = myPoint1(i).Parameter();
+    if( (myU - aU) * (myU - aU) <= tol2d )
+      break;
+  }
+  if (i <= nbSol)
+    return 0;
   mySqDist.Append(myP1.SquareDistance(myP2));
   myPoint1.Append(Extrema_POnCurv(myt,myP1));
   myPoint2.Append(Extrema_POnSurf(myU,myV,myP2));
diff --git a/src/Extrema/Extrema_FuncExtPS.cxx b/src/Extrema/Extrema_FuncExtPS.cxx
index 326e6023b1..9af49fbdba 100755
--- a/src/Extrema/Extrema_FuncExtPS.cxx
+++ b/src/Extrema/Extrema_FuncExtPS.cxx
@@ -130,6 +130,19 @@ Standard_Boolean Extrema_FuncExtPS::Values (const math_Vector& UV,
 Standard_Integer Extrema_FuncExtPS::GetStateNumber ()
 {
   if (!myPinit || !mySinit) Standard_TypeMismatch::Raise();
+  //comparison of solution with previous solutions
+  Standard_Integer i = 1, nbSol = mySqDist.Length();
+  Standard_Real tol2d = Precision::PConfusion() * Precision::PConfusion();
+   
+  for( ; i <=  nbSol; i++)
+  {
+    Standard_Real aU, aV;
+	myPoint(i).Parameter(aU, aV);
+	if( ((myU - aU ) * (myU - aU ) + (myV - aV ) * (myV - aV )) <= tol2d )
+      break;
+  }
+  if( i <= nbSol)
+	  return 0;
   mySqDist.Append(myPs.SquareDistance(myP));
   myPoint.Append(Extrema_POnSurf(myU,myV,myPs));
   return 0;
diff --git a/src/Extrema/Extrema_GenExtPS.cdl b/src/Extrema/Extrema_GenExtPS.cdl
index 67f7d5000d..30067b84c0 100755
--- a/src/Extrema/Extrema_GenExtPS.cdl
+++ b/src/Extrema/Extrema_GenExtPS.cdl
@@ -36,7 +36,8 @@ uses  	POnSurf       from Extrema,
 	ExtAlgo from Extrema,
 	HArray1OfSphere from Bnd,
 	Vector        from math,
-	HArray2OfPnt from TColgp
+	HArray2OfPnt from TColgp,
+	HArray1OfReal from TColStd
 
 raises  NotDone      from StdFail,
     	OutOfRange   from Standard,
@@ -137,8 +138,15 @@ is
     BuildTree(me : in out)
     is static private;
     
-    FindSolution(me: in out; P : Pnt from gp; UV : Vector from math; PasU, PasV : Real; f : ExtFlag from Extrema)
+    FindSolution(me: in out; P : Pnt from gp; UV : Vector from math; theNoU, theNoV : Integer; f : ExtFlag from Extrema)
     is static private;
+    
+    GetGridPoints(me: in out;  theSurf: Surface from Adaptor3d) is private;
+    	---Purpose: Selection of points to build grid, depending on the type of surface
+    
+    BuildGrid(me: in out) is private;
+    	---Purpose: Creation of grid of parametric points
+  
 
 fields
     myDone    : Boolean;
@@ -158,5 +166,7 @@ fields
     myS       : SurfacePtr from Adaptor3d;
     myFlag    : ExtFlag from Extrema;
     myAlgo    : ExtAlgo from Extrema;
+    myUParams : HArray1OfReal from TColStd;
+    myVParams : HArray1OfReal from TColStd;
 
 end GenExtPS;
diff --git a/src/Extrema/Extrema_GenExtPS.cxx b/src/Extrema/Extrema_GenExtPS.cxx
index 300628880a..89013dff0d 100755
--- a/src/Extrema/Extrema_GenExtPS.cxx
+++ b/src/Extrema/Extrema_GenExtPS.cxx
@@ -37,7 +37,16 @@
 #include <Extrema_ExtFlag.hxx>
 #include <Bnd_Array1OfSphere.hxx>
 #include <Bnd_HArray1OfSphere.hxx>
-
+#include <Precision.hxx>
+#include <Geom_OffsetSurface.hxx>
+#include <Geom_RectangularTrimmedSurface.hxx>
+#include <Geom_BSplineSurface.hxx>
+#include <Geom_BezierSurface.hxx>
+#include <Adaptor3d_HSurface.hxx>
+#include <Adaptor3d_HCurve.hxx>
+#include <Adaptor3d_Curve.hxx>
+#include <Geom_BSplineCurve.hxx>
+#include <Geom_BezierCurve.hxx>
 //IMPLEMENT_HARRAY1(Extrema_HArray1OfSphere)
 
 
@@ -322,7 +331,6 @@ void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
 				  const Standard_Real    TolU, 
 				  const Standard_Real    TolV)
 {
-  myInit = Standard_True;
   myS = (Adaptor3d_SurfacePtr)&S;
   myusample = NbU;
   myvsample = NbV;
@@ -339,35 +347,191 @@ void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
   myF.Initialize(S);
 
   mySphereUBTree.Nullify();
+  myUParams.Nullify();
+  myVParams.Nullify();
+  myInit = Standard_False;
+}
 
-  if(myAlgo == Extrema_ExtAlgo_Grad)
+inline static void fillParams(const TColStd_Array1OfReal& theKnots,
+                              Standard_Integer theDegree,
+                              Standard_Real theParMin,
+                              Standard_Real theParMax,
+                              Handle_TColStd_HArray1OfReal& theParams,
+                              Standard_Integer theSample)
+{
+  NCollection_Vector<Standard_Real> aParams;
+  Standard_Integer i = 1;
+  Standard_Real aPrevPar = theParMin;
+  aParams.Append(aPrevPar);
+  //calculation the array of parametric points depending on the knots array variation and degree of given surface
+  for ( ; i <  theKnots.Length() && theKnots(i) < (theParMax - Precision::PConfusion()); i++ )
   {
-	  //If flag was changed and extrema not reinitialized Extrema would fail
-    mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
-  Standard_Real PasU = myusup - myumin;
-  Standard_Real PasV = myvsup - myvmin;
-  Standard_Real U0 = PasU / myusample / 100.;
-  Standard_Real V0 = PasV / myvsample / 100.;
-  gp_Pnt P1;
-  PasU = (PasU - U0) / (myusample - 1);
-  PasV = (PasV - V0) / (myvsample - 1);
-  U0 = U0/2. + myumin;
-  V0 = V0/2. + myvmin;
+    if (  theKnots(i+1) < theParMin + Precision::PConfusion())
+      continue;
+
+    Standard_Real aStep = (theKnots(i+1) - theKnots(i))/Max(theDegree,2);
+    Standard_Integer k =1;
+    for( ; k <= theDegree ; k++)
+    {
+      Standard_Real aPar = theKnots(i) + k * aStep;
+      if(aPar > theParMax - Precision::PConfusion())
+        break;
+      if(aPar > aPrevPar + Precision::PConfusion() )
+      {
+        aParams.Append(aPar);
+        aPrevPar = aPar;
+      }
+    }
+
+  }
+  aParams.Append(theParMax);
+  Standard_Integer nbPar = aParams.Length();
+  //in case of an insufficient number of points the grid will be built later 
+  if (nbPar < theSample)
+    return;
+  theParams = new TColStd_HArray1OfReal(1, nbPar );
+  for( i = 0; i < nbPar; i++)
+    theParams->SetValue(i+1,aParams(i));
+}
+
+void Extrema_GenExtPS::GetGridPoints( const Adaptor3d_Surface& theSurf)
+{
+  //creation parametric points for BSpline and Bezier surfaces
+  //with taking into account of Degree and NbKnots of BSpline or Bezier geometry
+  if(theSurf.GetType() == GeomAbs_OffsetSurface)
+    GetGridPoints(theSurf.BasisSurface()->Surface());
+  //parametric points for BSpline surfaces
+  else if( theSurf.GetType() == GeomAbs_BSplineSurface) 
+  {
+    Handle(Geom_BSplineSurface) aBspl = theSurf.BSpline();
+    if(!aBspl.IsNull())
+    {
+      TColStd_Array1OfReal aUKnots(1, aBspl->NbUKnots());
+      aBspl->UKnots( aUKnots);
+      TColStd_Array1OfReal aVKnots(1, aBspl->NbVKnots());
+      aBspl->VKnots( aVKnots);
+      fillParams(aUKnots,aBspl->UDegree(),myumin, myusup, myUParams, myusample);
+      fillParams(aVKnots,aBspl->VDegree(),myvmin, myvsup, myVParams, myvsample);
+    }
+  }
+  //calculation parametric points for Bezier surfaces
+  else if(theSurf.GetType() == GeomAbs_BezierSurface)
+  {
+    Handle(Geom_BezierSurface) aBezier = theSurf.Bezier();
+    if(aBezier.IsNull())
+      return;
+
+    TColStd_Array1OfReal aUKnots(1,2);
+    TColStd_Array1OfReal aVKnots(1,2);
+    aBezier->Bounds(aUKnots(1), aUKnots(2), aVKnots(1), aVKnots(2));
+    fillParams(aUKnots, aBezier->UDegree(), myumin, myusup, myUParams, myusample);
+    fillParams(aVKnots, aBezier->VDegree(), myvmin, myvsup, myVParams, myvsample);
 
-// Calculation of distances
+  }
+  //creation points for surfaces based on BSpline or Bezier curves
+  else if(theSurf.GetType() == GeomAbs_SurfaceOfRevolution || 
+    theSurf.GetType() == GeomAbs_SurfaceOfExtrusion)
+  {
+    Handle(TColStd_HArray1OfReal) anArrKnots;
+    Standard_Integer aDegree = 0;
+    GeomAbs_CurveType aType = theSurf.BasisCurve()->Curve().GetType();
+    if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BSplineCurve)
+    {
+      Handle(Geom_BSplineCurve) aBspl = theSurf.BasisCurve()->Curve().BSpline();
+      if(!aBspl.IsNull())
+      {
+        anArrKnots = new TColStd_HArray1OfReal(1,aBspl->NbKnots());
+        aBspl->Knots( anArrKnots->ChangeArray1() );
+        aDegree = aBspl->Degree();
+        
+      }
+
+    }
+    if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BezierCurve)
+    {
+      Handle(Geom_BezierCurve) aBez = theSurf.BasisCurve()->Curve().Bezier();
+      if(!aBez.IsNull())
+      {
+        anArrKnots = new TColStd_HArray1OfReal(1,2);
+        anArrKnots->SetValue(1, aBez->FirstParameter());
+        anArrKnots->SetValue(2, aBez->LastParameter());
+        aDegree = aBez->Degree();
+        
+      }
+    }
+    if(anArrKnots.IsNull())
+      return;
+    if( theSurf.GetType() == GeomAbs_SurfaceOfRevolution )
+      fillParams( anArrKnots->Array1(), aDegree, myvmin, myvsup, myVParams, myvsample);
+    else
+      fillParams( anArrKnots->Array1(), aDegree, myumin, myusup, myUParams, myusample);
+  }
+  //update the number of points in sample
+  if(!myUParams.IsNull())
+    myusample = myUParams->Length();
+  if( !myVParams.IsNull())
+    myvsample = myVParams->Length();
+  
+}
+
+void Extrema_GenExtPS::BuildGrid()
+{
+  //if grid was already built do nothing
+  if(myInit)
+    return;
+  
+  //build parametric grid in case of a complex surface geometry (BSpline and Bezier surfaces)
+ GetGridPoints(*myS);
+  
+  //build grid in other cases 
+  if( myUParams.IsNull() )
+  {
+    Standard_Real PasU = myusup - myumin;
+    Standard_Real U0 = PasU / myusample / 100.;
+    PasU = (PasU - U0) / (myusample - 1);
+    U0 = U0/2. + myumin;
+    myUParams = new TColStd_HArray1OfReal(1,myusample );
+    Standard_Integer NoU;
+    Standard_Real U = U0;
+    for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
+      myUParams->SetValue(NoU, U);
+
+  }
+
+  if( myVParams.IsNull())
+  {
+    Standard_Real PasV = myvsup - myvmin;
+    Standard_Real V0 = PasV / myvsample / 100.;
+    PasV = (PasV - V0) / (myvsample - 1);
+    V0 = V0/2. + myvmin;
+    
+    myVParams = new TColStd_HArray1OfReal(1,myvsample );
+    Standard_Integer  NoV;
+    Standard_Real V = V0;
+   
+    for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
+      myVParams->SetValue(NoV, V);
+  }
+  //If flag was changed and extrema not reinitialized Extrema would fail
+  mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
+  // Calculation of distances
 
   Standard_Integer NoU, NoV;
-  Standard_Real U, V;
-  for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
-    for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
-      P1 = myS->Value(U, V);
+ 
+  for ( NoU = 1 ; NoU <= myusample; NoU++ ) {
+    for ( NoV = 1 ; NoV <= myvsample; NoV++) {
+      gp_Pnt P1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
       mypoints->SetValue(NoU,NoV,P1);
     }
   }
-  }
+  
+  myInit = Standard_True;
+}
+
 
-  //mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
 
+
+  
 /*
 a- Constitution of the table of distances (TbDist(0,myusample+1,0,myvsample+1)):
    ---------------------------------------------------------------
@@ -375,9 +539,6 @@ a- Constitution of the table of distances (TbDist(0,myusample+1,0,myvsample+1)):
 
 // Parameterisation of the sample
 
-
-}
-
 void Extrema_GenExtPS::BuildTree()
 {
   // if tree already exists, assume it is already correctly filled
@@ -394,16 +555,26 @@ void Extrema_GenExtPS::BuildTree()
   U0 = U0/2. + myumin;
   V0 = V0/2. + myvmin;
 
+  //build grid of parametric points 
+  myUParams = new TColStd_HArray1OfReal(1,myusample );
+  myVParams = new TColStd_HArray1OfReal(1,myvsample );
+  Standard_Integer NoU, NoV;
+  Standard_Real U = U0, V = V0;
+  for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
+    myUParams->SetValue(NoU, U);
+  for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
+    myVParams->SetValue(NoV, V);
+
   // Calculation of distances
   mySphereUBTree = new Extrema_UBTreeOfSphere;
   Extrema_UBTreeFillerOfSphere aFiller(*mySphereUBTree);
   Standard_Integer i = 0;
-  Standard_Real U, V;
+  
   mySphereArray = new Bnd_HArray1OfSphere(0, myusample * myvsample);
-  Standard_Integer NoU, NoV;
-  for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
-    for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
-      P1 = myS->Value(U, V);
+ 
+  for ( NoU = 1; NoU <= myusample; NoU++ ) {
+    for ( NoV = 1; NoV <= myvsample; NoV++) {
+      P1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
       Bnd_Sphere aSph(P1.XYZ(), 0/*mytolu < mytolv ? mytolu : mytolv*/, NoU, NoV);
       aFiller.Add(i, aSph);
       mySphereArray->SetValue( i, aSph );
@@ -413,7 +584,10 @@ void Extrema_GenExtPS::BuildTree()
   aFiller.Fill();
 }
 
-void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, const math_Vector& UV, const Standard_Real PasU, const Standard_Real PasV, const Extrema_ExtFlag f)
+void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, 
+                                    const math_Vector& UV, 
+                                    const Standard_Integer theNoU, 
+                                    const Standard_Integer theNoV, const Extrema_ExtFlag f)
 {
   math_Vector Tol(1,2);
   Tol(1) = mytolu;
@@ -438,6 +612,7 @@ void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, const math_Vector& UV, cons
   
   math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
   Standard_Boolean ToResolveOnSubgrid = Standard_False;
+  Standard_Boolean NewSolution = Standard_False;
   if (f == Extrema_ExtFlag_MIN)
   {
     if(S.IsDone())
@@ -447,46 +622,46 @@ void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, const math_Vector& UV, cons
       gp_Pnt PSol = myS->Value(root(1), root(2));
       DistSol = P.SquareDistance(PSol);
       if(Abs(errors(1)) > eps || Abs(errors(2)) > eps || DistStart < DistSol)
+      {
         //try to improve solution on subgrid of sample points
         ToResolveOnSubgrid = Standard_True;
+      }
     }
     else
+    {
+      //keep found roots and try to find solution
+      Standard_Integer nbExt = myF.NbExt();
+      Standard_Integer k = 1;
+      for( ; k <= nbExt; k++)
+      {
+        Standard_Real aD = myF.SquareDistance(k);
+        if(aD < DistSol)
+        {
+          DistSol = aD;
+          myF.Point(k).Parameter(UV(1),UV(2));
+          NewSolution = Standard_True;
+        }
+      }
       ToResolveOnSubgrid = Standard_True;
+    }
     
     if (ToResolveOnSubgrid)
     {
-      Standard_Real u1 = Max(UV(1) - PasU, myumin), u2 = Min(UV(1) + PasU, myusup);
-      Standard_Real v1 = Max(UV(2) - PasV, myvmin), v2 = Min(UV(2) + PasV, myvsup);
-      
-      if(u2 - u1 < 2.*PasU) {
-        if(Abs(u1 - myumin) < 1.e-9) {
-          u2 = u1 + 2.*PasU;
-          u2 = Min(u2, myusup);
-        }
-        if(Abs(u2 - myusup) < 1.e-9) {
-          u1 = u2 - 2.*PasU;
-          u1 = Max(u1, myumin);
-        }
-      }
-      
-      if(v2 - v1 < 2.*PasV) {
-        if(Abs(v1 - myvmin) < 1.e-9) {
-          v2 = v1 + 2.*PasV;
-          v2 = Min(v2, myvsup);
-        }
-        if(Abs(v2 - myvsup) < 1.e-9) {
-          v1 = v2 - 2.*PasV;
-          v1 = Max(v1, myvmin);
-        }
-      }
-      
+      //extension of interval to find new solution
+      Standard_Real u1 = theNoU == 1 ? myumin : myUParams->Value(theNoU-1), 
+        u2 = theNoU == myusample ? myusup : myUParams->Value(theNoU + 1);
+      Standard_Real v1 = theNoV == 1 ? myvmin : myVParams->Value(theNoV-1),
+        v2 = theNoV == myvsample ? myvsup : myVParams->Value(theNoV + 1);
+            
       Standard_Real du = (u2 - u1)/(nbsubsample-1);
       Standard_Real dv = (v2 - v1)/(nbsubsample-1);
       Standard_Real u, v;
       Standard_Real dist;
       
-      Standard_Boolean NewSolution = Standard_False;
+      //try to find solution on subgrid
       Standard_Integer Nu, Nv;
+      Standard_Integer minU = 0;
+      Standard_Integer minV = 0;
       for (Nu = 1, u = u1; Nu < nbsubsample; Nu++, u += du) {
         for (Nv = 1, v = v1; Nv < nbsubsample; Nv++, v += dv) {
           gp_Pnt Puv = myS->Value(u, v);
@@ -497,13 +672,20 @@ void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, const math_Vector& UV, cons
             UV(2) = v;
             NewSolution = Standard_True;
             DistSol = dist;
+            minU = Nu;
+            minV = Nv;
           }
         }
       }
       
       if(NewSolution) {
         //try to precise
+        UVinf(1) = u1 + (minU == 1 ? 0 : minU - 2) * du;
+        UVinf(2) = v1 + (minV == 1 ? 0 : minV - 2) * dv;
+        UVsup(1) = u1 + (minU == nbsubsample ? nbsubsample : minU +1) * du;;
+        UVsup(2) = v1 + (minV == nbsubsample ? nbsubsample : minV +1) * dv;
         math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
+
       }
     } //end of if (ToResolveOnSubgrid)
   } //end of if (f == Extrema_ExtFlag_MIN)
@@ -518,40 +700,33 @@ void Extrema_GenExtPS::SetFlag(const Extrema_ExtFlag F)
 
 void Extrema_GenExtPS::SetAlgo(const Extrema_ExtAlgo A)
 {
+  if(myAlgo != A)
+     myInit = Standard_False;
   myAlgo = A;
+ 
 }
 
 void Extrema_GenExtPS::Perform(const gp_Pnt& P) 
 {  
-  //BuildTree();
   myDone = Standard_False;
   myF.SetPoint(P);
-
-  Standard_Real PasU = myusup - myumin;
-  Standard_Real PasV = myvsup - myvmin;
-  Standard_Real U, U0 = PasU / myusample / 100.;
-  Standard_Real V, V0 = PasV / myvsample / 100.;
-  PasU = (PasU - U0) / (myusample - 1);
-  PasV = (PasV - V0) / (myvsample - 1);
-  U0 = U0/2.+myumin;
-  V0 = V0/2.+myvmin;
-
-  //math_Vector Tol(1, 2);
+  
   math_Vector UV(1,2);
 
   if(myAlgo == Extrema_ExtAlgo_Grad)
   {
+    BuildGrid();
     Standard_Integer NoU,NoV;
 
     TColStd_Array2OfReal TheDist(0, myusample+1, 0, myvsample+1);
-    for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
-      for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
+    
+    for ( NoU = 1 ; NoU <= myusample; NoU++) {
+      for ( NoV = 1; NoV <= myvsample; NoV++) {
         TheDist(NoU, NoV) = P.SquareDistance(mypoints->Value(NoU, NoV));
       }
     }
 
-    TColStd_Array2OfInteger TbSel(0,myusample+1,0,myvsample+1);
-    TbSel.Init(0);
+   
 
     Standard_Real Dist;
 
@@ -567,7 +742,6 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
       }
       for (NoU = 1; NoU <= myusample; NoU++) {
         for (NoV = 1; NoV <= myvsample; NoV++) {
-          if (TbSel(NoU,NoV) == 0) {
             Dist = TheDist(NoU,NoV);
             if ((TheDist(NoU-1,NoV-1) >= Dist) &&
               (TheDist(NoU-1,NoV  ) >= Dist) &&
@@ -578,11 +752,12 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
               (TheDist(NoU+1,NoV  ) >= Dist) &&
               (TheDist(NoU+1,NoV+1) >= Dist)) {
                 //Create array of UV vectors to calculate min
-                UV(1) = U0 + (NoU - 1) * PasU;
-                UV(2) = V0 + (NoV - 1) * PasV;
-                FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
+                
+                UV(1) = myUParams->Value(NoU);
+                UV(2) = myVParams->Value(NoV);
+                FindSolution(P, UV, NoU, NoV, Extrema_ExtFlag_MIN);
             }
-          }
+         
         }
       }
     }
@@ -599,7 +774,6 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
       }
       for (NoU = 1; NoU <= myusample; NoU++) {
         for (NoV = 1; NoV <= myvsample; NoV++) {
-          if (TbSel(NoU,NoV) == 0) {
             Dist = TheDist(NoU,NoV);
             if ((TheDist(NoU-1,NoV-1) <= Dist) &&
               (TheDist(NoU-1,NoV  ) <= Dist) &&
@@ -610,12 +784,11 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
               (TheDist(NoU+1,NoV  ) <= Dist) &&
               (TheDist(NoU+1,NoV+1) <= Dist)) {
                 //Create array of UV vectors to calculate max
-                UV(1) = U0 + (NoU - 1) * PasU;
-                UV(2) = V0 + (NoV - 1) * PasV;
-                FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MAX);
+                UV(1) = myUParams->Value(NoU);
+                UV(2) = myVParams->Value(NoV);
+                FindSolution(P, UV, NoU, NoV, Extrema_ExtFlag_MAX);
             }
-          }
-        }
+         }
       }
     }
   }
@@ -632,10 +805,11 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
       //TODO: check if no solution in binary tree
       Bnd_Sphere& aSph = aSelector.Sphere();
 
-      UV(1) = U0 + (aSph.U() - 1) * PasU;
-      UV(2) = V0 + (aSph.V() - 1) * PasV;
+      UV(1) = myUParams->Value(aSph.U());//U0 + (aSph.U() - 1) * PasU;
+      UV(2) = myVParams->Value(aSph.V());//V0 + (aSph.V() - 1) * PasV;
 
-      FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
+      //FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
+      FindSolution(P, UV, aSph.U(), aSph.V(), Extrema_ExtFlag_MIN);
     }
     if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
     {
@@ -646,11 +820,12 @@ void Extrema_GenExtPS::Perform(const gp_Pnt& P)
       Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
       //TODO: check if no solution in binary tree
       Bnd_Sphere& aSph = aSelector.Sphere();
+      UV(1) = myUParams->Value(aSph.U());
+      UV(2) = myVParams->Value(aSph.V());
+      //UV(1) = U0 + (aSph.U() - 1) * PasU;
+      //UV(2) = V0 + (aSph.V() - 1) * PasV;
 
-      UV(1) = U0 + (aSph.U() - 1) * PasU;
-      UV(2) = V0 + (aSph.V() - 1) * PasV;
-
-      FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MAX);
+      FindSolution(P, UV, aSph.U(),aSph.V(), Extrema_ExtFlag_MAX);
     }
   }
 }
diff --git a/src/math/math_FunctionSetRoot.cxx b/src/math/math_FunctionSetRoot.cxx
index f0c5594915..9f338b3c5b 100755
--- a/src/math/math_FunctionSetRoot.cxx
+++ b/src/math/math_FunctionSetRoot.cxx
@@ -747,7 +747,7 @@ void math_FunctionSetRoot::Perform(math_FunctionSetWithDerivatives& F,
   // s'il on est dejas sur la solution, il faut leurer ce test pour eviter
   // de faire une seconde iteration...
   Save(0) = Max (F2, EpsSqrt);
-
+  Standard_Real aTol_Func = Epsilon(F2);
   FSR_DEBUG("=== Mode Debug de Function Set Root" << endl)
   FSR_DEBUG("    F2 Initial = " << F2)
 
@@ -1038,12 +1038,18 @@ void math_FunctionSetRoot::Perform(math_FunctionSetWithDerivatives& F,
     //fin du test solution
    
     // Analyse de la progression...
-    if (F2 < PreviousMinimum) { 
+    //comparison of current minimum and previous minimum
+    if ((F2 - PreviousMinimum) <= aTol_Func){ 
       if (Kount > 5) {
 	// L'historique est il bon ?
 	if (F2 >= 0.95*Save(Kount - 5)) {
 	  if (!ChangeDirection) ChangeDirection = Standard_True;
-	  else return; //  si un gain inf a 5% on sort
+	  else 
+    {
+      Done = Standard_True;
+      State = F.GetStateNumber();
+      return; //  si un gain inf a 5% on sort
+    }
 	}
 	else ChangeDirection = Standard_False; //Si oui on recommence
       }
@@ -1084,9 +1090,15 @@ void math_FunctionSetRoot::Perform(math_FunctionSetWithDerivatives& F,
 	  return;
 	}
       }
-      else return; // y a plus d'issues
+      else 
+      {
+        
+        State = F.GetStateNumber();
+        return; // y a plus d'issues
+      }
     }
   }
+  State = F.GetStateNumber();
 }
 
 
-- 
2.20.1