1 // File: Extrema_GenExtPS.cxx
2 // Created: Tue Jul 18 08:21:34 1995
3 // Author: Modelistation
6 // Modified by skv - Thu Sep 30 15:21:07 2004 OCC593
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 #include <Bnd_Sphere.hxx>
20 #include <Extrema_HUBTreeOfSphere.hxx>
21 #include <Extrema_ExtFlag.hxx>
22 #include <Bnd_Array1OfSphere.hxx>
23 #include <Bnd_HArray1OfSphere.hxx>
25 //IMPLEMENT_HARRAY1(Extrema_HArray1OfSphere)
28 class Bnd_SphereUBTreeSelector : public Extrema_UBTreeOfSphere::Selector
32 Bnd_SphereUBTreeSelector (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
35 mySphereArray(theSphereArray),
38 //myXYZ = gp_Pnt(0, 0, 0);
41 void DefineCheckPoint( const gp_Pnt& theXYZ )
44 Bnd_Sphere& Sphere() const
47 virtual Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const = 0;
49 virtual Standard_Boolean Accept(const Standard_Integer& theObj) = 0;
53 const Handle(Bnd_HArray1OfSphere)& mySphereArray;
58 class Bnd_SphereUBTreeSelectorMin : public Bnd_SphereUBTreeSelector
61 Bnd_SphereUBTreeSelectorMin (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
63 : Bnd_SphereUBTreeSelector(theSphereArray, theSol),
67 void SetMinDist( const Standard_Real theMinDist )
68 { myMinDist = theMinDist; }
70 Standard_Real MinDist() const
73 Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const
75 Bnd_SphereUBTreeSelectorMin* me =
76 const_cast<Bnd_SphereUBTreeSelectorMin*>(this);
77 // myMinDist is decreased each time a nearer object is found
78 return theBnd.IsOut( myXYZ.XYZ(), me->myMinDist );
81 Standard_Boolean Accept(const Standard_Integer&);
84 Standard_Real myMinDist;
87 Standard_Boolean Bnd_SphereUBTreeSelectorMin::Accept(const Standard_Integer& theInd)
89 const Bnd_Sphere& aSph = mySphereArray->Value(theInd);
90 Standard_Real aCurDist;
92 if ( (aCurDist = aSph.SquareDistance(myXYZ.XYZ())) < mySol.SquareDistance(myXYZ.XYZ()) )
95 if ( aCurDist < myMinDist )
101 return Standard_False;
104 class Bnd_SphereUBTreeSelectorMax : public Bnd_SphereUBTreeSelector
107 Bnd_SphereUBTreeSelectorMax (const Handle(Bnd_HArray1OfSphere)& theSphereArray,
109 : Bnd_SphereUBTreeSelector(theSphereArray, theSol),
113 void SetMaxDist( const Standard_Real theMaxDist )
114 { myMaxDist = theMaxDist; }
116 Standard_Real MaxDist() const
117 { return myMaxDist; }
119 Standard_Boolean Reject( const Bnd_Sphere &theBnd ) const
121 Bnd_SphereUBTreeSelectorMax* me =
122 const_cast<Bnd_SphereUBTreeSelectorMax*>(this);
123 // myMaxDist is decreased each time a nearer object is found
124 return theBnd.IsOut( myXYZ.XYZ(), me->myMaxDist );
127 Standard_Boolean Accept(const Standard_Integer&);
130 Standard_Real myMaxDist;
133 Standard_Boolean Bnd_SphereUBTreeSelectorMax::Accept(const Standard_Integer& theInd)
135 const Bnd_Sphere& aSph = mySphereArray->Value(theInd);
136 Standard_Real aCurDist;
138 if ( (aCurDist = aSph.SquareDistance(myXYZ.XYZ())) > mySol.SquareDistance(myXYZ.XYZ()) )
141 if ( aCurDist > myMaxDist )
142 myMaxDist = aCurDist;
144 return Standard_True;
147 return Standard_False;
152 //=============================================================================
154 /*-----------------------------------------------------------------------------
156 Find all extremum distances between point P and surface
157 S using sampling (NbU,NbV).
160 The algorithm bases on the hypothesis that sampling is precise enough,
161 if there exist N extreme distances between the point and the surface,
162 so there also exist N extrema between the point and the grid.
163 So, the algorithm consists in starting from extrema of the grid to find the
164 extrema of the surface.
165 The extrema are calculated by the algorithm math_FunctionSetRoot with the
167 - F: Extrema_FuncExtPS created from P and S,
168 - UV: math_Vector the components which of are parameters of the extremum on the
170 - Tol: Min(TolU,TolV), (Prov.:math_FunctionSetRoot does not autorize a vector)
171 - UVinf: math_Vector the components which of are lower limits of u and v,
172 - UVsup: math_Vector the components which of are upper limits of u and v.
175 a- Creation of the table of distances (TbDist(0,NbU+1,0,NbV+1)):
176 The table is expanded at will; lines 0 and NbU+1 and
177 columns 0 and NbV+1 are initialized at RealFirst() or RealLast()
178 to simplify the tests carried out at stage b
179 (there is no need to test if the point is on border of the grid).
180 b- Calculation of extrema:
181 First the minimums and then the maximums are found. These 2 procedured
182 pass in a similar way:
184 - 'borders' of table TbDist (RealLast() in case of minimums
185 and RealLast() in case of maximums),
186 - table TbSel(0,NbU+1,0,NbV+1) of selection of points for
187 calculation of local extremum (0). When a point will selected,
188 it will not be selectable, as well as the ajacent points
189 (8 at least). The corresponding addresses will be set to 1.
190 b.b- Calculation of minimums (or maximums):
191 All distances from table TbDist are parsed in a loop:
192 - search minimum (or maximum) in the grid,
193 - calculate extremum on the surface,
194 - update table TbSel.
195 -----------------------------------------------------------------------------*/
197 static Standard_Boolean IsoIsDeg (const Adaptor3d_Surface& S,
198 const Standard_Real Param,
199 const GeomAbs_IsoType IT,
200 const Standard_Real TolMin,
201 const Standard_Real TolMax)
203 Standard_Real U1=0.,U2=0.,V1=0.,V2=0.,T;
204 Standard_Boolean Along = Standard_True;
205 U1 = S.FirstUParameter();
206 U2 = S.LastUParameter();
207 V1 = S.FirstVParameter();
208 V2 = S.LastVParameter();
211 Standard_Real Step,D1NormMax;
212 if (IT == GeomAbs_IsoV)
216 for (T=U1;T<=U2;T=T+Step)
218 S.D1(T,Param,P,D1U,D1V);
219 D1NormMax=Max(D1NormMax,D1U.Magnitude());
222 if (D1NormMax >TolMax || D1NormMax < TolMin )
223 Along = Standard_False;
229 for (T=V1;T<=V2;T=T+Step)
231 S.D1(Param,T,P,D1U,D1V);
232 D1NormMax=Max(D1NormMax,D1V.Magnitude());
235 if (D1NormMax >TolMax || D1NormMax < TolMin )
236 Along = Standard_False;
242 //----------------------------------------------------------
243 Extrema_GenExtPS::Extrema_GenExtPS()
245 myDone = Standard_False;
246 myInit = Standard_False;
247 myFlag = Extrema_ExtFlag_MINMAX;
248 myAlgo = Extrema_ExtAlgo_Grad;
252 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt& P,
253 const Adaptor3d_Surface& S,
254 const Standard_Integer NbU,
255 const Standard_Integer NbV,
256 const Standard_Real TolU,
257 const Standard_Real TolV,
258 const Extrema_ExtFlag F,
259 const Extrema_ExtAlgo A)
260 : myF (P,S), myFlag(F), myAlgo(A)
262 Initialize(S, NbU, NbV, TolU, TolV);
266 Extrema_GenExtPS::Extrema_GenExtPS (const gp_Pnt& P,
267 const Adaptor3d_Surface& S,
268 const Standard_Integer NbU,
269 const Standard_Integer NbV,
270 const Standard_Real Umin,
271 const Standard_Real Usup,
272 const Standard_Real Vmin,
273 const Standard_Real Vsup,
274 const Standard_Real TolU,
275 const Standard_Real TolV,
276 const Extrema_ExtFlag F,
277 const Extrema_ExtAlgo A)
278 : myF (P,S), myFlag(F), myAlgo(A)
280 Initialize(S, NbU, NbV, Umin, Usup, Vmin, Vsup, TolU, TolV);
285 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
286 const Standard_Integer NbU,
287 const Standard_Integer NbV,
288 const Standard_Real TolU,
289 const Standard_Real TolV)
291 myumin = S.FirstUParameter();
292 myusup = S.LastUParameter();
293 myvmin = S.FirstVParameter();
294 myvsup = S.LastVParameter();
295 Initialize(S,NbU,NbV,myumin,myusup,myvmin,myvsup,TolU,TolV);
299 void Extrema_GenExtPS::Initialize(const Adaptor3d_Surface& S,
300 const Standard_Integer NbU,
301 const Standard_Integer NbV,
302 const Standard_Real Umin,
303 const Standard_Real Usup,
304 const Standard_Real Vmin,
305 const Standard_Real Vsup,
306 const Standard_Real TolU,
307 const Standard_Real TolV)
309 myInit = Standard_True;
310 myS = (Adaptor3d_SurfacePtr)&S;
320 if ((myusample < 2) ||
321 (myvsample < 2)) { Standard_OutOfRange::Raise(); }
325 mySphereUBTree.Nullify();
327 if(myAlgo == Extrema_ExtAlgo_Grad)
329 //If flag was changed and extrema not reinitialized Extrema would fail
330 mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
331 Standard_Real PasU = myusup - myumin;
332 Standard_Real PasV = myvsup - myvmin;
333 Standard_Real U0 = PasU / myusample / 100.;
334 Standard_Real V0 = PasV / myvsample / 100.;
336 PasU = (PasU - U0) / (myusample - 1);
337 PasV = (PasV - V0) / (myvsample - 1);
341 // Calculation of distances
343 Standard_Integer NoU, NoV;
345 for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
346 for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
347 P1 = myS->Value(U, V);
348 mypoints->SetValue(NoU,NoV,P1);
353 //mypoints = new TColgp_HArray2OfPnt(0,myusample+1,0,myvsample+1);
356 a- Constitution of the table of distances (TbDist(0,myusample+1,0,myvsample+1)):
357 ---------------------------------------------------------------
360 // Parameterisation of the sample
365 void Extrema_GenExtPS::BuildTree()
367 // if tree already exists, assume it is already correctly filled
368 if ( ! mySphereUBTree.IsNull() )
371 Standard_Real PasU = myusup - myumin;
372 Standard_Real PasV = myvsup - myvmin;
373 Standard_Real U0 = PasU / myusample / 100.;
374 Standard_Real V0 = PasV / myvsample / 100.;
376 PasU = (PasU - U0) / (myusample - 1);
377 PasV = (PasV - V0) / (myvsample - 1);
381 // Calculation of distances
382 mySphereUBTree = new Extrema_UBTreeOfSphere;
383 Extrema_UBTreeFillerOfSphere aFiller(*mySphereUBTree);
384 Standard_Integer i = 0;
386 mySphereArray = new Bnd_HArray1OfSphere(0, myusample * myvsample);
387 Standard_Integer NoU, NoV;
388 for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
389 for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
390 P1 = myS->Value(U, V);
391 Bnd_Sphere aSph(P1.XYZ(), 0/*mytolu < mytolv ? mytolu : mytolv*/, NoU, NoV);
392 aFiller.Add(i, aSph);
393 mySphereArray->SetValue( i, aSph );
400 void Extrema_GenExtPS::FindSolution(const gp_Pnt& P, const math_Vector& UV, const Standard_Real PasU, const Standard_Real PasV, const Extrema_ExtFlag f)
402 math_Vector Tol(1,2);
406 math_Vector UVinf(1,2), UVsup(1,2);
412 math_Vector errors(1,2);
413 math_Vector root(1, 2);
414 Standard_Real eps = 1.e-9;
415 Standard_Integer nbsubsample = 11;
417 Standard_Integer aNbMaxIter = 100;
419 gp_Pnt PStart = myS->Value(UV(1), UV(2));
420 Standard_Real DistStart = P.SquareDistance(PStart);
421 Standard_Real DistSol = DistStart;
423 math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
424 Standard_Boolean ToResolveOnSubgrid = Standard_False;
425 if (f == Extrema_ExtFlag_MIN)
430 myF.Value(root, errors);
431 gp_Pnt PSol = myS->Value(root(1), root(2));
432 DistSol = P.SquareDistance(PSol);
433 if(Abs(errors(1)) > eps || Abs(errors(2)) > eps || DistStart < DistSol)
434 //try to improve solution on subgrid of sample points
435 ToResolveOnSubgrid = Standard_True;
438 ToResolveOnSubgrid = Standard_True;
440 if (ToResolveOnSubgrid)
442 Standard_Real u1 = Max(UV(1) - PasU, myumin), u2 = Min(UV(1) + PasU, myusup);
443 Standard_Real v1 = Max(UV(2) - PasV, myvmin), v2 = Min(UV(2) + PasV, myvsup);
445 if(u2 - u1 < 2.*PasU) {
446 if(Abs(u1 - myumin) < 1.e-9) {
448 u2 = Min(u2, myusup);
450 if(Abs(u2 - myusup) < 1.e-9) {
452 u1 = Max(u1, myumin);
456 if(v2 - v1 < 2.*PasV) {
457 if(Abs(v1 - myvmin) < 1.e-9) {
459 v2 = Min(v2, myvsup);
461 if(Abs(v2 - myvsup) < 1.e-9) {
463 v1 = Max(v1, myvmin);
467 Standard_Real du = (u2 - u1)/(nbsubsample-1);
468 Standard_Real dv = (v2 - v1)/(nbsubsample-1);
472 Standard_Boolean NewSolution = Standard_False;
473 Standard_Integer Nu, Nv;
474 for (Nu = 1, u = u1; Nu < nbsubsample; Nu++, u += du) {
475 for (Nv = 1, v = v1; Nv < nbsubsample; Nv++, v += dv) {
476 gp_Pnt Puv = myS->Value(u, v);
477 dist = P.SquareDistance(Puv);
482 NewSolution = Standard_True;
490 math_FunctionSetRoot S (myF,UV,Tol,UVinf,UVsup, aNbMaxIter);
492 } //end of if (ToResolveOnSubgrid)
493 } //end of if (f == Extrema_ExtFlag_MIN)
495 myDone = Standard_True;
498 void Extrema_GenExtPS::SetFlag(const Extrema_ExtFlag F)
503 void Extrema_GenExtPS::SetAlgo(const Extrema_ExtAlgo A)
508 void Extrema_GenExtPS::Perform(const gp_Pnt& P)
511 myDone = Standard_False;
514 Standard_Real PasU = myusup - myumin;
515 Standard_Real PasV = myvsup - myvmin;
516 Standard_Real U, U0 = PasU / myusample / 100.;
517 Standard_Real V, V0 = PasV / myvsample / 100.;
518 PasU = (PasU - U0) / (myusample - 1);
519 PasV = (PasV - V0) / (myvsample - 1);
523 //math_Vector Tol(1, 2);
526 if(myAlgo == Extrema_ExtAlgo_Grad)
528 Standard_Integer NoU,NoV;
530 TColStd_Array2OfReal TheDist(0, myusample+1, 0, myvsample+1);
531 for ( NoU = 1, U = U0; NoU <= myusample; NoU++, U += PasU) {
532 for ( NoV = 1, V = V0; NoV <= myvsample; NoV++, V += PasV) {
533 TheDist(NoU, NoV) = P.SquareDistance(mypoints->Value(NoU, NoV));
537 TColStd_Array2OfInteger TbSel(0,myusample+1,0,myvsample+1);
542 if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX)
544 for (NoV = 0; NoV <= myvsample+1; NoV++) {
545 TheDist(0,NoV) = RealLast();
546 TheDist(myusample+1,NoV) = RealLast();
548 for (NoU = 1; NoU <= myusample; NoU++) {
549 TheDist(NoU,0) = RealLast();
550 TheDist(NoU,myvsample+1) = RealLast();
552 for (NoU = 1; NoU <= myusample; NoU++) {
553 for (NoV = 1; NoV <= myvsample; NoV++) {
554 if (TbSel(NoU,NoV) == 0) {
555 Dist = TheDist(NoU,NoV);
556 if ((TheDist(NoU-1,NoV-1) >= Dist) &&
557 (TheDist(NoU-1,NoV ) >= Dist) &&
558 (TheDist(NoU-1,NoV+1) >= Dist) &&
559 (TheDist(NoU ,NoV-1) >= Dist) &&
560 (TheDist(NoU ,NoV+1) >= Dist) &&
561 (TheDist(NoU+1,NoV-1) >= Dist) &&
562 (TheDist(NoU+1,NoV ) >= Dist) &&
563 (TheDist(NoU+1,NoV+1) >= Dist)) {
564 //Create array of UV vectors to calculate min
565 UV(1) = U0 + (NoU - 1) * PasU;
566 UV(2) = V0 + (NoV - 1) * PasV;
567 FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
574 if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
576 for (NoV = 0; NoV <= myvsample+1; NoV++) {
577 TheDist(0,NoV) = RealFirst();
578 TheDist(myusample+1,NoV) = RealFirst();
580 for (NoU = 1; NoU <= myusample; NoU++) {
581 TheDist(NoU,0) = RealFirst();
582 TheDist(NoU,myvsample+1) = RealFirst();
584 for (NoU = 1; NoU <= myusample; NoU++) {
585 for (NoV = 1; NoV <= myvsample; NoV++) {
586 if (TbSel(NoU,NoV) == 0) {
587 Dist = TheDist(NoU,NoV);
588 if ((TheDist(NoU-1,NoV-1) <= Dist) &&
589 (TheDist(NoU-1,NoV ) <= Dist) &&
590 (TheDist(NoU-1,NoV+1) <= Dist) &&
591 (TheDist(NoU ,NoV-1) <= Dist) &&
592 (TheDist(NoU ,NoV+1) <= Dist) &&
593 (TheDist(NoU+1,NoV-1) <= Dist) &&
594 (TheDist(NoU+1,NoV ) <= Dist) &&
595 (TheDist(NoU+1,NoV+1) <= Dist)) {
596 //Create array of UV vectors to calculate max
597 UV(1) = U0 + (NoU - 1) * PasU;
598 UV(2) = V0 + (NoV - 1) * PasV;
599 FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MAX);
609 if(myFlag == Extrema_ExtFlag_MIN || myFlag == Extrema_ExtFlag_MINMAX)
611 Bnd_Sphere aSol = mySphereArray->Value(0);
612 Bnd_SphereUBTreeSelectorMin aSelector(mySphereArray, aSol);
613 //aSelector.SetMaxDist( RealLast() );
614 aSelector.DefineCheckPoint( P );
615 Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
616 //TODO: check if no solution in binary tree
617 Bnd_Sphere& aSph = aSelector.Sphere();
619 UV(1) = U0 + (aSph.U() - 1) * PasU;
620 UV(2) = V0 + (aSph.V() - 1) * PasV;
622 FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MIN);
624 if(myFlag == Extrema_ExtFlag_MAX || myFlag == Extrema_ExtFlag_MINMAX)
626 Bnd_Sphere aSol = mySphereArray->Value(0);
627 Bnd_SphereUBTreeSelectorMax aSelector(mySphereArray, aSol);
628 //aSelector.SetMaxDist( RealLast() );
629 aSelector.DefineCheckPoint( P );
630 Standard_Integer aNbSel = mySphereUBTree->Select( aSelector );
631 //TODO: check if no solution in binary tree
632 Bnd_Sphere& aSph = aSelector.Sphere();
634 UV(1) = U0 + (aSph.U() - 1) * PasU;
635 UV(2) = V0 + (aSph.V() - 1) * PasV;
637 FindSolution(P, UV, PasU, PasV, Extrema_ExtFlag_MAX);
641 //=============================================================================
643 Standard_Boolean Extrema_GenExtPS::IsDone () const { return myDone; }
644 //=============================================================================
646 Standard_Integer Extrema_GenExtPS::NbExt () const
648 if (!IsDone()) { StdFail_NotDone::Raise(); }
651 //=============================================================================
653 Standard_Real Extrema_GenExtPS::SquareDistance (const Standard_Integer N) const
655 if (!IsDone()) { StdFail_NotDone::Raise(); }
656 return myF.SquareDistance(N);
658 //=============================================================================
660 Extrema_POnSurf Extrema_GenExtPS::Point (const Standard_Integer N) const
662 if (!IsDone()) { StdFail_NotDone::Raise(); }
665 //=============================================================================