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