06d3f1fa1c01b12bb1f7966ba49a6d41312ccccb
[occt.git] / src / Extrema / Extrema_GenExtPS.cxx
1 // File:        Extrema_GenExtPS.cxx
2 // Created:     Tue Jul 18 08:21:34 1995
3 // Author:      Modelistation
4 //              <model@metrox>
5
6 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593
7
8
9 #include <Extrema_GenExtPS.ixx>
10 #include <StdFail_NotDone.hxx>
11 #include <Standard_OutOfRange.hxx>
12 #include <TColStd_Array2OfInteger.hxx>
13 #include <TColStd_Array2OfReal.hxx>
14 #include <TColgp_Array2OfPnt.hxx>
15 #include <math_FunctionSetRoot.hxx>
16 #include <math_Vector.hxx>
17 #include <math_NewtonFunctionSetRoot.hxx>
18 #include <GeomAbs_IsoType.hxx>
19
20
21
22
23 //=============================================================================
24
25 /*-----------------------------------------------------------------------------
26 Fonction:
27    Recherche de toutes les distances extremales entre le point P et la surface
28   S a partir d'un echantillonnage (NbU,NbV).
29
30 Methode:
31    L'algorithme part de l'hypothese que l'echantillonnage est suffisamment fin
32   pour que, s'il existe N distances extremales entre le point et la surface,
33   alors il existe aussi N extrema entre le point et la grille.
34   Ainsi, l'algorithme consiste a partir des extrema de la grille pour trouver
35   les extrema de la surface.
36    Les extrema sont calcules par l'algorithme math_FunctionSetRoot avec les
37   arguments suivants:
38   - F: Extrema_FuncExtPS cree a partir de P et S,
39   - UV: math_Vector dont les composantes sont les parametres de l'extremum sur
40     la grille,
41   - Tol: Min(TolU,TolV), (Prov.:math_FunctionSetRoot n'autorise pas un vecteur)
42   - UVinf: math_Vector dont les composantes sont les bornes inferieures de u et
43     v,
44   - UVsup: math_Vector dont les composantes sont les bornes superieures de u et
45     v.
46
47 Traitement:
48   a- Constitution du tableau des distances (TbDist(0,NbU+1,0,NbV+1)):
49       Le tableau est volontairement etendu; les lignes 0 et NbU+1 et les
50      colonnes 0 et NbV+1 seront initialisees a RealFirst() ou RealLast()
51      pour simplifier les tests effectues dans l'etape b
52      (on n'a pas besoin de tester si le point est sur un bord de la grille).
53   b- Calcul des extrema:
54       On recherche d'abord les minima et ensuite les maxima. Ces 2 traitements
55      se passent de facon similaire:
56   b.a- Initialisations:
57       - des 'bords' du tableau TbDist (a RealLast() dans le cas des minima
58         et a RealLast() dans le cas des maxima),
59       - du tableau TbSel(0,NbU+1,0,NbV+1) de selection des points pour un
60         calcul d'extremum local (a 0). Lorsqu'un point sera selectionne,
61         il ne sera plus selectionnable, ainsi que ses points adjacents
62         (8 au maximum). Les adresses correspondantes seront mises a 1.
63   b.b- Calcul des minima (ou maxima):
64        On boucle sur toutes les distances du tableau TbDist:
65       - recherche d'un minimum (ou maximum) sur la grille,
66       - calcul de l'extremum sur la surface,
67       - mise a jour du tableau TbSel.
68 -----------------------------------------------------------------------------*/
69
70 static Standard_Boolean IsoIsDeg  (const Adaptor3d_Surface& S,
71                                    const Standard_Real      Param,
72                                    const GeomAbs_IsoType    IT,
73                                    const Standard_Real      TolMin,
74                                    const Standard_Real      TolMax) 
75 {
76     Standard_Real U1=0.,U2=0.,V1=0.,V2=0.,T;
77     Standard_Boolean Along = Standard_True;
78     U1 = S.FirstUParameter();
79     U2 = S.LastUParameter();
80     V1 = S.FirstVParameter();
81     V2 = S.LastVParameter();
82     gp_Vec D1U,D1V;
83     gp_Pnt P;
84     Standard_Real Step,D1NormMax;
85     if (IT == GeomAbs_IsoV) 
86     {
87       Step = (U2 - U1)/10;
88       D1NormMax=0.;
89       for (T=U1;T<=U2;T=T+Step) 
90       {
91         S.D1(T,Param,P,D1U,D1V);
92         D1NormMax=Max(D1NormMax,D1U.Magnitude());
93       }
94
95       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
96            Along = Standard_False;
97     }
98     else 
99     {
100       Step = (V2 - V1)/10;
101       D1NormMax=0.;
102       for (T=V1;T<=V2;T=T+Step) 
103       {
104         S.D1(Param,T,P,D1U,D1V);
105         D1NormMax=Max(D1NormMax,D1V.Magnitude());
106       }
107
108       if (D1NormMax >TolMax || D1NormMax < TolMin ) 
109            Along = Standard_False;
110
111
112     }
113     return Along;
114 }
115 //----------------------------------------------------------
116 Extrema_GenExtPS::Extrema_GenExtPS() 
117 {
118   myDone = Standard_False;
119   myInit = Standard_False;
120 }
121
122
123 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
124                               const Adaptor3d_Surface& S,
125                               const Standard_Integer NbU, 
126                               const Standard_Integer NbV,
127                               const Standard_Real    TolU, 
128                               const Standard_Real    TolV) 
129 : myF (P,S)
130 {
131   Initialize(S, NbU, NbV, TolU, TolV);
132   Perform(P);
133 }
134
135 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt&          P,
136                               const Adaptor3d_Surface& S,
137                               const Standard_Integer NbU, 
138                               const Standard_Integer NbV,
139                               const Standard_Real    Umin,
140                               const Standard_Real    Usup,
141                               const Standard_Real    Vmin,
142                               const Standard_Real    Vsup,
143                               const Standard_Real    TolU, 
144                               const Standard_Real    TolV) 
145 : myF (P,S)
146 {
147   Initialize(S, NbU, NbV, Umin, Usup, Vmin, Vsup, TolU, TolV);
148   Perform(P);
149 }
150
151
152 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
153                                   const Standard_Integer NbU, 
154                                   const Standard_Integer NbV,
155                                   const Standard_Real    TolU, 
156                                   const Standard_Real    TolV)
157 {
158   myumin = S.FirstUParameter();
159   myusup = S.LastUParameter();
160   myvmin = S.FirstVParameter();
161   myvsup = S.LastVParameter();
162   Initialize(S,NbU,NbV,myumin,myusup,myvmin,myvsup,TolU,TolV);
163 }
164
165
166 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
167                                   const Standard_Integer NbU, 
168                                   const Standard_Integer NbV,
169                                   const Standard_Real    Umin,
170                                   const Standard_Real    Usup,
171                                   const Standard_Real    Vmin,
172                                   const Standard_Real    Vsup,
173                                   const Standard_Real    TolU, 
174                                   const Standard_Real    TolV)
175 {
176   myInit = Standard_True;
177   myS = (Adaptor3d_SurfacePtr)&S;
178   myusample = NbU;
179   myvsample = NbV;
180   mytolu = TolU;
181   mytolv = TolV;
182   myumin = Umin;
183   myusup = Usup;
184   myvmin = Vmin;
185   myvsup = Vsup;
186
187   if ((myusample < 2) ||
188       (myvsample < 2)) { Standard_OutOfRange::Raise(); }
189
190   myF.Initialize(S);
191
192   mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
193
194 /*
195 a- Constitution du tableau des distances (TbDist(0,myusample+1,0,myvsample+1)):
196    ---------------------------------------------------------------
197 */
198
199 // Parametrage de l echantillon
200
201   Standard_Real PasU = myusup - myumin;
202   Standard_Real PasV = myvsup - myvmin;
203   Standard_Real U0 = PasU / myusample / 100.;
204   Standard_Real V0 = PasV / myvsample / 100.;
205   gp_Pnt P1;
206   PasU = (PasU - U0) / (myusample - 1);
207   PasV = (PasV - V0) / (myvsample - 1);
208   U0 = U0/2. + myumin;
209   V0 = V0/2. + myvmin;
210
211 // Calcul des distances
212
213   Standard_Integer NoU, NoV;
214   Standard_Real U, V;
215   for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
216     for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
217       P1 = myS->Value(U, V);
218       mypoints->SetValue(NoU,NoV,P1);
219     }
220   }
221 }
222
223
224
225 void Extrema_GenExtPS::Perform(const gp_Pnt& P) 
226 {  
227   myDone = Standard_False;
228   myF.SetPoint(P);
229
230
231 /*
232 b- Calcul des minima:
233    -----------------
234    b.a) Initialisations:
235 */
236
237 // Parametrage de l echantillon
238
239   Standard_Real PasU = myusup - myumin;
240   Standard_Real PasV = myvsup - myvmin;
241   Standard_Real U, U0 = PasU / myusample / 100.;
242   Standard_Real V, V0 = PasV / myvsample / 100.;
243   PasU = (PasU - U0) / (myusample - 1);
244   PasV = (PasV - V0) / (myvsample - 1);
245   U0 = U0/2.+myumin;
246   V0 = V0/2.+myvmin;
247
248 //     - generales
249   math_Vector Tol(1, 2);
250   Tol(1) = mytolu;
251   Tol(2) = mytolv;
252   math_Vector UV(1,2), UVinf(1,2), UVsup(1,2);
253   UVinf(1) = myumin;
254   UVinf(2) = myvmin;
255   UVsup(1) = myusup;
256   UVsup(2) = myvsup;
257
258 //     - des 'bords' du tableau mytbdist
259   Standard_Integer NoU,NoV;
260
261   TColStd_Array2OfReal TheDist(0, myusample+1, 0, myvsample+1);
262   for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
263     for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
264       TheDist(NoU, NoV) = P.SquareDistance(mypoints->Value(NoU, NoV));
265     }
266   }
267   
268   for (NoV = 0; NoV <= myvsample+1; NoV++) {
269     TheDist(0,NoV) = RealLast();
270     TheDist(myusample+1,NoV) = RealLast();
271   }
272   for (NoU = 1; NoU <= myusample; NoU++) {
273     TheDist(NoU,0) = RealLast();
274     TheDist(NoU,myvsample+1) = RealLast();
275   }
276
277 //     - du tableau TbSel(0,myusample+1,0,myvsample+1) de selection des points
278   TColStd_Array2OfInteger TbSel(0,myusample+1,0,myvsample+1);
279   TbSel.Init(0);
280 /*
281    b.b) Calcul des minima:
282 */
283 //     - recherche d un minimum sur la grille
284   math_Vector errors(1,2);
285   math_Vector root(1, 2);
286   Standard_Real eps = 1.e-9;
287   Standard_Integer nbsubsample = 11;
288   Standard_Integer Nu, Nv;
289   Standard_Real Dist;
290   for (NoU = 1; NoU <= myusample; NoU++) {
291     for (NoV = 1; NoV <= myvsample; NoV++) {
292       if (TbSel(NoU,NoV) == 0) {
293         Dist = TheDist(NoU,NoV);
294         if ((TheDist(NoU-1,NoV-1) >= Dist) &&
295             (TheDist(NoU-1,NoV  ) >= Dist) &&
296             (TheDist(NoU-1,NoV+1) >= Dist) &&
297             (TheDist(NoU  ,NoV-1) >= Dist) &&
298             (TheDist(NoU  ,NoV+1) >= Dist) &&
299             (TheDist(NoU+1,NoV-1) >= Dist) &&
300             (TheDist(NoU+1,NoV  ) >= Dist) &&
301             (TheDist(NoU+1,NoV+1) >= Dist)) {
302
303 //     - calcul de l extremum sur la surface:
304           UV(1) = U0 + (NoU - 1) * PasU;
305           UV(2) = V0 + (NoV - 1) * PasV;
306 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593 Begin
307 //        math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup);
308            // Default value for math_FunctionSetRoot.
309           Standard_Integer aNbMaxIter = 100;
310
311           if (myF.HasDegIso())
312             aNbMaxIter = 150;
313
314           math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
315           if(S.IsDone()) {
316             root = S.Root();
317             myF.Value(root, errors);
318             if(Abs(errors(1)) > eps || Abs(errors(2)) > eps) {
319               //try to improve solution on subgrid of sample points
320               gp_Pnt PSol = myS->Value(root(1), root(2));
321               Standard_Real DistSol = P.SquareDistance(PSol);
322
323               Standard_Real u1 = Max(UV(1) - PasU, myumin), u2 = Min(UV(1) + PasU, myusup);
324               Standard_Real v1 = Max(UV(2) - PasV, myvmin), v2 = Min(UV(2) + PasV, myvsup);
325               
326               if(u2 - u1 < 2.*PasU) {
327                 if(Abs(u1 - myumin) < 1.e-9) {
328                   u2 = u1 + 2.*PasU;
329                   u2 = Min(u2, myusup);
330                 }
331                 if(Abs(u2 - myusup) < 1.e-9) {
332                   u1 = u2 - 2.*PasU;
333                   u1 = Max(u1, myumin);
334                 }
335               }
336
337               if(v2 - v1 < 2.*PasV) {
338                 if(Abs(v1 - myvmin) < 1.e-9) {
339                   v2 = v1 + 2.*PasV;
340                   v2 = Min(v2, myvsup);
341                 }
342                 if(Abs(v2 - myvsup) < 1.e-9) {
343                   v1 = v2 - 2.*PasV;
344                   v1 = Max(v1, myvmin);
345                 }
346               }
347
348               Standard_Real du = (u2 - u1)/(nbsubsample-1);
349               Standard_Real dv = (v2 - v1)/(nbsubsample-1);
350               Standard_Real u, v;
351               Standard_Real dist;
352
353               Standard_Boolean NewSolution = Standard_False;
354               for (Nu = 1, u = u1; Nu < nbsubsample; Nu++, u += du) {
355                 for (Nv = 1, v = v1; Nv < nbsubsample; Nv++, v += dv) {
356                   gp_Pnt Puv = myS->Value(u, v);
357                   dist = P.SquareDistance(Puv);
358                   
359                   if(dist < DistSol) {
360                     UV(1) = u;
361                     UV(2) = v;
362                     NewSolution = Standard_True;
363                     DistSol = dist;
364                   }
365                 }
366               }
367               if(NewSolution) {
368                 //try to precise
369                 math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
370               }
371
372             }
373           }
374 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593 End
375       
376 //     - mise a jour du tableau TbSel     
377           for (Nu = NoU-1; Nu <= NoU+1; Nu++) {
378             for (Nv = NoV-1; Nv <= NoV+1; Nv++) {
379               TbSel(Nu,Nv) = 1;
380             }
381           }
382         }
383       } // if (TbSel(NoU,NoV)
384     } // for (NoV = 1; ...
385   } // for (NoU = 1; ...
386 /*
387 c- Calcul des maxima:
388    -----------------
389    c.a) Initialisations:
390 */
391 //     - des 'bords' du tableau mytbdist
392   for (NoV = 0; NoV <= myvsample+1; NoV++) {
393     TheDist(0,NoV) = RealFirst();
394     TheDist(myusample+1,NoV) = RealFirst();
395   }
396   for (NoU = 1; NoU <= myusample; NoU++) {
397     TheDist(NoU,0) = RealFirst();
398     TheDist(NoU,myvsample+1) = RealFirst();
399   }
400
401 //     - du tableau TbSel(0,myusample+1,0,myvsample+1) de selection des points
402   for (NoU = 0; NoU <= myusample+1; NoU++) {
403     for (NoV = 0; NoV <= myvsample+1; NoV++) {
404       TbSel(NoU,NoV) = 0;
405     }
406   }
407 /*
408    c.b) Calcul des maxima:
409 */
410 //     - recherche d un maximum sur la grille
411   for (NoU = 1; NoU <= myusample; NoU++) {
412     for (NoV = 1; NoV <= myvsample; NoV++) {
413       if (TbSel(NoU,NoV) == 0) {
414         Dist = TheDist(NoU,NoV);
415         if ((TheDist(NoU-1,NoV-1) <= Dist) &&
416             (TheDist(NoU-1,NoV  ) <= Dist) &&
417             (TheDist(NoU-1,NoV+1) <= Dist) &&
418             (TheDist(NoU  ,NoV-1) <= Dist) &&
419             (TheDist(NoU  ,NoV+1) <= Dist) &&
420             (TheDist(NoU+1,NoV-1) <= Dist) &&
421             (TheDist(NoU+1,NoV  ) <= Dist) &&
422             (TheDist(NoU+1,NoV+1) <= Dist)) {
423
424 //     - calcul de l extremum sur la surface:
425           UV(1) = U0 + (NoU - 1) * PasU;
426           UV(2) = V0 + (NoV - 1) * PasV;
427 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593 Begin
428 //        math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup);
429            // Default value for math_FunctionSetRoot.
430           Standard_Integer aNbMaxIter = 100;
431
432           if (myF.HasDegIso())
433             aNbMaxIter = 150;
434
435           math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
436           if(S.IsDone()) {
437             root = S.Root();
438             myF.Value(root, errors);
439           }
440 //  Modified by skv - Thu Sep 30 15:21:07 2004 OCC593 End
441       
442 //     - mise a jour du tableau TbSel     
443           for (Nu = NoU-1; Nu <= NoU+1; Nu++) {
444             for (Nv = NoV-1; Nv <= NoV+1; Nv++) {
445               TbSel(Nu,Nv) = 1;
446             }
447           }
448         }
449       } // if (TbSel(NoU,NoV))
450     } // for (NoV = 1; ...)
451   } // for (NoU = 1; ...)
452   myDone = Standard_True;
453 }
454 //=============================================================================
455
456 Standard_Boolean Extrema_GenExtPS::IsDone () const { return myDone; }
457 //=============================================================================
458
459 Standard_Integer Extrema_GenExtPS::NbExt () const
460 {
461   if (!IsDone()) { StdFail_NotDone::Raise(); }
462   return myF.NbExt();
463 }
464 //=============================================================================
465
466 Standard_Real Extrema_GenExtPS::SquareDistance (const Standard_Integer N) const
467 {
468   if (!IsDone()) { StdFail_NotDone::Raise(); }
469   return myF.SquareDistance(N);
470 }
471 //=============================================================================
472
473 Extrema_POnSurf Extrema_GenExtPS::Point (const Standard_Integer N) const
474 {
475   if (!IsDone()) { StdFail_NotDone::Raise(); }
476   return myF.Point(N);
477 }
478 //=============================================================================
479