0027059: Point->Curve Projection/Extrema fails [OCCT 7 only]
[occt.git] / src / Extrema / Extrema_GExtPC.gxx
1 // Created on: 1992-10-19
2 // Created by: Laurent PAINNOT
3 // Copyright (c) 1992-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 // Modified by cma, Fri Dec 10 18:11:56 1993
18
19 #include Extrema_EPC_hxx
20 #include ThePOnC_hxx
21 #include TheSequenceOfPOnC_hxx
22 #include ThePoint_hxx
23 #include TheVector_hxx
24 #include TheExtPElC_hxx
25 #include <StdFail_NotDone.hxx>
26 #include <Standard_Failure.hxx>
27 #include <GeomAbs_CurveType.hxx>
28 #include <Precision.hxx>
29 #include <ElCLib.hxx>
30 #include <TColStd_Array1OfReal.hxx>
31 #include <NCollection_Array1.hxx>
32
33
34 //=======================================================================
35 //function : Perform
36 //purpose  : 
37 //=======================================================================
38 void Extrema_GExtPC::Perform(const ThePoint& P)
39 {
40   mySqDist.Clear();
41   mypoint.Clear();
42   myismin.Clear();
43   Standard_Integer i, NbExt, n;                     
44   Standard_Real U;
45   mysample = 17;
46   Standard_Real t3d = Precision::Confusion();
47   if (Precision::IsInfinite(myuinf)) mydist1 = RealLast();
48   else {
49     Pf = TheCurveTool::Value(*((TheCurve*)myC), myuinf);
50     mydist1 = P.SquareDistance(Pf);
51   }
52   
53   if (Precision::IsInfinite(myusup)) mydist2 = RealLast();
54   else {
55     Pl = TheCurveTool::Value(*((TheCurve*)myC), myusup);
56     mydist2 = P.SquareDistance(Pl);
57   }
58
59   TheCurve & aCurve = *((TheCurve*)myC);
60
61   switch(type) {
62   case GeomAbs_Circle: 
63     {
64       myExtPElC.Perform(P, TheCurveTool::Circle(aCurve), t3d, myuinf, myusup);
65     }
66     break;
67   case GeomAbs_Ellipse: 
68     {
69       myExtPElC.Perform(P, TheCurveTool::Ellipse(aCurve), t3d, myuinf, myusup);
70     }
71     break;
72   case GeomAbs_Parabola: 
73     {
74       myExtPElC.Perform(P, TheCurveTool::Parabola(aCurve), t3d,myuinf,myusup);
75     }
76     break;
77   case GeomAbs_Hyperbola: 
78     {
79       myExtPElC.Perform(P,TheCurveTool::Hyperbola(aCurve),t3d, myuinf, myusup);
80     }
81     break;
82   case GeomAbs_Line: 
83     {
84       myExtPElC.Perform(P, TheCurveTool::Line(aCurve), t3d, myuinf, myusup);
85     }
86     break;
87   case GeomAbs_BezierCurve:
88     {
89       myintuinf = myuinf;
90       myintusup = myusup;
91       mysample = (TheCurveTool::Bezier(aCurve))->NbPoles() * 2;
92       myExtPC.Initialize(aCurve);
93       IntervalPerform(P);
94       return;
95     }
96   case GeomAbs_BSplineCurve: 
97     {
98       const Standard_Integer 
99         aFirstIdx = TheCurveTool::BSpline(aCurve)->FirstUKnotIndex(),
100         aLastIdx  = TheCurveTool::BSpline(aCurve)->LastUKnotIndex();
101       // const reference can not be used due to implementation of BRep_Adaptor.
102       TColStd_Array1OfReal aKnots(aFirstIdx, aLastIdx);
103       TheCurveTool::BSpline(aCurve)->Knots(aKnots); 
104
105       // Workaround to work with:
106       // blend, where knots may be moved from parameter space.
107       Standard_Real aPeriodJump = 0.0;
108       // Avoid problem with too close knots.
109       const Standard_Real aTolCoeff = (myusup - myuinf) * Precision::PConfusion();
110       if (TheCurveTool::IsPeriodic(aCurve))
111       {
112         Standard_Integer aPeriodShift =
113           Standard_Integer ((myuinf - aKnots(aFirstIdx)) / TheCurveTool::Period(aCurve));
114         
115         if (myuinf < aKnots(aFirstIdx) - aTolCoeff)
116           aPeriodShift--;
117
118         aPeriodJump = TheCurveTool::Period(aCurve) * aPeriodShift;
119       }
120
121       Standard_Integer anIdx;
122
123       // Find first and last used knot.
124       Standard_Integer aFirstUsedKnot = aFirstIdx,
125                         aLastUsedKnot = aLastIdx;
126       for(anIdx = aFirstIdx; anIdx <= aLastIdx; anIdx++)
127       {
128         Standard_Real aKnot = aKnots(anIdx) + aPeriodJump;
129         if (myuinf >= aKnot - aTolCoeff)
130           aFirstUsedKnot = anIdx;
131         else
132           break;
133
134       }
135       for(anIdx = aLastIdx; anIdx >= aFirstIdx; anIdx--)
136       {
137         Standard_Real aKnot = aKnots(anIdx) + aPeriodJump;
138         if (myusup <= aKnot + aTolCoeff)
139           aLastUsedKnot = anIdx;
140         else
141           break;
142       }
143
144       if (aFirstUsedKnot == aLastUsedKnot)
145       {
146         // Degenerated case:
147         // Some bounds lies out of curve param space.
148         // In this case build one interval with [myuinf, myusup].
149         // Parameters of these indexes will be redefined.
150         aFirstUsedKnot = aFirstIdx;
151         aLastUsedKnot  = aFirstIdx + 1;
152       }
153
154       mysample = (TheCurveTool::BSpline(aCurve))->Degree() + 1;
155
156       // Fill sample points.
157       Standard_Integer aValIdx = 1;
158       NCollection_Array1<Standard_Real> aVal(1, (mysample) * (aLastUsedKnot - aFirstUsedKnot) + 1);
159       NCollection_Array1<Standard_Real> aParam(1, (mysample) * (aLastUsedKnot - aFirstUsedKnot) + 1);
160       for(anIdx = aFirstUsedKnot; anIdx < aLastUsedKnot; anIdx++)
161       {
162         Standard_Real aF = aKnots(anIdx) + aPeriodJump,
163                       aL = aKnots(anIdx + 1) + aPeriodJump;
164
165         if (anIdx == aFirstUsedKnot)
166           aF = myuinf;
167         if (anIdx == aLastUsedKnot - 1)
168           aL = myusup;
169
170         Standard_Real aStep = (aL - aF) / mysample;
171         for(Standard_Integer aPntIdx = 0; aPntIdx < mysample; aPntIdx++)
172         {
173           Standard_Real aCurrentParam = aF + aStep * aPntIdx;
174           aVal(aValIdx) = TheCurveTool::Value(aCurve, aCurrentParam).SquareDistance(P);
175           aParam(aValIdx) = aCurrentParam;
176           aValIdx++;
177         }
178       }
179       // Fill last point.
180       aVal(aValIdx) = TheCurveTool::Value(aCurve, myusup).SquareDistance(P);
181       aParam(aValIdx) = myusup;
182
183        myExtPC.Initialize(aCurve);
184
185       // Find extremas.
186       for(anIdx = aVal.Lower() + 1; anIdx < aVal.Upper(); anIdx++)
187       {
188         if (aVal(anIdx) <= Precision::SquareConfusion())
189         {
190           mySqDist.Append(aVal(anIdx));
191           myismin.Append(Standard_True);
192           mypoint.Append(ThePOnC(aParam(anIdx), TheCurveTool::Value(aCurve, aParam(anIdx))));
193         }
194         if ((aVal(anIdx) >= aVal(anIdx + 1) &&
195              aVal(anIdx) >= aVal(anIdx - 1)) ||
196             (aVal(anIdx) <= aVal(anIdx + 1) &&
197              aVal(anIdx) <= aVal(anIdx - 1)) )
198         {
199           myintuinf = aParam(anIdx - 1);
200           myintusup = aParam(anIdx + 1);
201
202           IntervalPerform(P);
203         }
204       }
205
206       // Solve on the first and last intervals.
207       if (mydist1 > Precision::SquareConfusion())
208       {
209         ThePoint aP1, aP2;
210         TheVector aV1, aV2;
211         TheCurveTool::D1(aCurve, aParam.Value(aParam.Lower()),     aP1, aV1);
212         TheCurveTool::D1(aCurve, aParam.Value(aParam.Lower() + 1), aP2, aV2);
213         TheVector aBase1(P, aP1), aBase2(P, aP2);
214         Standard_Real aVal1 = aV1.Dot(aBase1); // Derivative of (C(u) - P)^2
215         Standard_Real aVal2 = aV2.Dot(aBase2); // Derivative of (C(u) - P)^2
216
217         // Derivatives have opposite signs - min or max inside of interval (sufficient condition).
218         // Necessary condition - when point lies on curve.
219         // Necessary condition - when derivative of point is too small.
220         if(aVal1 * aVal2 <= 0.0 ||
221            aBase1.Dot(aBase2) <= 0.0 ||
222            2.0 * Abs(aVal1) < Precision::Confusion() )
223         {
224           myintuinf = aParam(aVal.Lower());
225           myintusup = aParam(aVal.Lower() + 1);
226           IntervalPerform(P);
227         }
228       }
229
230       if (mydist2 > Precision::SquareConfusion())
231       {
232         ThePoint aP1, aP2;
233         TheVector aV1, aV2;
234         TheCurveTool::D1(aCurve, aParam.Value(aParam.Upper() - 1), aP1, aV1);
235         TheCurveTool::D1(aCurve, aParam.Value(aParam.Upper()),     aP2, aV2);
236         TheVector aBase1(P, aP1), aBase2(P, aP2);
237         Standard_Real aVal1 = aV1.Dot(aBase1); // Derivative of (C(u) - P)^2
238         Standard_Real aVal2 = aV2.Dot(aBase2); // Derivative of (C(u) - P)^2
239
240         // Derivatives have opposite signs - min or max inside of interval (sufficient condition).
241         // Necessary condition - when point lies on curve.
242         // Necessary condition - when derivative of point is too small.
243         if(aVal1 * aVal2 <= 0.0 ||
244            aBase1.Dot(aBase2) <= 0.0 ||
245            2.0 * Abs(aVal2) < Precision::Confusion() )
246         {
247           myintuinf = aParam(aVal.Upper() - 1);
248           myintusup = aParam(aVal.Upper());
249           IntervalPerform(P);
250         }
251       }
252
253       mydone = Standard_True;
254       break;
255     }
256   default:
257     {
258       Standard_Boolean IntExtIsDone = Standard_False;
259       Standard_Boolean IntIsNotValid;
260       n = TheCurveTool::NbIntervals(aCurve, GeomAbs_C2);
261       TColStd_Array1OfReal theInter(1, n+1);
262       Standard_Boolean isPeriodic = TheCurveTool::IsPeriodic(aCurve);
263       TheCurveTool::Intervals(aCurve, theInter, GeomAbs_C2);
264       mysample = Max(mysample/n, 17);
265       TheVector V1;
266       ThePoint PP;
267       Standard_Real s1 = 0.0 ;
268       Standard_Real s2 = 0.0;
269       myExtPC.Initialize(aCurve);
270       for (i = 1; i <= n; i++)
271       {
272         myintuinf = theInter(i);
273         myintusup = theInter(i+1);
274         
275         Standard_Real anInfToCheck = myintuinf;
276         Standard_Real aSupToCheck = myintusup;
277         
278         if (isPeriodic) {
279           Standard_Real aPeriod = TheCurveTool::Period(aCurve);
280           anInfToCheck = ElCLib::InPeriod(myintuinf, myuinf, myuinf+aPeriod);
281           aSupToCheck = myintusup+(anInfToCheck-myintuinf);
282         }    
283         IntIsNotValid = (myuinf > aSupToCheck) || (myusup < anInfToCheck);
284
285         if(IntIsNotValid) continue;
286
287         if (myuinf >= anInfToCheck) anInfToCheck = myuinf;
288         if (myusup <= aSupToCheck) aSupToCheck = myusup;
289         if((aSupToCheck - anInfToCheck) <= mytolu) continue;
290         
291         if (i != 1)
292           {
293           TheCurveTool::D1(aCurve, myintuinf, PP, V1);
294           s1 = (TheVector(P, PP))*V1;
295           if (s1*s2 < 0.0) {
296             mySqDist.Append(PP.SquareDistance(P));
297             myismin.Append((s1 < 0.0));
298             mypoint.Append(ThePOnC(myintuinf, PP));
299           }
300         }
301         if (i != n) {
302           TheCurveTool::D1(aCurve, myintusup, PP, V1);
303           s2 = (TheVector(P, PP))*V1;
304         }
305
306         IntervalPerform(P);
307         IntExtIsDone = IntExtIsDone || mydone;
308       }
309
310       mydone = IntExtIsDone;
311       break;
312     }
313   }
314
315   // Postprocessing.
316   if (type == GeomAbs_BSplineCurve ||
317       type == GeomAbs_OffsetCurve ||
318       type == GeomAbs_OtherCurve)
319   {
320     // Additional checking if the point is on the first or last point of the curve
321     // and does not added yet.
322     if (mydist1 < Precision::SquareConfusion() || 
323         mydist2 < Precision::SquareConfusion())
324     {
325       Standard_Boolean isFirstAdded = Standard_False;
326       Standard_Boolean isLastAdded  = Standard_False;
327       Standard_Integer aNbPoints = mypoint.Length();
328       for (i = 1; i <= aNbPoints; i++)
329       {
330         U = mypoint.Value(i).Parameter();
331         if (Abs(U - myuinf) < mytolu)
332           isFirstAdded = Standard_True;
333         else if (Abs(myusup - U) < mytolu)
334           isLastAdded = Standard_True;
335       }
336       if (!isFirstAdded && mydist1 < Precision::SquareConfusion())
337       {
338         mySqDist.Prepend(mydist1);
339         myismin.Prepend(Standard_True);
340         mypoint.Prepend(ThePOnC(myuinf, Pf));
341       }
342       if (!isLastAdded && mydist2 < Precision::SquareConfusion())
343       {
344         mySqDist.Append(mydist2);
345         myismin.Append(Standard_True);
346         mypoint.Append(ThePOnC(myusup, Pl));
347       }
348       mydone = Standard_True;
349     }
350   }
351   else
352   {
353     // In analytical case
354     mydone = myExtPElC.IsDone();
355     if (mydone)
356     {
357       NbExt = myExtPElC.NbExt();
358       for (i = 1; i <= NbExt; i++)
359       {
360         // Verification de la validite des parametres:
361         ThePOnC PC = myExtPElC.Point(i);
362         U = PC.Parameter();
363         if (TheCurveTool::IsPeriodic(aCurve))
364         {
365           U = ElCLib::InPeriod(U, myuinf, myuinf+TheCurveTool::Period(aCurve));
366         }
367         if ((U >= myuinf-mytolu) && (U <= myusup+mytolu))
368         {
369           PC.SetValues(U, myExtPElC.Point(i).Value());
370           mySqDist.Append(myExtPElC.SquareDistance(i));
371           myismin.Append(myExtPElC.IsMin(i));
372           mypoint.Append(PC);
373         }
374       }
375     }
376   }
377 }
378
379
380 //=======================================================================
381 //function : Initialize
382 //purpose  : 
383 //=======================================================================
384
385 void Extrema_GExtPC::Initialize(const TheCurve&     C,
386                                 const Standard_Real Uinf,
387                                 const Standard_Real Usup,
388                                 const Standard_Real TolF) 
389 {
390   myC = (Standard_Address)&C;
391   myintuinf = myuinf = Uinf;
392   myintusup = myusup = Usup;
393   mytolf = TolF;
394   mytolu = TheCurveTool::Resolution(*((TheCurve*)myC), Precision::Confusion());
395   type = TheCurveTool::GetType(C);
396   mydone = Standard_False;
397   mydist1 = RealLast();
398   mydist2 = RealLast();
399   mysample = 17;
400 }
401
402
403 //=======================================================================
404 //function : IntervalPerform
405 //purpose  : 
406 //=======================================================================
407
408 void Extrema_GExtPC::IntervalPerform(const ThePoint& P)
409 {
410   Standard_Integer i;
411   Standard_Real U;
412   myExtPC.Initialize(mysample, myintuinf, myintusup, mytolu, mytolf);
413   myExtPC.Perform(P);
414   mydone = myExtPC.IsDone();
415   if (mydone)
416   {
417     Standard_Integer NbExt = myExtPC.NbExt();
418     for (i = 1; i <= NbExt; i++)
419     {
420       // Verification de la validite des parametres pour le cas trimme:
421       ThePOnC PC = myExtPC.Point(i);
422       U = PC.Parameter();
423       if (TheCurveTool::IsPeriodic(*((TheCurve*)myC)))
424       {
425         U = ElCLib::InPeriod(U, myuinf, myuinf+TheCurveTool::Period(*((TheCurve*)myC)));
426       }
427       if ((U >= myuinf - mytolu) && (U <= myusup + mytolu))
428       {
429         PC.SetValues(U, PC.Value());
430         mySqDist.Append(myExtPC.SquareDistance(i));
431         myismin.Append(myExtPC.IsMin(i));
432         mypoint.Append(PC);
433       }
434     }
435   }
436 }
437
438
439
440
441 //=======================================================================
442 //function : Extrema_GExtPC
443 //purpose  : 
444 //=======================================================================
445
446 Extrema_GExtPC::Extrema_GExtPC()
447 {
448   myC = 0;
449   mydone = Standard_False;
450   mydist1 = RealLast();
451   mydist2 = RealLast();
452   mytolu = 0.0;
453   mytolf = 0.0;
454   mysample = 17;
455   myintuinf = myintusup = myuinf = myusup = Precision::Infinite();
456   type = GeomAbs_OtherCurve;
457 }
458
459 //=======================================================================
460 //function : Extrema_GExtPC
461 //purpose  : 
462 //=======================================================================
463
464 Extrema_GExtPC::Extrema_GExtPC(const ThePoint&           P, 
465                                const TheCurve&           C,
466                                const Standard_Real       Uinf,
467                                const Standard_Real       Usup,
468                                const Standard_Real       TolF) 
469 {
470   Initialize(C, Uinf, Usup, TolF);
471   Perform(P);
472 }
473
474 //=======================================================================
475 //function : Extrema_GExtPC
476 //purpose  : 
477 //=======================================================================
478
479 Extrema_GExtPC::Extrema_GExtPC(const ThePoint&     P, 
480                                const TheCurve&     C,
481                                const Standard_Real TolF) 
482 {
483   Initialize(C, TheCurveTool::FirstParameter(C), 
484              TheCurveTool::LastParameter(C), TolF);
485   Perform(P);
486 }
487
488
489 //=======================================================================
490 //function : IsDone
491 //purpose  : 
492 //=======================================================================
493
494 Standard_Boolean Extrema_GExtPC::IsDone() const
495 {
496   return mydone;
497 }
498
499
500 //=======================================================================
501 //function : Value
502 //purpose  : 
503 //=======================================================================
504
505 Standard_Real Extrema_GExtPC::SquareDistance(const Standard_Integer N) const 
506 {
507   if(!mydone) StdFail_NotDone::Raise();
508   if ((N < 1) || (N > mySqDist.Length())) Standard_OutOfRange::Raise();
509   return mySqDist.Value(N);
510 }
511
512
513 //=======================================================================
514 //function : NbExt
515 //purpose  : 
516 //=======================================================================
517
518 Standard_Integer Extrema_GExtPC::NbExt() const
519 {
520   if(!mydone) StdFail_NotDone::Raise();
521   return mySqDist.Length();
522 }
523
524
525 //=======================================================================
526 //function : IsMin
527 //purpose  : 
528 //=======================================================================
529
530 Standard_Boolean Extrema_GExtPC::IsMin(const Standard_Integer N) const
531 {
532   if(!mydone) StdFail_NotDone::Raise();
533   if ((N < 1) || (N > mySqDist.Length())) Standard_OutOfRange::Raise();
534   return myismin.Value(N);
535 }
536
537
538
539 //=======================================================================
540 //function : Point
541 //purpose  : 
542 //=======================================================================
543
544 const ThePOnC & Extrema_GExtPC::Point(const Standard_Integer N) const
545 {
546   if(!mydone) StdFail_NotDone::Raise();
547   if ((N < 1) || (N > mySqDist.Length())) Standard_OutOfRange::Raise();
548   return mypoint.Value(N);
549 }
550
551
552 //=======================================================================
553 //function : TrimmedDistances
554 //purpose  : 
555 //=======================================================================
556
557 void Extrema_GExtPC::TrimmedSquareDistances(Standard_Real& dist1, 
558                                       Standard_Real& dist2,
559                                       ThePoint& P1,
560                                       ThePoint& P2) const
561 {
562   dist1 = mydist1;
563   dist2 = mydist2;
564   P1 = Pf;
565   P2 = Pl;
566 }