0027565: [Regression to OCCT 7.0.0] Number of Intersections Is Wrong
[occt.git] / src / Extrema / Extrema_GenExtCC.gxx
1 // Created on: 1995-07-18
2 // Created by: Modelistation
3 // Copyright (c) 1995-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17 #include <algorithm>
18
19 #include <Extrema_GlobOptFuncCC.hxx>
20 #include <math_GlobOptMin.hxx>
21 #include <Standard_NullObject.hxx>
22 #include <Standard_OutOfRange.hxx>
23 #include <StdFail_NotDone.hxx>
24 #include <TColStd_Array1OfReal.hxx>
25 #include <Precision.hxx>
26 #include <NCollection_Vector.hxx>
27 #include <NCollection_CellFilter.hxx>
28
29 // Comparator, used in std::sort.
30 static Standard_Boolean comp(const gp_XY& theA,
31                              const gp_XY& theB)
32 {
33   if (theA.X() < theB.X())
34   {
35     return Standard_True;
36   }
37   else
38   {
39     if (theA.X() == theB.X())
40     {
41       if (theA.Y() < theB.Y())
42         return Standard_True;
43     }
44   }
45
46   return Standard_False;
47 }
48
49 class Extrema_CCPointsInspector : public NCollection_CellFilter_InspectorXY
50 {
51 public:
52   typedef gp_XY Target;
53   //! Constructor; remembers the tolerance
54   Extrema_CCPointsInspector (const Standard_Real theTol)
55   {
56     myTol = theTol * theTol;
57     myIsFind = Standard_False;
58   }
59
60   void ClearFind()
61   {
62     myIsFind = Standard_False;
63   }
64
65   Standard_Boolean isFind()
66   {
67     return myIsFind;
68   }
69
70   //! Set current point to search for coincidence
71   void SetCurrent (const gp_XY& theCurPnt) 
72   { 
73     myCurrent = theCurPnt;
74   }
75
76   //! Implementation of inspection method
77   NCollection_CellFilter_Action Inspect (const Target& theObject)
78   {
79     gp_XY aPt = myCurrent.Subtracted(theObject);
80     const Standard_Real aSQDist = aPt.SquareModulus();
81     if(aSQDist < myTol)
82     {
83       myIsFind = Standard_True;
84     }
85
86     return CellFilter_Keep;
87   }
88
89 private:
90   Standard_Real myTol;
91   gp_XY myCurrent;
92   Standard_Boolean myIsFind;
93 };
94
95 //=======================================================================
96 //function : Extrema_GenExtCC
97 //purpose  : 
98 //=======================================================================
99 Extrema_GenExtCC::Extrema_GenExtCC()
100 : myIsFindSingleSolution(Standard_False),
101   myParallel(Standard_False),
102   myCurveMinTol(Precision::PConfusion()),
103   myLowBorder(1,2),
104   myUppBorder(1,2),
105   myDone(Standard_False)
106 {
107   myC[0] = myC[1] = 0;
108 }
109
110 //=======================================================================
111 //function : Extrema_GenExtCC
112 //purpose  : 
113 //=======================================================================
114 Extrema_GenExtCC::Extrema_GenExtCC(const Curve1& C1,
115                                    const Curve2& C2)
116 : myIsFindSingleSolution(Standard_False),
117   myParallel(Standard_False),
118   myCurveMinTol(Precision::PConfusion()),
119   myLowBorder(1,2),
120   myUppBorder(1,2),
121   myDone(Standard_False)
122 {
123   myC[0] = (Standard_Address)&C1;
124   myC[1] = (Standard_Address)&C2;
125   myLowBorder(1) = C1.FirstParameter();
126   myLowBorder(2) = C2.FirstParameter();
127   myUppBorder(1) = C1.LastParameter();
128   myUppBorder(2) = C2.LastParameter();
129 }
130
131 //=======================================================================
132 //function : Extrema_GenExtCC
133 //purpose  : 
134 //=======================================================================
135 Extrema_GenExtCC::Extrema_GenExtCC(const Curve1& C1,
136                                    const Curve2& C2,
137                                    const Standard_Real Uinf,
138                                    const Standard_Real Usup,
139                                    const Standard_Real Vinf,
140                                    const Standard_Real Vsup)
141 : myIsFindSingleSolution(Standard_False),
142   myParallel(Standard_False),
143   myCurveMinTol(Precision::PConfusion()),
144   myLowBorder(1,2),
145   myUppBorder(1,2),
146   myDone(Standard_False)
147 {
148   myC[0] = (Standard_Address)&C1;
149   myC[1] = (Standard_Address)&C2;
150   myLowBorder(1) = Uinf;
151   myLowBorder(2) = Vinf;
152   myUppBorder(1) = Usup;
153   myUppBorder(2) = Vsup;
154 }
155
156 //=======================================================================
157 //function : SetParams
158 //purpose  : 
159 //=======================================================================
160 void Extrema_GenExtCC::SetParams(const Curve1& C1,
161                                  const Curve2& C2,
162                                  const Standard_Real Uinf,
163                                  const Standard_Real Usup,
164                                  const Standard_Real Vinf,
165                                  const Standard_Real Vsup)
166 {
167   myC[0] = (Standard_Address)&C1;
168   myC[1] = (Standard_Address)&C2;
169   myLowBorder(1) = Uinf;
170   myLowBorder(2) = Vinf;
171   myUppBorder(1) = Usup;
172   myUppBorder(2) = Vsup;
173 }
174
175 //=======================================================================
176 //function : SetTolerance
177 //purpose  : 
178 //=======================================================================
179 void Extrema_GenExtCC::SetTolerance(Standard_Real theTol)
180 {
181   myCurveMinTol = theTol;
182 }
183
184 //=======================================================================
185 //function : Perform
186 //purpose  : 
187 //=======================================================================
188 void Extrema_GenExtCC::Perform()
189 {
190   myDone = Standard_False;
191   myParallel = Standard_False;
192
193   Curve1 &C1 = *(Curve1*)myC[0];
194   Curve2 &C2 = *(Curve2*)myC[1];
195
196   Standard_Integer aNbInter[2];
197   GeomAbs_Shape aContinuity = GeomAbs_C2;
198   aNbInter[0] = C1.NbIntervals(aContinuity);
199   aNbInter[1] = C2.NbIntervals(aContinuity);
200
201   if (aNbInter[0] * aNbInter[1] > 100)
202   {
203     aContinuity = GeomAbs_C1;
204     aNbInter[0] = C1.NbIntervals(aContinuity);
205     aNbInter[1] = C2.NbIntervals(aContinuity);
206   }
207
208   TColStd_Array1OfReal anIntervals1(1, aNbInter[0] + 1);
209   TColStd_Array1OfReal anIntervals2(1, aNbInter[1] + 1);
210   C1.Intervals(anIntervals1, aContinuity);
211   C2.Intervals(anIntervals2, aContinuity);
212
213   // Lipchitz constant computation.
214   Standard_Real aLC = 9.0; // Default value.
215   const Standard_Real aMaxDer1 = 1.0 / C1.Resolution(1.0);
216   const Standard_Real aMaxDer2 = 1.0 / C2.Resolution(1.0);
217   Standard_Real aMaxDer = Max(aMaxDer1, aMaxDer2) * Sqrt(2.0);
218   if (aLC > aMaxDer)
219     aLC = aMaxDer;
220
221   // Change constant value according to the concrete curve types.
222   Standard_Boolean isConstLockedFlag = Standard_False;
223   if (C1.GetType() == GeomAbs_Line)
224   {
225     aMaxDer = 1.0 / C2.Resolution(1.0);
226     if (aLC > aMaxDer)
227     {
228       isConstLockedFlag = Standard_True;
229       aLC = aMaxDer;
230     }
231   }
232   if (C2.GetType() == GeomAbs_Line)
233   {
234     aMaxDer = 1.0 / C1.Resolution(1.0);
235     if (aLC > aMaxDer)
236     {
237       isConstLockedFlag = Standard_True;
238       aLC = aMaxDer;
239     }
240   }
241
242   Extrema_GlobOptFuncCCC2 aFunc (C1, C2);
243   math_GlobOptMin aFinder(&aFunc, myLowBorder, myUppBorder, aLC);
244   aFinder.SetLipConstState(isConstLockedFlag);
245   aFinder.SetContinuity(aContinuity == GeomAbs_C2 ? 2 : 1);
246   Standard_Real aDiscTol = 1.0e-2;
247   Standard_Real aValueTol = 1.0e-2;
248   Standard_Real aSameTol = myCurveMinTol / (aDiscTol);
249   aFinder.SetTol(aDiscTol, aSameTol);
250   aFinder.SetFunctionalMinimalValue(0.0); // Best distance cannot be lower than 0.0.
251
252   // Size computed to have cell index inside of int32 value.
253   const Standard_Real aCellSize = Max(anIntervals1.Last() - anIntervals1.First(),
254                                       anIntervals2.Last() - anIntervals2.First())
255                                   * Precision::PConfusion() / (2.0 * Sqrt(2.0));
256   Extrema_CCPointsInspector anInspector(aCellSize);
257   NCollection_CellFilter<Extrema_CCPointsInspector> aFilter(aCellSize);
258   NCollection_Vector<gp_XY> aPnts;
259
260   Standard_Integer i,j,k;
261   math_Vector aFirstBorderInterval(1,2);
262   math_Vector aSecondBorderInterval(1,2);
263   Standard_Real aF = RealLast(); // Best functional value.
264   Standard_Real aCurrF = RealLast(); // Current functional value computed on current interval.
265   for(i = 1; i <= aNbInter[0]; i++)
266   {
267     for(j = 1; j <= aNbInter[1]; j++)
268     {
269       aFirstBorderInterval(1) = anIntervals1(i);
270       aFirstBorderInterval(2) = anIntervals2(j); 
271       aSecondBorderInterval(1) = anIntervals1(i + 1);
272       aSecondBorderInterval(2) = anIntervals2(j + 1);
273
274       aFinder.SetLocalParams(aFirstBorderInterval, aSecondBorderInterval);
275       aFinder.Perform(GetSingleSolutionFlag());
276
277       // Check that solution found on current interval is not worse than previous.
278       aCurrF = aFinder.GetF();
279       if (aCurrF >= aF + aSameTol * aValueTol)
280       {
281         continue;
282       }
283
284       // Clean previously computed solution if current one is better.
285       if (aCurrF > aF - aSameTol * aValueTol)
286       {
287         if (aCurrF < aF)
288           aF = aCurrF;
289       }
290       else
291       {
292         aF = aCurrF;
293         aFilter.Reset(aCellSize);
294         aPnts.Clear();
295       }
296
297       // Save found solutions avoiding repetitions.
298       math_Vector sol(1,2);
299       for(k = 1; k <= aFinder.NbExtrema(); k++)
300       {
301         aFinder.Points(k, sol);
302         gp_XY aPnt2d(sol(1), sol(2));
303
304         gp_XY aXYmin = anInspector.Shift(aPnt2d, -aCellSize);
305         gp_XY aXYmax = anInspector.Shift(aPnt2d,  aCellSize);
306
307         anInspector.ClearFind();
308         anInspector.SetCurrent(aPnt2d);
309         aFilter.Inspect(aXYmin, aXYmax, anInspector);
310         if (!anInspector.isFind())
311         {
312           // Point is out of close cells, add new one.
313           aFilter.Add(aPnt2d, aPnt2d);
314           aPnts.Append(gp_XY(sol(1), sol(2)));
315         }
316       }
317     }
318   }
319
320   if (aPnts.Size() == 0)
321   {
322     // No solutions.
323     myDone = Standard_False;
324     return;
325   }
326
327   // Check for infinity solutions case, for this:
328   // Sort points lexicographically and check midpoint between each two neighboring points.
329   // If all midpoints functional value is acceptable
330   // then set myParallel flag to true and return one solution.
331   std::sort(aPnts.begin(), aPnts.end(), comp);
332   Standard_Boolean isParallel = Standard_False;
333   Standard_Real aVal = 0.0;
334   math_Vector aVec(1,2, 0.0);
335
336   // Avoid mark parallel case when have duplicates out of tolerance.
337   // Bad conditioned task: bug25635_1, bug23706_10, bug23706_13.
338   if (aPnts.Size() >= 2)
339   {
340     isParallel = Standard_True;
341     for(Standard_Integer anIdx = aPnts.Lower(); anIdx <= aPnts.Upper() - 1; anIdx++)
342     {
343       const gp_XY& aCurrent = aPnts(anIdx);
344       const gp_XY& aNext    = aPnts(anIdx + 1);
345
346       aVec(1) = (aCurrent.X() + aNext.X()) * 0.5;
347       aVec(2) = (aCurrent.Y() + aNext.Y()) * 0.5;
348
349       aFunc.Value(aVec, aVal);
350
351       if (Abs(aVal - aF) > Precision::Confusion())
352       {
353         isParallel = Standard_False;
354         break;
355       }
356     }
357   }
358
359   if (isParallel)
360   {
361     const gp_XY& aCurrent = aPnts.First();
362     myPoints1.Append(aCurrent.X());
363     myPoints2.Append(aCurrent.Y());
364     myParallel = Standard_True;
365   }
366   else
367   {
368     for(Standard_Integer anIdx = aPnts.Lower(); anIdx <= aPnts.Upper(); anIdx++)
369     {
370       const gp_XY& aCurrent = aPnts(anIdx);
371       myPoints1.Append(aCurrent.X());
372       myPoints2.Append(aCurrent.Y());
373     }
374   }
375
376   myDone = Standard_True;
377 }
378
379 //=======================================================================
380 //function : IsDone
381 //purpose  : 
382 //=======================================================================
383 Standard_Boolean Extrema_GenExtCC::IsDone() const 
384 {
385   return myDone; 
386 }
387
388 //=======================================================================
389 //function : IsParallel
390 //purpose  : 
391 //=======================================================================
392 Standard_Boolean Extrema_GenExtCC::IsParallel() const 
393 {
394   return myParallel; 
395 }
396
397 //=======================================================================
398 //function : NbExt
399 //purpose  : 
400 //=======================================================================
401 Standard_Integer Extrema_GenExtCC::NbExt() const
402 {
403   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::NbExt()")
404
405   return myPoints1.Length();
406 }
407
408 //=======================================================================
409 //function : SquareDistance
410 //purpose  : 
411 //=======================================================================
412 Standard_Real Extrema_GenExtCC::SquareDistance(const Standard_Integer N) const
413 {
414   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::SquareDistance()")
415   Standard_OutOfRange_Raise_if ((N < 1 || N > NbExt()), "Extrema_GenExtCC::SquareDistance()")
416
417   return Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)).SquareDistance(Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
418 }
419
420 //=======================================================================
421 //function : Points
422 //purpose  : 
423 //=======================================================================
424 void Extrema_GenExtCC::Points(const Standard_Integer N,
425                               POnC& P1,
426                               POnC& P2) const
427 {
428   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::Points()")
429   Standard_OutOfRange_Raise_if ((N < 1 || N > NbExt()), "Extrema_GenExtCC::Points()")
430
431   P1.SetValues(myPoints1(N), Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)));
432   P2.SetValues(myPoints2(N), Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
433 }
434
435 //=======================================================================
436 //function : SetSingleSolutionFlag
437 //purpose  : 
438 //=======================================================================
439 void Extrema_GenExtCC::SetSingleSolutionFlag(const Standard_Boolean theFlag)
440 {
441   myIsFindSingleSolution = theFlag;
442 }
443
444 //=======================================================================
445 //function : GetSingleSolutionFlag
446 //purpose  : 
447 //=======================================================================
448 Standard_Boolean Extrema_GenExtCC::GetSingleSolutionFlag() const
449 {
450   return myIsFindSingleSolution;
451 }