0027299: Incorrect result of the normal projection algorithm
[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 approximation.
214   Standard_Real aLC = 9.0; // Default value.
215   Standard_Boolean isConstLockedFlag = Standard_False;
216   if (C1.GetType() == GeomAbs_Line)
217   {
218     Standard_Real aMaxDer = 1.0 / C2.Resolution(1.0);
219     if (aLC > aMaxDer)
220     {
221       isConstLockedFlag = Standard_True;
222       aLC = aMaxDer;
223     }
224   }
225   if (C2.GetType() == GeomAbs_Line)
226   {
227     Standard_Real aMaxDer = 1.0 / C1.Resolution(1.0);
228     if (aLC > aMaxDer)
229     {
230       isConstLockedFlag = Standard_True;
231       aLC = aMaxDer;
232     }
233   }
234
235   Extrema_GlobOptFuncCCC2 aFunc (C1, C2);
236   math_GlobOptMin aFinder(&aFunc, myLowBorder, myUppBorder, aLC);
237   aFinder.SetLipConstState(isConstLockedFlag);
238   aFinder.SetContinuity(aContinuity == GeomAbs_C2 ? 2 : 1);
239   Standard_Real aDiscTol = 1.0e-2;
240   Standard_Real aValueTol = 1.0e-2;
241   Standard_Real aSameTol = myCurveMinTol / (aDiscTol);
242   aFinder.SetTol(aDiscTol, aSameTol);
243   aFinder.SetFunctionalMinimalValue(0.0); // Best distance cannot be lower than 0.0.
244
245   // Size computed to have cell index inside of int32 value.
246   const Standard_Real aCellSize = Max(anIntervals1.Upper() - anIntervals1.Lower(),
247                                       anIntervals2.Upper() - anIntervals2.Lower())
248                                   * Precision::PConfusion() / (2.0 * Sqrt(2.0));
249   Extrema_CCPointsInspector anInspector(Precision::PConfusion());
250   NCollection_CellFilter<Extrema_CCPointsInspector> aFilter(aCellSize);
251   NCollection_Vector<gp_XY> aPnts;
252
253   Standard_Integer i,j,k;
254   math_Vector aFirstBorderInterval(1,2);
255   math_Vector aSecondBorderInterval(1,2);
256   Standard_Real aF = RealLast(); // Best functional value.
257   Standard_Real aCurrF = RealLast(); // Current functional value computed on current interval.
258   for(i = 1; i <= aNbInter[0]; i++)
259   {
260     for(j = 1; j <= aNbInter[1]; j++)
261     {
262       aFirstBorderInterval(1) = anIntervals1(i);
263       aFirstBorderInterval(2) = anIntervals2(j); 
264       aSecondBorderInterval(1) = anIntervals1(i + 1);
265       aSecondBorderInterval(2) = anIntervals2(j + 1);
266
267       aFinder.SetLocalParams(aFirstBorderInterval, aSecondBorderInterval);
268       aFinder.Perform(GetSingleSolutionFlag());
269
270       // Check that solution found on current interval is not worse than previous.
271       aCurrF = aFinder.GetF();
272       if (aCurrF >= aF + aSameTol * aValueTol)
273       {
274         continue;
275       }
276
277       // Clean previously computed solution if current one is better.
278       if (aCurrF > aF - aSameTol * aValueTol)
279       {
280         if (aCurrF < aF)
281           aF = aCurrF;
282       }
283       else
284       {
285         aF = aCurrF;
286         aFilter.Reset(aCellSize);
287         aPnts.Clear();
288       }
289
290       // Save found solutions avoiding repetitions.
291       math_Vector sol(1,2);
292       for(k = 1; k <= aFinder.NbExtrema(); k++)
293       {
294         aFinder.Points(k, sol);
295         gp_XY aPnt2d(sol(1), sol(2));
296
297         gp_XY aXYmin = anInspector.Shift(aPnt2d, -aCellSize);
298         gp_XY aXYmax = anInspector.Shift(aPnt2d,  aCellSize);
299
300         anInspector.ClearFind();
301         anInspector.SetCurrent(aPnt2d);
302         aFilter.Inspect(aXYmin, aXYmax, anInspector);
303         if (!anInspector.isFind())
304         {
305           // Point is out of close cells, add new one.
306           aFilter.Add(aPnt2d, aPnt2d);
307           aPnts.Append(gp_XY(sol(1), sol(2)));
308         }
309       }
310     }
311   }
312
313   if (aPnts.Size() == 0)
314   {
315     // No solutions.
316     myDone = Standard_False;
317     return;
318   }
319
320   // Check for infinity solutions case, for this:
321   // Sort points lexicographically and check midpoint between each two neighboring points.
322   // If all midpoints functional value is acceptable
323   // then set myParallel flag to true and return one solution.
324   std::sort(aPnts.begin(), aPnts.end(), comp);
325   Standard_Boolean isParallel = Standard_False;
326   Standard_Real aVal = 0.0;
327   math_Vector aVec(1,2, 0.0);
328
329   // Avoid mark parallel case when have duplicates out of tolerance.
330   // Bad conditioned task: bug25635_1, bug23706_10, bug23706_13.
331   const Standard_Integer aMinNbInfSol = 100;
332   if (aPnts.Size() >= aMinNbInfSol)
333   {
334     isParallel = Standard_True;
335     for(Standard_Integer anIdx = aPnts.Lower(); anIdx <= aPnts.Upper() - 1; anIdx++)
336     {
337       const gp_XY& aCurrent = aPnts(anIdx);
338       const gp_XY& aNext    = aPnts(anIdx + 1);
339
340       aVec(1) = (aCurrent.X() + aNext.X()) * 0.5;
341       aVec(2) = (aCurrent.Y() + aNext.Y()) * 0.5;
342
343       aFunc.Value(aVec, aVal);
344
345       if (Abs(aVal - aF) > Precision::Confusion())
346       {
347         isParallel = Standard_False;
348         break;
349       }
350     }
351   }
352
353   if (isParallel)
354   {
355     const gp_XY& aCurrent = aPnts.First();
356     myPoints1.Append(aCurrent.X());
357     myPoints2.Append(aCurrent.Y());
358     myParallel = Standard_True;
359   }
360   else
361   {
362     for(Standard_Integer anIdx = aPnts.Lower(); anIdx <= aPnts.Upper(); anIdx++)
363     {
364       const gp_XY& aCurrent = aPnts(anIdx);
365       myPoints1.Append(aCurrent.X());
366       myPoints2.Append(aCurrent.Y());
367     }
368   }
369
370   myDone = Standard_True;
371 }
372
373 //=======================================================================
374 //function : IsDone
375 //purpose  : 
376 //=======================================================================
377 Standard_Boolean Extrema_GenExtCC::IsDone() const 
378 {
379   return myDone; 
380 }
381
382 //=======================================================================
383 //function : IsParallel
384 //purpose  : 
385 //=======================================================================
386 Standard_Boolean Extrema_GenExtCC::IsParallel() const 
387 {
388   return myParallel; 
389 }
390
391 //=======================================================================
392 //function : NbExt
393 //purpose  : 
394 //=======================================================================
395 Standard_Integer Extrema_GenExtCC::NbExt() const
396 {
397   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::NbExt()")
398
399   return myPoints1.Length();
400 }
401
402 //=======================================================================
403 //function : SquareDistance
404 //purpose  : 
405 //=======================================================================
406 Standard_Real Extrema_GenExtCC::SquareDistance(const Standard_Integer N) const
407 {
408   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::SquareDistance()")
409   Standard_OutOfRange_Raise_if ((N < 1 || N > NbExt()), "Extrema_GenExtCC::SquareDistance()")
410
411   return Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)).SquareDistance(Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
412 }
413
414 //=======================================================================
415 //function : Points
416 //purpose  : 
417 //=======================================================================
418 void Extrema_GenExtCC::Points(const Standard_Integer N,
419                               POnC& P1,
420                               POnC& P2) const
421 {
422   StdFail_NotDone_Raise_if (!myDone, "Extrema_GenExtCC::Points()")
423   Standard_OutOfRange_Raise_if ((N < 1 || N > NbExt()), "Extrema_GenExtCC::Points()")
424
425   P1.SetValues(myPoints1(N), Tool1::Value(*((Curve1*)myC[0]), myPoints1(N)));
426   P2.SetValues(myPoints2(N), Tool2::Value(*((Curve2*)myC[1]), myPoints2(N)));
427 }
428
429 //=======================================================================
430 //function : SetSingleSolutionFlag
431 //purpose  : 
432 //=======================================================================
433 void Extrema_GenExtCC::SetSingleSolutionFlag(const Standard_Boolean theFlag)
434 {
435   myIsFindSingleSolution = theFlag;
436 }
437
438 //=======================================================================
439 //function : GetSingleSolutionFlag
440 //purpose  : 
441 //=======================================================================
442 Standard_Boolean Extrema_GenExtCC::GetSingleSolutionFlag() const
443 {
444   return myIsFindSingleSolution;
445 }