0022883: Extrema can not find projection of 3D point on surface.
[occt.git] / src / Extrema / Extrema_GenExtPS.cxx
1 // Created on: 1995-07-18
2 // Created by: Modelistation
3 // Copyright (c) 1995-1999 Matra Datavision
4 // Copyright (c) 1999-2012 OPEN CASCADE SAS
5 //
6 // The content of this file is subject to the Open CASCADE Technology Public
7 // License Version 6.5 (the "License"). You may not use the content of this file
8 // except in compliance with the License. Please obtain a copy of the License
9 // at http://www.opencascade.org and read it completely before using this file.
10 //
11 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
12 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
13 //
14 // The Original Code and all software distributed under the License is
15 // distributed on an "AS IS" basis, without warranty of any kind, and the
16 // Initial Developer hereby disclaims all such warranties, including without
17 // limitation, any warranties of merchantability, fitness for a particular
18 // purpose or non-infringement. Please see the License for the specific terms
19 // and conditions governing the rights and limitations under the License.
20
21
22 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593
23
24
25 #include <Extrema_GenExtPS.ixx>
26 #include <StdFail_NotDone.hxx>
27 #include <Standard_OutOfRange.hxx>
28 #include <TColStd_Array2OfInteger.hxx>
29 #include <TColStd_Array2OfReal.hxx>
30 #include <TColgp_Array2OfPnt.hxx>
31 #include <math_FunctionSetRoot.hxx>
32 #include <math_Vector.hxx>
33 #include <math_NewtonFunctionSetRoot.hxx>
34 #include <GeomAbs_IsoType.hxx>
35 #include <Bnd_Sphere.hxx>
36 #include <Extrema_HUBTreeOfSphere.hxx>
37 #include <Extrema_ExtFlag.hxx>
38 #include <Bnd_Array1OfSphere.hxx>
39 #include <Bnd_HArray1OfSphere.hxx>
40 #include <Precision.hxx>
41 #include <Geom_OffsetSurface.hxx>
42 #include <Geom_RectangularTrimmedSurface.hxx>
43 #include <Geom_BSplineSurface.hxx>
44 #include <Geom_BezierSurface.hxx>
45 #include <Adaptor3d_HSurface.hxx>
46 #include <Adaptor3d_HCurve.hxx>
47 #include <Adaptor3d_Curve.hxx>
48 #include <Geom_BSplineCurve.hxx>
49 #include <Geom_BezierCurve.hxx>
50 //IMPLEMENT_HARRAY1(Extrema_HArray1OfSphere)
51
52
53 class Bnd_SphereUBTreeSelector : public Extrema_UBTreeOfSphere::Selector
54 {
55  public:
56
57   Bnd_SphereUBTreeSelector (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
58 Bnd_Sphere& theSol)
59     : myXYZ(0,0,0),
60       mySphereArray(theSphereArray),
61       mySol(theSol)
62   {
63     //myXYZ = gp_Pnt(0, 0, 0);    
64   }
65
66   void DefineCheckPoint( const gp_Pnt& theXYZ )
67   { myXYZ = theXYZ; }
68
69   Bnd_Sphere& Sphere() const
70   { return mySol; }
71
72   virtual Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const = 0;
73   
74   virtual Standard_Boolean Accept(const Standard_Integer& theObj) = 0;
75
76  protected:
77   gp_Pnt                      myXYZ;
78   const Handle(Bnd_HArray1OfSphere)& mySphereArray;
79   Bnd_Sphere&                                            mySol;
80
81 };
82
83 class Bnd_SphereUBTreeSelectorMin : public Bnd_SphereUBTreeSelector
84 {
85 public:
86   Bnd_SphereUBTreeSelectorMin (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
87                 Bnd_Sphere& theSol)
88                 : Bnd_SphereUBTreeSelector(theSphereArray, theSol),
89                   myMinDist(RealLast())
90   {}
91   
92   void SetMinDist( const Standard_Real theMinDist )
93   { myMinDist = theMinDist; }
94
95   Standard_Real MinDist() const
96   { return myMinDist; }
97
98   Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const
99   { 
100     Bnd_SphereUBTreeSelectorMin* me =
101       const_cast<Bnd_SphereUBTreeSelectorMin*>(this);
102     // myMinDist is decreased each time a nearer object is found
103     return theBnd.IsOut( myXYZ.XYZ(), me->myMinDist );
104   }
105
106   Standard_Boolean Accept(const Standard_Integer&);
107
108 private:
109         Standard_Real   myMinDist;
110 };
111
112 Standard_Boolean Bnd_SphereUBTreeSelectorMin::Accept(const Standard_Integer& theInd)
113 {
114   const Bnd_Sphere& aSph = mySphereArray->Value(theInd);
115   Standard_Real aCurDist;
116
117     if ( (aCurDist = aSph.SquareDistance(myXYZ.XYZ())) < mySol.SquareDistance(myXYZ.XYZ()) )
118     {
119       mySol = aSph;
120       if ( aCurDist < myMinDist ) 
121         myMinDist = aCurDist;
122
123       return Standard_True;
124     }
125
126   return Standard_False;
127 }
128
129 class Bnd_SphereUBTreeSelectorMax : public Bnd_SphereUBTreeSelector
130 {
131 public:
132   Bnd_SphereUBTreeSelectorMax (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
133                 Bnd_Sphere& theSol)
134                 : Bnd_SphereUBTreeSelector(theSphereArray, theSol),
135                   myMaxDist(0)
136   {}
137
138   void SetMaxDist( const Standard_Real theMaxDist )
139   { myMaxDist = theMaxDist; }
140
141   Standard_Real MaxDist() const
142   { return myMaxDist; }
143
144   Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const
145   { 
146     Bnd_SphereUBTreeSelectorMax* me =
147       const_cast<Bnd_SphereUBTreeSelectorMax*>(this);
148     // myMaxDist is decreased each time a nearer object is found
149     return theBnd.IsOut( myXYZ.XYZ(), me->myMaxDist );
150   }
151
152   Standard_Boolean Accept(const Standard_Integer&);
153
154 private:
155         Standard_Real   myMaxDist;
156 };
157
158 Standard_Boolean Bnd_SphereUBTreeSelectorMax::Accept(const Standard_Integer& theInd)
159 {
160   const Bnd_Sphere& aSph = mySphereArray->Value(theInd);
161   Standard_Real aCurDist;
162
163     if ( (aCurDist = aSph.SquareDistance(myXYZ.XYZ())) > mySol.SquareDistance(myXYZ.XYZ()) )
164     {
165       mySol = aSph;
166       if ( aCurDist > myMaxDist ) 
167         myMaxDist = aCurDist;
168
169       return Standard_True;
170     }
171
172   return Standard_False;
173 }
174
175
176
177 //=============================================================================
178
179 /*-----------------------------------------------------------------------------
180 Function:
181    Find all extremum distances between point P and surface
182   S using sampling (NbU,NbV).
183
184 Method:
185    The algorithm bases on the hypothesis that sampling is precise enough, 
186   if there exist N extreme distances between the point and the surface,
187   so there also exist N extrema between the point and the grid.
188   So, the algorithm consists in starting from extrema of the grid to find the 
189   extrema of the surface.
190   The extrema are calculated by the algorithm math_FunctionSetRoot with the
191   following arguments:
192   - F: Extrema_FuncExtPS created from P and S,
193   - UV: math_Vector the components which of are parameters of the extremum on the 
194     grid,
195   - Tol: Min(TolU,TolV), (Prov.:math_FunctionSetRoot does not autorize a vector)
196   - UVinf: math_Vector the components which of are lower limits of u and v,
197   - UVsup: math_Vector the components which of are upper limits of u and v.
198
199 Processing:
200   a- Creation of the table of distances (TbDist(0,NbU+1,0,NbV+1)):
201      The table is expanded at will; lines 0 and NbU+1 and
202      columns 0 and NbV+1 are initialized at RealFirst() or RealLast()
203      to simplify the tests carried out at stage b
204      (there is no need to test if the point is on border of the grid).
205   b- Calculation of extrema:
206      First the minimums and then the maximums are found. These 2 procedured 
207      pass in a similar way:
208   b.a- Initialization:
209       - 'borders' of table  TbDist (RealLast() in case of minimums
210         and  RealLast() in case of maximums),
211       - table TbSel(0,NbU+1,0,NbV+1) of selection of points for 
212         calculation of local extremum (0). When a point will selected,
213         it will not be selectable, as well as the ajacent points 
214         (8 at least). The corresponding addresses will be set to 1.
215   b.b- Calculation of minimums (or maximums):
216        All distances from table TbDist are parsed in a loop:
217       - search minimum (or maximum) in the grid,
218       - calculate extremum on the surface,
219       - update table TbSel.
220 -----------------------------------------------------------------------------*/
221
222 static Standard_Boolean IsoIsDeg  (const Adaptor3d_Surface& S,
223                                    const Standard_Real      Param,
224                                    const GeomAbs_IsoType    IT,
225                                    const Standard_Real      TolMin,
226                                    const Standard_Real      TolMax) 
227 {
228     Standard_Real U1=0.,U2=0.,V1=0.,V2=0.,T;
229     Standard_Boolean Along = Standard_True;
230     U1 = S.FirstUParameter();
231     U2 = S.LastUParameter();
232     V1 = S.FirstVParameter();
233     V2 = S.LastVParameter();
234     gp_Vec D1U,D1V;
235     gp_Pnt P;
236     Standard_Real Step,D1NormMax;
237     if (IT == GeomAbs_IsoV) 
238     {
239       Step = (U2 - U1)/10;
240       D1NormMax=0.;
241       for (T=U1;T<=U2;T=T+Step) 
242       {
243         S.D1(T,Param,P,D1U,D1V);
244         D1NormMax=Max(D1NormMax,D1U.Magnitude());
245       }
246
247       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
248            Along = Standard_False;
249     }
250     else 
251     {
252       Step = (V2 - V1)/10;
253       D1NormMax=0.;
254       for (T=V1;T<=V2;T=T+Step) 
255       {
256         S.D1(Param,T,P,D1U,D1V);
257         D1NormMax=Max(D1NormMax,D1V.Magnitude());
258       }
259
260       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
261            Along = Standard_False;
262
263
264     }
265     return Along;
266 }
267 //----------------------------------------------------------
268 Extrema_GenExtPS::Extrema_GenExtPS() 
269 {
270   myDone = Standard_False;
271   myInit = Standard_False;
272   myFlag = Extrema_ExtFlag_MINMAX;
273   myAlgo = Extrema_ExtAlgo_Grad;
274 }
275
276
277 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
278                               const Adaptor3d_Surface& S,
279                               const Standard_Integer NbU, 
280                               const Standard_Integer NbV,
281                               const Standard_Real    TolU, 
282                               const Standard_Real    TolV,
283                               const Extrema_ExtFlag F,
284                               const Extrema_ExtAlgo A) 
285   : myF (P,S), myFlag(F), myAlgo(A)
286 {
287   Initialize(S, NbU, NbV, TolU, TolV);
288   Perform(P);
289 }
290
291 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
292                               const Adaptor3d_Surface& S,
293                               const Standard_Integer NbU, 
294                               const Standard_Integer NbV,
295                               const Standard_Real    Umin,
296                               const Standard_Real    Usup,
297                               const Standard_Real    Vmin,
298                               const Standard_Real    Vsup,
299                               const Standard_Real    TolU, 
300                               const Standard_Real    TolV,
301                               const Extrema_ExtFlag F,
302                               const Extrema_ExtAlgo A) 
303   : myF (P,S), myFlag(F), myAlgo(A)
304 {
305   Initialize(S, NbU, NbV, Umin, Usup, Vmin, Vsup, TolU, TolV);
306   Perform(P);
307 }
308
309
310 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
311                                   const Standard_Integer NbU, 
312                                   const Standard_Integer NbV,
313                                   const Standard_Real    TolU, 
314                                   const Standard_Real    TolV)
315 {
316   myumin = S.FirstUParameter();
317   myusup = S.LastUParameter();
318   myvmin = S.FirstVParameter();
319   myvsup = S.LastVParameter();
320   Initialize(S,NbU,NbV,myumin,myusup,myvmin,myvsup,TolU,TolV);
321 }
322
323
324 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
325                                   const Standard_Integer NbU, 
326                                   const Standard_Integer NbV,
327                                   const Standard_Real    Umin,
328                                   const Standard_Real    Usup,
329                                   const Standard_Real    Vmin,
330                                   const Standard_Real    Vsup,
331                                   const Standard_Real    TolU, 
332                                   const Standard_Real    TolV)
333 {
334   myS = (Adaptor3d_SurfacePtr)&S;
335   myusample = NbU;
336   myvsample = NbV;
337   mytolu = TolU;
338   mytolv = TolV;
339   myumin = Umin;
340   myusup = Usup;
341   myvmin = Vmin;
342   myvsup = Vsup;
343
344   if ((myusample < 2) ||
345       (myvsample < 2)) { Standard_OutOfRange::Raise(); }
346
347   myF.Initialize(S);
348
349   mySphereUBTree.Nullify();
350   myUParams.Nullify();
351   myVParams.Nullify();
352   myInit = Standard_False;
353 }
354
355 inline static void fillParams(const TColStd_Array1OfReal& theKnots,
356                               Standard_Integer theDegree,
357                               Standard_Real theParMin,
358                               Standard_Real theParMax,
359                               Handle_TColStd_HArray1OfReal& theParams,
360                               Standard_Integer theSample)
361 {
362   NCollection_Vector<Standard_Real> aParams;
363   Standard_Integer i = 1;
364   Standard_Real aPrevPar = theParMin;
365   aParams.Append(aPrevPar);
366   //calculation the array of parametric points depending on the knots array variation and degree of given surface
367   for ( ; i <  theKnots.Length() && theKnots(i) < (theParMax - Precision::PConfusion()); i++ )
368   {
369     if (  theKnots(i+1) < theParMin + Precision::PConfusion())
370       continue;
371
372     Standard_Real aStep = (theKnots(i+1) - theKnots(i))/Max(theDegree,2);
373     Standard_Integer k =1;
374     for( ; k <= theDegree ; k++)
375     {
376       Standard_Real aPar = theKnots(i) + k * aStep;
377       if(aPar > theParMax - Precision::PConfusion())
378         break;
379       if(aPar > aPrevPar + Precision::PConfusion() )
380       {
381         aParams.Append(aPar);
382         aPrevPar = aPar;
383       }
384     }
385
386   }
387   aParams.Append(theParMax);
388   Standard_Integer nbPar = aParams.Length();
389   //in case of an insufficient number of points the grid will be built later 
390   if (nbPar < theSample)
391     return;
392   theParams = new TColStd_HArray1OfReal(1, nbPar );
393   for( i = 0; i < nbPar; i++)
394     theParams->SetValue(i+1,aParams(i));
395 }
396
397 void Extrema_GenExtPS::GetGridPoints( const Adaptor3d_Surface& theSurf)
398 {
399   //creation parametric points for BSpline and Bezier surfaces
400   //with taking into account of Degree and NbKnots of BSpline or Bezier geometry
401   if(theSurf.GetType() == GeomAbs_OffsetSurface)
402     GetGridPoints(theSurf.BasisSurface()->Surface());
403   //parametric points for BSpline surfaces
404   else if( theSurf.GetType() == GeomAbs_BSplineSurface) 
405   {
406     Handle(Geom_BSplineSurface) aBspl = theSurf.BSpline();
407     if(!aBspl.IsNull())
408     {
409       TColStd_Array1OfReal aUKnots(1, aBspl->NbUKnots());
410       aBspl->UKnots( aUKnots);
411       TColStd_Array1OfReal aVKnots(1, aBspl->NbVKnots());
412       aBspl->VKnots( aVKnots);
413       fillParams(aUKnots,aBspl->UDegree(),myumin, myusup, myUParams, myusample);
414       fillParams(aVKnots,aBspl->VDegree(),myvmin, myvsup, myVParams, myvsample);
415     }
416   }
417   //calculation parametric points for Bezier surfaces
418   else if(theSurf.GetType() == GeomAbs_BezierSurface)
419   {
420     Handle(Geom_BezierSurface) aBezier = theSurf.Bezier();
421     if(aBezier.IsNull())
422       return;
423
424     TColStd_Array1OfReal aUKnots(1,2);
425     TColStd_Array1OfReal aVKnots(1,2);
426     aBezier->Bounds(aUKnots(1), aUKnots(2), aVKnots(1), aVKnots(2));
427     fillParams(aUKnots, aBezier->UDegree(), myumin, myusup, myUParams, myusample);
428     fillParams(aVKnots, aBezier->VDegree(), myvmin, myvsup, myVParams, myvsample);
429
430   }
431   //creation points for surfaces based on BSpline or Bezier curves
432   else if(theSurf.GetType() == GeomAbs_SurfaceOfRevolution || 
433     theSurf.GetType() == GeomAbs_SurfaceOfExtrusion)
434   {
435     Handle(TColStd_HArray1OfReal) anArrKnots;
436     Standard_Integer aDegree = 0;
437     GeomAbs_CurveType aType = theSurf.BasisCurve()->Curve().GetType();
438     if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BSplineCurve)
439     {
440       Handle(Geom_BSplineCurve) aBspl = theSurf.BasisCurve()->Curve().BSpline();
441       if(!aBspl.IsNull())
442       {
443         anArrKnots = new TColStd_HArray1OfReal(1,aBspl->NbKnots());
444         aBspl->Knots( anArrKnots->ChangeArray1() );
445         aDegree = aBspl->Degree();
446         
447       }
448
449     }
450     if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BezierCurve)
451     {
452       Handle(Geom_BezierCurve) aBez = theSurf.BasisCurve()->Curve().Bezier();
453       if(!aBez.IsNull())
454       {
455         anArrKnots = new TColStd_HArray1OfReal(1,2);
456         anArrKnots->SetValue(1, aBez->FirstParameter());
457         anArrKnots->SetValue(2, aBez->LastParameter());
458         aDegree = aBez->Degree();
459         
460       }
461     }
462     if(anArrKnots.IsNull())
463       return;
464     if( theSurf.GetType() == GeomAbs_SurfaceOfRevolution )
465       fillParams( anArrKnots->Array1(), aDegree, myvmin, myvsup, myVParams, myvsample);
466     else
467       fillParams( anArrKnots->Array1(), aDegree, myumin, myusup, myUParams, myusample);
468   }
469   //update the number of points in sample
470   if(!myUParams.IsNull())
471     myusample = myUParams->Length();
472   if( !myVParams.IsNull())
473     myvsample = myVParams->Length();
474   
475 }
476
477 void Extrema_GenExtPS::BuildGrid()
478 {
479   //if grid was already built do nothing
480   if(myInit)
481     return;
482   
483   //build parametric grid in case of a complex surface geometry (BSpline and Bezier surfaces)
484  GetGridPoints(*myS);
485   
486   //build grid in other cases 
487   if( myUParams.IsNull() )
488   {
489     Standard_Real PasU = myusup - myumin;
490     Standard_Real U0 = PasU / myusample / 100.;
491     PasU = (PasU - U0) / (myusample - 1);
492     U0 = U0/2. + myumin;
493     myUParams = new TColStd_HArray1OfReal(1,myusample );
494     Standard_Integer NoU;
495     Standard_Real U = U0;
496     for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
497       myUParams->SetValue(NoU, U);
498
499   }
500
501   if( myVParams.IsNull())
502   {
503     Standard_Real PasV = myvsup - myvmin;
504     Standard_Real V0 = PasV / myvsample / 100.;
505     PasV = (PasV - V0) / (myvsample - 1);
506     V0 = V0/2. + myvmin;
507     
508     myVParams = new TColStd_HArray1OfReal(1,myvsample );
509     Standard_Integer  NoV;
510     Standard_Real V = V0;
511    
512     for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
513       myVParams->SetValue(NoV, V);
514   }
515   //If flag was changed and extrema not reinitialized Extrema would fail
516   mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
517   // Calculation of distances
518
519   Standard_Integer NoU, NoV;
520  
521   for ( NoU = 1 ; NoU <= myusample; NoU++ ) {
522     for ( NoV = 1 ; NoV <= myvsample; NoV++) {
523       gp_Pnt P1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
524       mypoints->SetValue(NoU,NoV,P1);
525     }
526   }
527   
528   myInit = Standard_True;
529 }
530
531
532
533
534   
535 /*
536 a- Constitution of the table of distances (TbDist(0,myusample+1,0,myvsample+1)):
537    ---------------------------------------------------------------
538 */
539
540 // Parameterisation of the sample
541
542 void Extrema_GenExtPS::BuildTree()
543 {
544   // if tree already exists, assume it is already correctly filled
545   if ( ! mySphereUBTree.IsNull() )
546     return;
547
548   Standard_Real PasU = myusup - myumin;
549   Standard_Real PasV = myvsup - myvmin;
550   Standard_Real U0 = PasU / myusample / 100.;
551   Standard_Real V0 = PasV / myvsample / 100.;
552   gp_Pnt P1;
553   PasU = (PasU - U0) / (myusample - 1);
554   PasV = (PasV - V0) / (myvsample - 1);
555   U0 = U0/2. + myumin;
556   V0 = V0/2. + myvmin;
557
558   //build grid of parametric points 
559   myUParams = new TColStd_HArray1OfReal(1,myusample );
560   myVParams = new TColStd_HArray1OfReal(1,myvsample );
561   Standard_Integer NoU, NoV;
562   Standard_Real U = U0, V = V0;
563   for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
564     myUParams->SetValue(NoU, U);
565   for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
566     myVParams->SetValue(NoV, V);
567
568   // Calculation of distances
569   mySphereUBTree = new Extrema_UBTreeOfSphere;
570   Extrema_UBTreeFillerOfSphere aFiller(*mySphereUBTree);
571   Standard_Integer i = 0;
572   
573   mySphereArray = new Bnd_HArray1OfSphere(0, myusample * myvsample);
574  
575   for ( NoU = 1; NoU <= myusample; NoU++ ) {
576     for ( NoV = 1; NoV <= myvsample; NoV++) {
577       P1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
578       Bnd_Sphere aSph(P1.XYZ(), 0/*mytolu < mytolv ? mytolu : mytolv*/, NoU, NoV);
579       aFiller.Add(i, aSph);
580       mySphereArray->SetValue( i, aSph );
581       i++;
582     }
583   }
584   aFiller.Fill();
585 }
586
587 void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, 
588                                     const math_Vector& UV, 
589                                     const Standard_Integer theNoU, 
590                                     const Standard_Integer theNoV, const Extrema_ExtFlag f)
591 {
592   math_Vector Tol(1,2);
593   Tol(1) = mytolu;
594   Tol(2) = mytolv;
595
596   math_Vector UVinf(1,2), UVsup(1,2);
597   UVinf(1) = myumin;
598   UVinf(2) = myvmin;
599   UVsup(1) = myusup;
600   UVsup(2) = myvsup;
601
602   math_Vector errors(1,2);
603   math_Vector root(1, 2);
604   Standard_Real eps = 1.e-9;
605   Standard_Integer nbsubsample = 11;
606
607   Standard_Integer aNbMaxIter = 100;
608
609   gp_Pnt PStart = myS->Value(UV(1), UV(2));
610   Standard_Real DistStart = P.SquareDistance(PStart);
611   Standard_Real DistSol = DistStart;
612   
613   math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
614   Standard_Boolean ToResolveOnSubgrid = Standard_False;
615   Standard_Boolean NewSolution = Standard_False;
616   if (f == Extrema_ExtFlag_MIN)
617   {
618     if(S.IsDone())
619     {
620       root = S.Root();
621       myF.Value(root, errors);
622       gp_Pnt PSol = myS->Value(root(1), root(2));
623       DistSol = P.SquareDistance(PSol);
624       if(Abs(errors(1)) > eps || Abs(errors(2)) > eps || DistStart < DistSol)
625       {
626         //try to improve solution on subgrid of sample points
627         ToResolveOnSubgrid = Standard_True;
628       }
629     }
630     else
631     {
632       //keep found roots and try to find solution
633       Standard_Integer nbExt = myF.NbExt();
634       Standard_Integer k = 1;
635       for( ; k <= nbExt; k++)
636       {
637         Standard_Real aD = myF.SquareDistance(k);
638         if(aD < DistSol)
639         {
640           DistSol = aD;
641           myF.Point(k).Parameter(UV(1),UV(2));
642           NewSolution = Standard_True;
643         }
644       }
645       ToResolveOnSubgrid = Standard_True;
646     }
647     
648     if (ToResolveOnSubgrid)
649     {
650       //extension of interval to find new solution
651       Standard_Real u1 = theNoU == 1 ? myumin : myUParams->Value(theNoU-1), 
652         u2 = theNoU == myusample ? myusup : myUParams->Value(theNoU + 1);
653       Standard_Real v1 = theNoV == 1 ? myvmin : myVParams->Value(theNoV-1),
654         v2 = theNoV == myvsample ? myvsup : myVParams->Value(theNoV + 1);
655             
656       Standard_Real du = (u2 - u1)/(nbsubsample-1);
657       Standard_Real dv = (v2 - v1)/(nbsubsample-1);
658       Standard_Real u, v;
659       Standard_Real dist;
660       
661       //try to find solution on subgrid
662       Standard_Integer Nu, Nv;
663       Standard_Integer minU = 0;
664       Standard_Integer minV = 0;
665       for (Nu = 1, u = u1; Nu < nbsubsample; Nu++, u += du) {
666         for (Nv = 1, v = v1; Nv < nbsubsample; Nv++, v += dv) {
667           gp_Pnt Puv = myS->Value(u, v);
668           dist = P.SquareDistance(Puv);
669           
670           if(dist < DistSol) {
671             UV(1) = u;
672             UV(2) = v;
673             NewSolution = Standard_True;
674             DistSol = dist;
675             minU = Nu;
676             minV = Nv;
677           }
678         }
679       }
680       
681       if(NewSolution) {
682         //try to precise
683         UVinf(1) = u1 + (minU == 1 ? 0 : minU - 2) * du;
684         UVinf(2) = v1 + (minV == 1 ? 0 : minV - 2) * dv;
685         UVsup(1) = u1 + (minU == nbsubsample ? nbsubsample : minU +1) * du;;
686         UVsup(2) = v1 + (minV == nbsubsample ? nbsubsample : minV +1) * dv;
687         math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
688
689       }
690     } //end of if (ToResolveOnSubgrid)
691   } //end of if (f == Extrema_ExtFlag_MIN)
692   
693   myDone = Standard_True;
694 }
695
696 void Extrema_GenExtPS::SetFlag(const Extrema_ExtFlag F)
697 {
698   myFlag = F;
699 }
700
701 void Extrema_GenExtPS::SetAlgo(const Extrema_ExtAlgo A)
702 {
703   if(myAlgo != A)
704      myInit = Standard_False;
705   myAlgo = A;
706  
707 }
708
709 void Extrema_GenExtPS::Perform(const gp_Pnt& P) 
710 {  
711   myDone = Standard_False;
712   myF.SetPoint(P);
713   
714   math_Vector UV(1,2);
715
716   if(myAlgo == Extrema_ExtAlgo_Grad)
717   {
718     BuildGrid();
719     Standard_Integer NoU,NoV;
720
721     TColStd_Array2OfReal TheDist(0, myusample+1, 0, myvsample+1);
722     
723     for ( NoU = 1 ; NoU <= myusample; NoU++) {
724       for ( NoV = 1; NoV <= myvsample; NoV++) {
725         TheDist(NoU, NoV) = P.SquareDistance(mypoints->Value(NoU, NoV));
726       }
727     }
728
729    
730
731     Standard_Real Dist;
732
733     if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX) 
734     {
735       for (NoV = 0; NoV <= myvsample+1; NoV++) {
736         TheDist(0,NoV) = RealLast();
737         TheDist(myusample+1,NoV) = RealLast();
738       }
739       for (NoU = 1; NoU <= myusample; NoU++) {
740         TheDist(NoU,0) = RealLast();
741         TheDist(NoU,myvsample+1) = RealLast();
742       }
743       for (NoU = 1; NoU <= myusample; NoU++) {
744         for (NoV = 1; NoV <= myvsample; NoV++) {
745             Dist = TheDist(NoU,NoV);
746             if ((TheDist(NoU-1,NoV-1) >= Dist) &&
747               (TheDist(NoU-1,NoV  ) >= Dist) &&
748               (TheDist(NoU-1,NoV+1) >= Dist) &&
749               (TheDist(NoU  ,NoV-1) >= Dist) &&
750               (TheDist(NoU  ,NoV+1) >= Dist) &&
751               (TheDist(NoU+1,NoV-1) >= Dist) &&
752               (TheDist(NoU+1,NoV  ) >= Dist) &&
753               (TheDist(NoU+1,NoV+1) >= Dist)) {
754                 //Create array of UV vectors to calculate min
755                 
756                 UV(1) = myUParams->Value(NoU);
757                 UV(2) = myVParams->Value(NoV);
758                 FindSolution(P, UV, NoU, NoV, Extrema_ExtFlag_MIN);
759             }
760          
761         }
762       }
763     }
764     
765     if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
766     {
767       for (NoV = 0; NoV <= myvsample+1; NoV++) {
768         TheDist(0,NoV) = RealFirst();
769         TheDist(myusample+1,NoV) = RealFirst();
770       }
771       for (NoU = 1; NoU <= myusample; NoU++) {
772         TheDist(NoU,0) = RealFirst();
773         TheDist(NoU,myvsample+1) = RealFirst();
774       }
775       for (NoU = 1; NoU <= myusample; NoU++) {
776         for (NoV = 1; NoV <= myvsample; NoV++) {
777             Dist = TheDist(NoU,NoV);
778             if ((TheDist(NoU-1,NoV-1) <= Dist) &&
779               (TheDist(NoU-1,NoV  ) <= Dist) &&
780               (TheDist(NoU-1,NoV+1) <= Dist) &&
781               (TheDist(NoU  ,NoV-1) <= Dist) &&
782               (TheDist(NoU  ,NoV+1) <= Dist) &&
783               (TheDist(NoU+1,NoV-1) <= Dist) &&
784               (TheDist(NoU+1,NoV  ) <= Dist) &&
785               (TheDist(NoU+1,NoV+1) <= Dist)) {
786                 //Create array of UV vectors to calculate max
787                 UV(1) = myUParams->Value(NoU);
788                 UV(2) = myVParams->Value(NoV);
789                 FindSolution(P, UV, NoU, NoV, Extrema_ExtFlag_MAX);
790             }
791          }
792       }
793     }
794   }
795   else
796   {
797     BuildTree();
798     if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX)
799     {
800       Bnd_Sphere aSol = mySphereArray->Value(0);
801       Bnd_SphereUBTreeSelectorMin aSelector(mySphereArray, aSol);
802       //aSelector.SetMaxDist( RealLast() );
803       aSelector.DefineCheckPoint( P );
804       Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
805       //TODO: check if no solution in binary tree
806       Bnd_Sphere& aSph = aSelector.Sphere();
807
808       UV(1) = myUParams->Value(aSph.U());//U0 + (aSph.U() - 1) * PasU;
809       UV(2) = myVParams->Value(aSph.V());//V0 + (aSph.V() - 1) * PasV;
810
811       //FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
812       FindSolution(P, UV, aSph.U(), aSph.V(), Extrema_ExtFlag_MIN);
813     }
814     if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
815     {
816       Bnd_Sphere aSol = mySphereArray->Value(0);
817       Bnd_SphereUBTreeSelectorMax aSelector(mySphereArray, aSol);
818       //aSelector.SetMaxDist( RealLast() );
819       aSelector.DefineCheckPoint( P );
820       Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
821       //TODO: check if no solution in binary tree
822       Bnd_Sphere& aSph = aSelector.Sphere();
823       UV(1) = myUParams->Value(aSph.U());
824       UV(2) = myVParams->Value(aSph.V());
825       //UV(1) = U0 + (aSph.U() - 1) * PasU;
826       //UV(2) = V0 + (aSph.V() - 1) * PasV;
827
828       FindSolution(P, UV, aSph.U(),aSph.V(), Extrema_ExtFlag_MAX);
829     }
830   }
831 }
832 //=============================================================================
833
834 Standard_Boolean Extrema_GenExtPS::IsDone () const { return myDone; }
835 //=============================================================================
836
837 Standard_Integer Extrema_GenExtPS::NbExt () const
838 {
839   if (!IsDone()) { StdFail_NotDone::Raise(); }
840   return myF.NbExt();
841 }
842 //=============================================================================
843
844 Standard_Real Extrema_GenExtPS::SquareDistance (const Standard_Integer N) const
845 {
846   if (!IsDone()) { StdFail_NotDone::Raise(); }
847   return myF.SquareDistance(N);
848 }
849 //=============================================================================
850
851 Extrema_POnSurf Extrema_GenExtPS::Point (const Standard_Integer N) const
852 {
853   if (!IsDone()) { StdFail_NotDone::Raise(); }
854   return myF.Point(N);
855 }
856 //=============================================================================