0022610: The algorithm GeomAPI_ProjectPointOnSurf produces wrong results
[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  * This function computes the point on surface parameters on edge.
177  * if it coincides with theParam0 or theParam1, it is returned.
178  */
179 static Extrema_POnSurfParams ComputeEdgeParameters
180       (const Standard_Boolean       IsUEdge,
181        const Extrema_POnSurfParams &theParam0,
182        const Extrema_POnSurfParams &theParam1,
183        const Adaptor3d_SurfacePtr  &theSurf,
184        const gp_Pnt                &thePoint,
185        const Standard_Real          theDiffTol)
186 {
187   const Standard_Real aSqrDist01 =
188     theParam0.Value().SquareDistance(theParam1.Value());
189
190   if (aSqrDist01 <= theDiffTol) {
191     // The points are confused. Get the first point and change its type.
192     return theParam0;
193   } else {
194     const Standard_Real aDiffDist =
195       Abs(theParam0.GetSqrDistance() - theParam1.GetSqrDistance());
196
197     if (aDiffDist >= aSqrDist01 - theDiffTol) {
198       // The shortest distance is one of the nodes.
199       if (theParam0.GetSqrDistance() > theParam1.GetSqrDistance()) {
200         // The shortest distance is the point 1.
201         return theParam1;
202       } else {
203         // The shortest distance is the point 0.
204         return theParam0;
205       }
206     } else {
207       // The shortest distance is inside the edge.
208       gp_XYZ aPoP(thePoint.XYZ().Subtracted(theParam0.Value().XYZ()));
209       gp_XYZ aPoP1(theParam1.Value().XYZ().Subtracted(theParam0.Value().XYZ()));
210       Standard_Real aRatio = aPoP.Dot(aPoP1)/aSqrDist01;
211       Standard_Real aU[2];
212       Standard_Real aV[2];
213
214       theParam0.Parameter(aU[0], aV[0]);
215       theParam1.Parameter(aU[1], aV[1]);
216   
217       Standard_Real aUPar = aU[0];
218       Standard_Real aVPar = aV[0];
219
220       if (IsUEdge) {
221         aUPar += aRatio*(aU[1] - aU[0]);
222       } else {
223         aVPar += aRatio*(aV[1] - aV[0]);
224       }
225
226       Extrema_POnSurfParams aParam(aUPar, aVPar, theSurf->Value(aUPar, aVPar));
227       Standard_Integer anIndices[2];
228
229       theParam0.GetIndices(anIndices[0], anIndices[1]);
230       aParam.SetElementType(IsUEdge ? Extrema_UIsoEdge : Extrema_VIsoEdge);
231       aParam.SetSqrDistance(thePoint.SquareDistance(aParam.Value()));
232       aParam.SetIndices(anIndices[0], anIndices[1]);
233
234       return aParam;
235     }
236   }
237 }
238
239 //=============================================================================
240
241 /*-----------------------------------------------------------------------------
242 Function:
243    Find all extremum distances between point P and surface
244   S using sampling (NbU,NbV).
245
246 Method:
247    The algorithm bases on the hypothesis that sampling is precise enough, 
248   if there exist N extreme distances between the point and the surface,
249   so there also exist N extrema between the point and the grid.
250   So, the algorithm consists in starting from extrema of the grid to find the 
251   extrema of the surface.
252   The extrema are calculated by the algorithm math_FunctionSetRoot with the
253   following arguments:
254   - F: Extrema_FuncExtPS created from P and S,
255   - UV: math_Vector the components which of are parameters of the extremum on the 
256     grid,
257   - Tol: Min(TolU,TolV), (Prov.:math_FunctionSetRoot does not autorize a vector)
258   - UVinf: math_Vector the components which of are lower limits of u and v,
259   - UVsup: math_Vector the components which of are upper limits of u and v.
260
261 Processing:
262   a- Creation of the table of distances (TbDist(0,NbU+1,0,NbV+1)):
263      The table is expanded at will; lines 0 and NbU+1 and
264      columns 0 and NbV+1 are initialized at RealFirst() or RealLast()
265      to simplify the tests carried out at stage b
266      (there is no need to test if the point is on border of the grid).
267   b- Calculation of extrema:
268      First the minimums and then the maximums are found. These 2 procedured 
269      pass in a similar way:
270   b.a- Initialization:
271       - 'borders' of table  TbDist (RealLast() in case of minimums
272         and  RealLast() in case of maximums),
273       - table TbSel(0,NbU+1,0,NbV+1) of selection of points for 
274         calculation of local extremum (0). When a point will selected,
275         it will not be selectable, as well as the ajacent points 
276         (8 at least). The corresponding addresses will be set to 1.
277   b.b- Calculation of minimums (or maximums):
278        All distances from table TbDist are parsed in a loop:
279       - search minimum (or maximum) in the grid,
280       - calculate extremum on the surface,
281       - update table TbSel.
282 -----------------------------------------------------------------------------*/
283
284 static Standard_Boolean IsoIsDeg  (const Adaptor3d_Surface& S,
285                                    const Standard_Real      Param,
286                                    const GeomAbs_IsoType    IT,
287                                    const Standard_Real      TolMin,
288                                    const Standard_Real      TolMax) 
289 {
290     Standard_Real U1=0.,U2=0.,V1=0.,V2=0.,T;
291     Standard_Boolean Along = Standard_True;
292     U1 = S.FirstUParameter();
293     U2 = S.LastUParameter();
294     V1 = S.FirstVParameter();
295     V2 = S.LastVParameter();
296     gp_Vec D1U,D1V;
297     gp_Pnt P;
298     Standard_Real Step,D1NormMax;
299     if (IT == GeomAbs_IsoV) 
300     {
301       Step = (U2 - U1)/10;
302       D1NormMax=0.;
303       for (T=U1;T<=U2;T=T+Step) 
304       {
305         S.D1(T,Param,P,D1U,D1V);
306         D1NormMax=Max(D1NormMax,D1U.Magnitude());
307       }
308
309       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
310            Along = Standard_False;
311     }
312     else 
313     {
314       Step = (V2 - V1)/10;
315       D1NormMax=0.;
316       for (T=V1;T<=V2;T=T+Step) 
317       {
318         S.D1(Param,T,P,D1U,D1V);
319         D1NormMax=Max(D1NormMax,D1V.Magnitude());
320       }
321
322       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
323            Along = Standard_False;
324
325
326     }
327     return Along;
328 }
329 //----------------------------------------------------------
330 Extrema_GenExtPS::Extrema_GenExtPS() 
331 {
332   myDone = Standard_False;
333   myInit = Standard_False;
334   myFlag = Extrema_ExtFlag_MINMAX;
335   myAlgo = Extrema_ExtAlgo_Grad;
336 }
337
338
339 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
340                               const Adaptor3d_Surface& S,
341                               const Standard_Integer NbU, 
342                               const Standard_Integer NbV,
343                               const Standard_Real    TolU, 
344                               const Standard_Real    TolV,
345                               const Extrema_ExtFlag F,
346                               const Extrema_ExtAlgo A) 
347   : myF (P,S), myFlag(F), myAlgo(A)
348 {
349   Initialize(S, NbU, NbV, TolU, TolV);
350   Perform(P);
351 }
352
353 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
354                               const Adaptor3d_Surface& S,
355                               const Standard_Integer NbU, 
356                               const Standard_Integer NbV,
357                               const Standard_Real    Umin,
358                               const Standard_Real    Usup,
359                               const Standard_Real    Vmin,
360                               const Standard_Real    Vsup,
361                               const Standard_Real    TolU, 
362                               const Standard_Real    TolV,
363                               const Extrema_ExtFlag F,
364                               const Extrema_ExtAlgo A) 
365   : myF (P,S), myFlag(F), myAlgo(A)
366 {
367   Initialize(S, NbU, NbV, Umin, Usup, Vmin, Vsup, TolU, TolV);
368   Perform(P);
369 }
370
371
372 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
373                                   const Standard_Integer NbU, 
374                                   const Standard_Integer NbV,
375                                   const Standard_Real    TolU, 
376                                   const Standard_Real    TolV)
377 {
378   myumin = S.FirstUParameter();
379   myusup = S.LastUParameter();
380   myvmin = S.FirstVParameter();
381   myvsup = S.LastVParameter();
382   Initialize(S,NbU,NbV,myumin,myusup,myvmin,myvsup,TolU,TolV);
383 }
384
385
386 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
387                                   const Standard_Integer NbU, 
388                                   const Standard_Integer NbV,
389                                   const Standard_Real    Umin,
390                                   const Standard_Real    Usup,
391                                   const Standard_Real    Vmin,
392                                   const Standard_Real    Vsup,
393                                   const Standard_Real    TolU, 
394                                   const Standard_Real    TolV)
395 {
396   myS = (Adaptor3d_SurfacePtr)&S;
397   myusample = NbU;
398   myvsample = NbV;
399   mytolu = TolU;
400   mytolv = TolV;
401   myumin = Umin;
402   myusup = Usup;
403   myvmin = Vmin;
404   myvsup = Vsup;
405
406   if ((myusample < 2) ||
407       (myvsample < 2)) { Standard_OutOfRange::Raise(); }
408
409   myF.Initialize(S);
410
411   mySphereUBTree.Nullify();
412   myUParams.Nullify();
413   myVParams.Nullify();
414   myInit = Standard_False;
415 }
416
417 inline static void fillParams(const TColStd_Array1OfReal& theKnots,
418                               Standard_Integer theDegree,
419                               Standard_Real theParMin,
420                               Standard_Real theParMax,
421                               Handle_TColStd_HArray1OfReal& theParams,
422                               Standard_Integer theSample)
423 {
424   NCollection_Vector<Standard_Real> aParams;
425   Standard_Integer i = 1;
426   Standard_Real aPrevPar = theParMin;
427   aParams.Append(aPrevPar);
428   //calculation the array of parametric points depending on the knots array variation and degree of given surface
429   for ( ; i <  theKnots.Length() && theKnots(i) < (theParMax - Precision::PConfusion()); i++ )
430   {
431     if (  theKnots(i+1) < theParMin + Precision::PConfusion())
432       continue;
433
434     Standard_Real aStep = (theKnots(i+1) - theKnots(i))/Max(theDegree,2);
435     Standard_Integer k =1;
436     for( ; k <= theDegree ; k++)
437     {
438       Standard_Real aPar = theKnots(i) + k * aStep;
439       if(aPar > theParMax - Precision::PConfusion())
440         break;
441       if(aPar > aPrevPar + Precision::PConfusion() )
442       {
443         aParams.Append(aPar);
444         aPrevPar = aPar;
445       }
446     }
447
448   }
449   aParams.Append(theParMax);
450   Standard_Integer nbPar = aParams.Length();
451   //in case of an insufficient number of points the grid will be built later 
452   if (nbPar < theSample)
453     return;
454   theParams = new TColStd_HArray1OfReal(1, nbPar );
455   for( i = 0; i < nbPar; i++)
456     theParams->SetValue(i+1,aParams(i));
457 }
458
459 void Extrema_GenExtPS::GetGridPoints( const Adaptor3d_Surface& theSurf)
460 {
461   //creation parametric points for BSpline and Bezier surfaces
462   //with taking into account of Degree and NbKnots of BSpline or Bezier geometry
463   if(theSurf.GetType() == GeomAbs_OffsetSurface)
464     GetGridPoints(theSurf.BasisSurface()->Surface());
465   //parametric points for BSpline surfaces
466   else if( theSurf.GetType() == GeomAbs_BSplineSurface) 
467   {
468     Handle(Geom_BSplineSurface) aBspl = theSurf.BSpline();
469     if(!aBspl.IsNull())
470     {
471       TColStd_Array1OfReal aUKnots(1, aBspl->NbUKnots());
472       aBspl->UKnots( aUKnots);
473       TColStd_Array1OfReal aVKnots(1, aBspl->NbVKnots());
474       aBspl->VKnots( aVKnots);
475       fillParams(aUKnots,aBspl->UDegree(),myumin, myusup, myUParams, myusample);
476       fillParams(aVKnots,aBspl->VDegree(),myvmin, myvsup, myVParams, myvsample);
477     }
478   }
479   //calculation parametric points for Bezier surfaces
480   else if(theSurf.GetType() == GeomAbs_BezierSurface)
481   {
482     Handle(Geom_BezierSurface) aBezier = theSurf.Bezier();
483     if(aBezier.IsNull())
484       return;
485
486     TColStd_Array1OfReal aUKnots(1,2);
487     TColStd_Array1OfReal aVKnots(1,2);
488     aBezier->Bounds(aUKnots(1), aUKnots(2), aVKnots(1), aVKnots(2));
489     fillParams(aUKnots, aBezier->UDegree(), myumin, myusup, myUParams, myusample);
490     fillParams(aVKnots, aBezier->VDegree(), myvmin, myvsup, myVParams, myvsample);
491
492   }
493   //creation points for surfaces based on BSpline or Bezier curves
494   else if(theSurf.GetType() == GeomAbs_SurfaceOfRevolution || 
495     theSurf.GetType() == GeomAbs_SurfaceOfExtrusion)
496   {
497     Handle(TColStd_HArray1OfReal) anArrKnots;
498     Standard_Integer aDegree = 0;
499     GeomAbs_CurveType aType = theSurf.BasisCurve()->Curve().GetType();
500     if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BSplineCurve)
501     {
502       Handle(Geom_BSplineCurve) aBspl = theSurf.BasisCurve()->Curve().BSpline();
503       if(!aBspl.IsNull())
504       {
505         anArrKnots = new TColStd_HArray1OfReal(1,aBspl->NbKnots());
506         aBspl->Knots( anArrKnots->ChangeArray1() );
507         aDegree = aBspl->Degree();
508         
509       }
510
511     }
512     if(theSurf.BasisCurve()->Curve().GetType() == GeomAbs_BezierCurve)
513     {
514       Handle(Geom_BezierCurve) aBez = theSurf.BasisCurve()->Curve().Bezier();
515       if(!aBez.IsNull())
516       {
517         anArrKnots = new TColStd_HArray1OfReal(1,2);
518         anArrKnots->SetValue(1, aBez->FirstParameter());
519         anArrKnots->SetValue(2, aBez->LastParameter());
520         aDegree = aBez->Degree();
521         
522       }
523     }
524     if(anArrKnots.IsNull())
525       return;
526     if( theSurf.GetType() == GeomAbs_SurfaceOfRevolution )
527       fillParams( anArrKnots->Array1(), aDegree, myvmin, myvsup, myVParams, myvsample);
528     else
529       fillParams( anArrKnots->Array1(), aDegree, myumin, myusup, myUParams, myusample);
530   }
531   //update the number of points in sample
532   if(!myUParams.IsNull())
533     myusample = myUParams->Length();
534   if( !myVParams.IsNull())
535     myvsample = myVParams->Length();
536   
537 }
538
539 void Extrema_GenExtPS::BuildGrid(const gp_Pnt &thePoint)
540 {
541   Standard_Integer NoU, NoV;
542
543   //if grid was already built skip its creation
544   if (!myInit) {
545     //build parametric grid in case of a complex surface geometry (BSpline and Bezier surfaces)
546     GetGridPoints(*myS);
547     
548     //build grid in other cases 
549     if( myUParams.IsNull() )
550     {
551       Standard_Real PasU = myusup - myumin;
552       Standard_Real U0 = PasU / myusample / 100.;
553       PasU = (PasU - U0) / (myusample - 1);
554       U0 = U0/2. + myumin;
555       myUParams = new TColStd_HArray1OfReal(1,myusample );
556       Standard_Integer NoU;
557       Standard_Real U = U0;
558       for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
559         myUParams->SetValue(NoU, U);
560     }
561   
562     if( myVParams.IsNull())
563     {
564       Standard_Real PasV = myvsup - myvmin;
565       Standard_Real V0 = PasV / myvsample / 100.;
566       PasV = (PasV - V0) / (myvsample - 1);
567       V0 = V0/2. + myvmin;
568       
569       myVParams = new TColStd_HArray1OfReal(1,myvsample );
570       Standard_Integer  NoV;
571       Standard_Real V = V0;
572      
573       for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
574         myVParams->SetValue(NoV, V);
575     }
576
577     //If flag was changed and extrema not reinitialized Extrema would fail
578     myPoints = new Extrema_HArray2OfPOnSurfParams
579       (0, myusample + 1, 0, myvsample + 1);
580     // Calculation of distances
581   
582     for ( NoU = 1 ; NoU <= myusample; NoU++ ) {
583       for ( NoV = 1 ; NoV <= myvsample; NoV++) {
584         gp_Pnt aP1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
585         Extrema_POnSurfParams aParam
586           (myUParams->Value(NoU), myVParams->Value(NoV), aP1);
587
588         aParam.SetElementType(Extrema_Node);
589         aParam.SetIndices(NoU, NoV);
590         myPoints->SetValue(NoU, NoV, aParam);
591       }
592     }
593
594     // Fill boundary with negative square distance.
595     // It is used for computation of Maximum.
596     for (NoV = 0; NoV <= myvsample + 1; NoV++) {
597       myPoints->ChangeValue(0, NoV).SetSqrDistance(-1.);
598       myPoints->ChangeValue(myusample + 1, NoV).SetSqrDistance(-1.);
599     }
600
601     for (NoU = 1; NoU <= myusample; NoU++) {
602       myPoints->ChangeValue(NoU, 0).SetSqrDistance(-1.);
603       myPoints->ChangeValue(NoU, myvsample + 1).SetSqrDistance(-1.);
604     }
605     
606     myInit = Standard_True;
607   }
608
609   // Compute distances to mesh.
610   // Step 1. Compute distances to nodes.
611   for ( NoU = 1 ; NoU <= myusample; NoU++ ) {
612     for ( NoV = 1 ; NoV <= myvsample; NoV++) {
613       Extrema_POnSurfParams &aParam = myPoints->ChangeValue(NoU, NoV);
614
615       aParam.SetSqrDistance(thePoint.SquareDistance(aParam.Value()));
616     }
617   }
618
619   // For search of minimum compute distances to mesh.
620   if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX) 
621   {
622     // This is the tolerance of difference of squared values.
623     // No need to set it too small.
624     const Standard_Real aDiffTol = mytolu + mytolv;
625
626     // Step 2. Compute distances to edges.
627     // Assume UEdge(i, j) = { Point(i, j); Point(i + 1, j    ) }
628     // Assume VEdge(i, j) = { Point(i, j); Point(i,     j + 1) }
629     Handle(Extrema_HArray2OfPOnSurfParams) aUEdgePntParams =
630           new Extrema_HArray2OfPOnSurfParams(1, myusample - 1, 1, myvsample);
631     Handle(Extrema_HArray2OfPOnSurfParams) aVEdgePntParams =
632           new Extrema_HArray2OfPOnSurfParams(1, myusample, 1, myvsample - 1);
633
634     for ( NoU = 1 ; NoU <= myusample; NoU++ ) {
635       for ( NoV = 1 ; NoV <= myvsample; NoV++) {
636         const Extrema_POnSurfParams &aParam0 = myPoints->Value(NoU, NoV);
637
638         if (NoU < myusample) {
639           // Compute parameters to UEdge.
640           const Extrema_POnSurfParams &aParam1 = myPoints->Value(NoU + 1, NoV);
641           Extrema_POnSurfParams aUEdgeParam = ComputeEdgeParameters
642               (Standard_True, aParam0, aParam1, myS, thePoint, aDiffTol);
643
644           aUEdgePntParams->SetValue(NoU, NoV, aUEdgeParam);
645         }
646
647         if (NoV < myvsample) {
648           // Compute parameters to VEdge.
649           const Extrema_POnSurfParams &aParam1 = myPoints->Value(NoU, NoV + 1);
650           Extrema_POnSurfParams aVEdgeParam = ComputeEdgeParameters
651               (Standard_False, aParam0, aParam1, myS, thePoint, aDiffTol);
652
653           aVEdgePntParams->SetValue(NoU, NoV, aVEdgeParam);
654         }
655       }
656     }
657
658     // Step 3. Compute distances to faces.
659     // Assume myFacePntParams(i, j) =
660     // { Point(i, j); Point(i + 1, j); Point(i + 1, j + 1); Point(i, j + 1) }
661     //   Or
662     // { UEdge(i, j); VEdge(i + 1, j); UEdge(i, j + 1); VEdge(i, j) }
663     myFacePntParams = 
664           new Extrema_HArray2OfPOnSurfParams(0, myusample, 0, myvsample);
665     Standard_Real aSqrDist01;
666     Standard_Real aDiffDist;
667     Standard_Boolean isOut;
668
669     for ( NoU = 1 ; NoU < myusample; NoU++ ) {
670       for ( NoV = 1 ; NoV < myvsample; NoV++) {
671         const Extrema_POnSurfParams &aUE0 = aUEdgePntParams->Value(NoU, NoV);
672         const Extrema_POnSurfParams &aUE1 = aUEdgePntParams->Value(NoU, NoV+1);
673         const Extrema_POnSurfParams &aVE0 = aVEdgePntParams->Value(NoU, NoV);
674         const Extrema_POnSurfParams &aVE1 = aVEdgePntParams->Value(NoU+1, NoV);
675
676         aSqrDist01 = aUE0.Value().SquareDistance(aUE1.Value());
677         aDiffDist = Abs(aUE0.GetSqrDistance() - aUE1.GetSqrDistance());
678         isOut = Standard_False;
679
680         if (aDiffDist >= aSqrDist01 - aDiffTol) {
681           // The projection is outside the face.
682           isOut = Standard_True;
683         } else {
684           aSqrDist01 = aVE0.Value().SquareDistance(aVE1.Value());
685           aDiffDist = Abs(aVE0.GetSqrDistance() - aVE1.GetSqrDistance());
686
687           if (aDiffDist >= aSqrDist01 - aDiffTol) {
688             // The projection is outside the face.
689             isOut = Standard_True;
690           }
691         }
692
693         if (isOut) {
694           // Get the closest point on an edge.
695           const Extrema_POnSurfParams &aUEMin =
696             aUE0.GetSqrDistance() < aUE1.GetSqrDistance() ? aUE0 : aUE1;
697           const Extrema_POnSurfParams &aVEMin =
698             aVE0.GetSqrDistance() < aVE1.GetSqrDistance() ? aVE0 : aVE1;
699           const Extrema_POnSurfParams &aEMin =
700             aUEMin.GetSqrDistance() < aVEMin.GetSqrDistance() ? aUEMin : aVEMin;
701
702           myFacePntParams->SetValue(NoU, NoV, aEMin);
703         } else {
704           // Find closest point inside the face.
705           Standard_Real aU[2];
706           Standard_Real aV[2];
707           Standard_Real aUPar;
708           Standard_Real aVPar;
709
710           // Compute U parameter.
711           aUE0.Parameter(aU[0], aV[0]);
712           aUE1.Parameter(aU[1], aV[1]);
713           aUPar = 0.5*(aU[0] + aU[1]);
714           // Compute V parameter.
715           aVE0.Parameter(aU[0], aV[0]);
716           aVE1.Parameter(aU[1], aV[1]);
717           aVPar = 0.5*(aV[0] + aV[1]);
718
719           Extrema_POnSurfParams aParam(aUPar, aVPar, myS->Value(aUPar, aVPar));
720
721           aParam.SetElementType(Extrema_Face);
722           aParam.SetSqrDistance(thePoint.SquareDistance(aParam.Value()));
723           aParam.SetIndices(NoU, NoV);
724           myFacePntParams->SetValue(NoU, NoV, aParam);
725         }
726       }
727     }
728
729     // Fill boundary with RealLast square distance.
730     for (NoV = 0; NoV <= myvsample; NoV++) {
731       myFacePntParams->ChangeValue(0, NoV).SetSqrDistance(RealLast());
732       myFacePntParams->ChangeValue(myusample, NoV).SetSqrDistance(RealLast());
733     }
734
735     for (NoU = 1; NoU < myusample; NoU++) {
736       myFacePntParams->ChangeValue(NoU, 0).SetSqrDistance(RealLast());
737       myFacePntParams->ChangeValue(NoU, myvsample).SetSqrDistance(RealLast());
738     }
739   }
740 }
741
742 /*
743 a- Constitution of the table of distances (TbDist(0,myusample+1,0,myvsample+1)):
744    ---------------------------------------------------------------
745 */
746
747 // Parameterisation of the sample
748
749 void Extrema_GenExtPS::BuildTree()
750 {
751   // if tree already exists, assume it is already correctly filled
752   if ( ! mySphereUBTree.IsNull() )
753     return;
754
755   Standard_Real PasU = myusup - myumin;
756   Standard_Real PasV = myvsup - myvmin;
757   Standard_Real U0 = PasU / myusample / 100.;
758   Standard_Real V0 = PasV / myvsample / 100.;
759   gp_Pnt P1;
760   PasU = (PasU - U0) / (myusample - 1);
761   PasV = (PasV - V0) / (myvsample - 1);
762   U0 = U0/2. + myumin;
763   V0 = V0/2. + myvmin;
764
765   //build grid of parametric points 
766   myUParams = new TColStd_HArray1OfReal(1,myusample );
767   myVParams = new TColStd_HArray1OfReal(1,myvsample );
768   Standard_Integer NoU, NoV;
769   Standard_Real U = U0, V = V0;
770   for ( NoU = 1 ; NoU <= myusample; NoU++, U += PasU) 
771     myUParams->SetValue(NoU, U);
772   for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV)
773     myVParams->SetValue(NoV, V);
774
775   // Calculation of distances
776   mySphereUBTree = new Extrema_UBTreeOfSphere;
777   Extrema_UBTreeFillerOfSphere aFiller(*mySphereUBTree);
778   Standard_Integer i = 0;
779   
780   mySphereArray = new Bnd_HArray1OfSphere(0, myusample * myvsample);
781  
782   for ( NoU = 1; NoU <= myusample; NoU++ ) {
783     for ( NoV = 1; NoV <= myvsample; NoV++) {
784       P1 = myS->Value(myUParams->Value(NoU), myVParams->Value(NoV));
785       Bnd_Sphere aSph(P1.XYZ(), 0/*mytolu < mytolv ? mytolu : mytolv*/, NoU, NoV);
786       aFiller.Add(i, aSph);
787       mySphereArray->SetValue( i, aSph );
788       i++;
789     }
790   }
791   aFiller.Fill();
792 }
793
794 void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, 
795                                     const Extrema_POnSurfParams &theParams)
796 {
797   math_Vector Tol(1,2);
798   Tol(1) = mytolu;
799   Tol(2) = mytolv;
800
801   math_Vector UV(1, 2);
802
803   theParams.Parameter(UV(1), UV(2));
804
805   math_Vector UVinf(1,2), UVsup(1,2);
806   UVinf(1) = myumin;
807   UVinf(2) = myvmin;
808   UVsup(1) = myusup;
809   UVsup(2) = myvsup;
810
811   math_Vector errors(1,2);
812   math_Vector root(1, 2);
813   Standard_Real eps = 1.e-9;
814   Standard_Integer nbsubsample = 11;
815
816   Standard_Integer aNbMaxIter = 100;
817
818   gp_Pnt PStart = theParams.Value();
819   Standard_Real DistStart = theParams.GetSqrDistance();
820   Standard_Real DistSol = DistStart;
821   
822   math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
823   
824   myDone = Standard_True;
825 }
826
827 void Extrema_GenExtPS::SetFlag(const Extrema_ExtFlag F)
828 {
829   myFlag = F;
830 }
831
832 void Extrema_GenExtPS::SetAlgo(const Extrema_ExtAlgo A)
833 {
834   if(myAlgo != A)
835      myInit = Standard_False;
836   myAlgo = A;
837  
838 }
839
840 void Extrema_GenExtPS::Perform(const gp_Pnt& P) 
841 {  
842   myDone = Standard_False;
843   myF.SetPoint(P);
844   
845   if(myAlgo == Extrema_ExtAlgo_Grad)
846   {
847     BuildGrid(P);
848     Standard_Integer NoU,NoV;
849
850     if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX) 
851     {
852       Extrema_ElementType anElemType;
853       Standard_Integer iU;
854       Standard_Integer iV;
855       Standard_Integer iU2;
856       Standard_Integer iV2;
857       Standard_Boolean isMin;
858       Standard_Integer i;
859
860       for (NoU = 1; NoU < myusample; NoU++) {
861         for (NoV = 1; NoV < myvsample; NoV++) {
862           const Extrema_POnSurfParams &aParam =
863             myFacePntParams->Value(NoU, NoV);
864
865           isMin = Standard_False;
866           anElemType = aParam.GetElementType();
867
868           if (anElemType == Extrema_Face) {
869             isMin = Standard_True;
870           } else {
871             // Check if it is a boundary edge or corner vertex.
872             aParam.GetIndices(iU, iV);
873
874             if (anElemType == Extrema_UIsoEdge) {
875               isMin = (iV == 1 || iV == myvsample);
876             } else if (anElemType == Extrema_VIsoEdge) {
877               isMin = (iU == 1 || iU == myusample);
878             } else if (anElemType == Extrema_Node) {
879               isMin = (iU == 1 || iU == myusample) &&
880                       (iV == 1 || iV == myvsample);
881             }
882
883             if (!isMin) {
884               // This is a middle element.
885               if (anElemType == Extrema_UIsoEdge ||
886                 (anElemType == Extrema_Node && (iU == 1 || iU == myusample))) {
887                 // Check the down face.
888                 const Extrema_POnSurfParams &aDownParam =
889                     myFacePntParams->Value(NoU, NoV - 1);
890
891                 if (aDownParam.GetElementType() == anElemType) {
892                   aDownParam.GetIndices(iU2, iV2);
893                   isMin = (iU == iU2 && iV == iV2);
894                 }
895               } else if (anElemType == Extrema_VIsoEdge ||
896                 (anElemType == Extrema_Node && (iV == 1 || iV == myvsample))) {
897                 // Check the right face.
898                 const Extrema_POnSurfParams &aRightParam =
899                     myFacePntParams->Value(NoU - 1, NoV);
900
901                 if (aRightParam.GetElementType() == anElemType) {
902                   aRightParam.GetIndices(iU2, iV2);
903                   isMin = (iU == iU2 && iV == iV2);
904                 }
905               } else if (iU == NoU && iV == NoV) {
906                 // Check the lower-left node. For this purpose it is necessary
907                 // to check lower-left, lower and left faces.
908                 isMin = Standard_True;
909
910                 const Extrema_POnSurfParams *anOtherParam[3] =
911                 { &myFacePntParams->Value(NoU, NoV - 1),     // Down
912                   &myFacePntParams->Value(NoU - 1, NoV - 1), // Lower-left
913                   &myFacePntParams->Value(NoU - 1, NoV) };   // Left
914
915                 for (i = 0; i < 3 && isMin; i++) {
916                   if (anOtherParam[i]->GetElementType() == Extrema_Node) {
917                     anOtherParam[i]->GetIndices(iU2, iV2);
918                     isMin = (iU == iU2 && iV == iV2);
919                   } else {
920                     isMin = Standard_False;
921                   }
922                 }
923               }
924             }
925           }
926
927           if (isMin) {
928             FindSolution(P, aParam);
929           }
930         }
931       }
932     }
933     
934     if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
935     {
936       Standard_Real Dist;
937
938       for (NoU = 1; NoU <= myusample; NoU++) {
939         for (NoV = 1; NoV <= myvsample; NoV++) {
940           Dist = myPoints->Value(NoU, NoV).GetSqrDistance();
941           if ((myPoints->Value(NoU-1,NoV-1).GetSqrDistance() <= Dist) &&
942               (myPoints->Value(NoU-1,NoV  ).GetSqrDistance() <= Dist) &&
943               (myPoints->Value(NoU-1,NoV+1).GetSqrDistance() <= Dist) &&
944               (myPoints->Value(NoU  ,NoV-1).GetSqrDistance() <= Dist) &&
945               (myPoints->Value(NoU  ,NoV+1).GetSqrDistance() <= Dist) &&
946               (myPoints->Value(NoU+1,NoV-1).GetSqrDistance() <= Dist) &&
947               (myPoints->Value(NoU+1,NoV  ).GetSqrDistance() <= Dist) &&
948               (myPoints->Value(NoU+1,NoV+1).GetSqrDistance() <= Dist)) {
949             // Find maximum.
950             FindSolution(P, myPoints->Value(NoU, NoV));
951           }
952         }
953       }
954     }
955   }
956   else
957   {
958     BuildTree();
959     if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX)
960     {
961       Bnd_Sphere aSol = mySphereArray->Value(0);
962       Bnd_SphereUBTreeSelectorMin aSelector(mySphereArray, aSol);
963       //aSelector.SetMaxDist( RealLast() );
964       aSelector.DefineCheckPoint( P );
965       Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
966       //TODO: check if no solution in binary tree
967       Bnd_Sphere& aSph = aSelector.Sphere();
968       Standard_Real aU = myUParams->Value(aSph.U());
969       Standard_Real aV = myVParams->Value(aSph.V());
970       Extrema_POnSurfParams aParams(aU, aV, myS->Value(aU, aV));
971
972       aParams.SetSqrDistance(P.SquareDistance(aParams.Value()));
973       aParams.SetIndices(aSph.U(), aSph.V());
974       FindSolution(P, aParams);
975     }
976     if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
977     {
978       Bnd_Sphere aSol = mySphereArray->Value(0);
979       Bnd_SphereUBTreeSelectorMax aSelector(mySphereArray, aSol);
980       //aSelector.SetMaxDist( RealLast() );
981       aSelector.DefineCheckPoint( P );
982       Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
983       //TODO: check if no solution in binary tree
984       Bnd_Sphere& aSph = aSelector.Sphere();
985       Standard_Real aU = myUParams->Value(aSph.U());
986       Standard_Real aV = myVParams->Value(aSph.V());
987       Extrema_POnSurfParams aParams(aU, aV, myS->Value(aU, aV));
988
989       aParams.SetSqrDistance(P.SquareDistance(aParams.Value()));
990       aParams.SetIndices(aSph.U(), aSph.V());
991
992       FindSolution(P, aParams);
993     }
994   }
995 }
996 //=============================================================================
997
998 Standard_Boolean Extrema_GenExtPS::IsDone () const { return myDone; }
999 //=============================================================================
1000
1001 Standard_Integer Extrema_GenExtPS::NbExt () const
1002 {
1003   if (!IsDone()) { StdFail_NotDone::Raise(); }
1004   return myF.NbExt();
1005 }
1006 //=============================================================================
1007
1008 Standard_Real Extrema_GenExtPS::SquareDistance (const Standard_Integer N) const
1009 {
1010   if (!IsDone()) { StdFail_NotDone::Raise(); }
1011   return myF.SquareDistance(N);
1012 }
1013 //=============================================================================
1014
1015 Extrema_POnSurf Extrema_GenExtPS::Point (const Standard_Integer N) const
1016 {
1017   if (!IsDone()) { StdFail_NotDone::Raise(); }
1018   return myF.Point(N);
1019 }
1020 //=============================================================================