deadfb6309f2f1d51d607e6b6b15746bd90f462d
[occt.git] / src / GCPnts / GCPnts_TangentialDeflection.pxx
1 // Created on: 1996-11-08
2 // Created by: Jean Claude VAUTHIER
3 // Copyright (c) 1996-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 <GCPnts_DeflectionType.hxx>
18 #include <Standard_ConstructionError.hxx>
19 #include <Precision.hxx>
20 #include <gp_XYZ.hxx>
21 #include <gp_Pnt.hxx>
22 #include <gp_Vec.hxx>
23 #include <gp.hxx>
24 #include <NCollection_List.hxx>
25 #include <math_PSO.hxx>
26 #include <math_BrentMinimum.hxx>
27
28 #define Us3 0.3333333333333333333333333333
29
30 void GCPnts_TangentialDeflection::EvaluateDu (
31   const TheCurve& C,
32   const Standard_Real      U,
33   gp_Pnt&   P,
34   Standard_Real&     Du,
35   Standard_Boolean&  NotDone) const {
36
37   gp_Vec T, N;
38   D2 (C, U, P, T, N);
39   Standard_Real Lt   = T.Magnitude ();
40   Standard_Real LTol = Precision::Confusion ();
41   if (Lt > LTol && N.Magnitude () > LTol) {
42     Standard_Real Lc = N.CrossMagnitude (T);
43     Standard_Real Ln = Lc/Lt;
44     if (Ln > LTol) {
45       Du = sqrt (8.0 * Max(curvatureDeflection, myMinLen) / Ln);
46       NotDone = Standard_False;
47     }
48   }
49 }
50
51
52 //=======================================================================
53 //function : GCPnts_TangentialDeflection
54 //purpose  : 
55 //=======================================================================
56
57 GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
58  const TheCurve&        C,
59  const Standard_Real    AngularDeflection,
60  const Standard_Real    CurvatureDeflection,
61  const Standard_Integer MinimumOfPoints,
62  const Standard_Real    UTol,
63  const Standard_Real    theMinLen)
64
65
66   Initialize (C,AngularDeflection,CurvatureDeflection,MinimumOfPoints,UTol,theMinLen); 
67 }
68
69
70 //=======================================================================
71 //function : GCPnts_TangentialDeflection
72 //purpose  : 
73 //=======================================================================
74
75 GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
76  const TheCurve&        C,
77  const Standard_Real    FirstParameter,
78  const Standard_Real    LastParameter,
79  const Standard_Real    AngularDeflection,
80  const Standard_Real    CurvatureDeflection,
81  const Standard_Integer MinimumOfPoints,
82  const Standard_Real    UTol,
83  const Standard_Real    theMinLen)
84
85
86   Initialize (C, 
87         FirstParameter, 
88         LastParameter,
89         AngularDeflection, 
90         CurvatureDeflection, 
91         MinimumOfPoints,
92         UTol, theMinLen);
93 }
94
95
96
97 //=======================================================================
98 //function : Initialize
99 //purpose  : 
100 //=======================================================================
101
102 void GCPnts_TangentialDeflection::Initialize (
103  const TheCurve&        C, 
104  const Standard_Real    AngularDeflection, 
105  const Standard_Real    CurvatureDeflection,
106  const Standard_Integer MinimumOfPoints,
107  const Standard_Real    UTol,
108  const Standard_Real    theMinLen)
109
110 {
111   Initialize (C, 
112               C.FirstParameter (), 
113               C.LastParameter (),
114               AngularDeflection, 
115               CurvatureDeflection,
116               MinimumOfPoints,
117               UTol, theMinLen);
118 }
119
120
121 //=======================================================================
122 //function : Initialize
123 //purpose  : 
124 //=======================================================================
125
126 void GCPnts_TangentialDeflection::Initialize (
127  const TheCurve&        C, 
128  const Standard_Real    FirstParameter, 
129  const Standard_Real    LastParameter,
130  const Standard_Real    AngularDeflection, 
131  const Standard_Real    CurvatureDeflection,
132  const Standard_Integer MinimumOfPoints,
133  const Standard_Real    UTol,
134  const Standard_Real    theMinLen)
135
136 {
137   
138   Standard_ConstructionError_Raise_if (CurvatureDeflection < Precision::Confusion () ||
139                                        AngularDeflection < Precision::Angular (),
140                                        "GCPnts_TangentialDeflection::Initialize - Zero Deflection")
141
142  parameters.Clear ();
143  points    .Clear ();
144  if (FirstParameter < LastParameter) {
145    firstu = FirstParameter;
146    lastu  = LastParameter;
147  }
148  else {
149    lastu  = FirstParameter;
150    firstu = LastParameter;
151  }
152  uTol                = UTol;
153  angularDeflection   = AngularDeflection;
154  curvatureDeflection = CurvatureDeflection;
155  minNbPnts           = Max (MinimumOfPoints, 2);
156  myMinLen            = Max(theMinLen, Precision::Confusion());
157
158  switch (C.GetType()) {
159
160  case GeomAbs_Line:   
161    PerformLinear (C);
162    break;
163
164  case GeomAbs_Circle: 
165    PerformCircular (C);
166    break;
167
168  case GeomAbs_BSplineCurve:
169    {
170      Handle_TheBSplineCurve BS = C.BSpline() ;
171      if (BS->NbPoles() == 2 ) PerformLinear (C);
172      else                     PerformCurve  (C);
173      break;
174    }
175  case GeomAbs_BezierCurve:
176    {
177      Handle_TheBezierCurve  BZ = C.Bezier(); 
178      if (BZ->NbPoles() == 2) PerformLinear (C);
179      else                    PerformCurve  (C);
180      break;
181    }
182  default: PerformCurve (C);
183    
184  }
185 }
186
187
188 //=======================================================================
189 //function : PerformLinear
190 //purpose  : 
191 //=======================================================================
192
193 void GCPnts_TangentialDeflection::PerformLinear (const TheCurve& C) {
194
195   gp_Pnt P;
196   D0 (C, firstu, P);
197   parameters.Append (firstu);
198   points    .Append (P);
199   if (minNbPnts > 2)
200   {
201     Standard_Real Du = (lastu - firstu) / minNbPnts;
202     Standard_Real U = firstu + Du;
203     for (Standard_Integer i = 2; i < minNbPnts; i++)
204     {
205       D0 (C, U, P);
206       parameters.Append (U);
207       points    .Append (P);
208       U += Du;
209     }
210   }
211   D0 (C, lastu, P);
212   parameters.Append (lastu);
213   points    .Append (P);
214 }
215
216 //=======================================================================
217 //function : PerformCircular
218 //purpose  : 
219 //=======================================================================
220
221 void GCPnts_TangentialDeflection::PerformCircular (const TheCurve& C) 
222 {
223   // akm 8/01/02 : check the radius before divide by it
224   Standard_Real dfR = C.Circle().Radius();
225   Standard_Real Du = GCPnts_TangentialDeflection::ArcAngularStep(
226     dfR, curvatureDeflection, angularDeflection, myMinLen);
227     
228   const Standard_Real aDiff = lastu - firstu;
229   // Round up number of points to satisfy curvatureDeflection more precisely
230   Standard_Integer NbPoints = (Standard_Integer)Min(Ceiling(aDiff / Du), 1.0e+6);
231   NbPoints = Max(NbPoints, minNbPnts - 1);
232   Du       = aDiff / NbPoints;
233
234   gp_Pnt P;
235   Standard_Real U = firstu;
236   for (Standard_Integer i = 1; i <= NbPoints; i++)
237   {
238     D0 (C, U, P);
239     parameters.Append (U);
240     points    .Append (P);
241     U += Du;
242   }
243   D0 (C, lastu, P);
244   parameters.Append (lastu);
245   points    .Append (P);
246 }
247
248
249 //=======================================================================
250 //function : PerformCurve
251 //purpose  : On respecte ll'angle et la fleche, on peut imposer un nombre
252 //           minimum de points sur un element lineaire
253 //=======================================================================
254 void GCPnts_TangentialDeflection::PerformCurve (const TheCurve& C)
255
256 {
257   Standard_Integer i, j;       
258   gp_XYZ V1, V2;
259   gp_Pnt MiddlePoint, CurrentPoint, LastPoint;   
260   Standard_Real Du, Dusave, MiddleU, L1, L2;
261
262   Standard_Real U1       = firstu;   
263   Standard_Real LTol     = Precision::Confusion ();  //protection longueur nulle
264   Standard_Real ATol     = 1.e-2 * angularDeflection;
265   if(ATol > 1.e-2)
266     ATol = 1.e-2;
267   else if(ATol < 1.e-7)
268     ATol = 1.e-7;
269
270   D0 (C, lastu, LastPoint);
271
272   //Initialization du calcul
273
274   Standard_Boolean NotDone = Standard_True;
275   Dusave = (lastu - firstu)*Us3;
276   Du     = Dusave;
277   EvaluateDu (C, U1, CurrentPoint, Du, NotDone);
278   parameters.Append (U1);
279   points    .Append (CurrentPoint);
280
281   // Used to detect "isLine" current bspline and in Du computation in general handling.
282   Standard_Integer NbInterv = C.NbIntervals(GeomAbs_CN);
283   TColStd_Array1OfReal Intervs(1, NbInterv+1);
284   C.Intervals(Intervs, GeomAbs_CN);
285
286   if (NotDone || Du > 5. * Dusave) {
287     //C'est soit une droite, soit une singularite :
288     V1 = (LastPoint.XYZ() - CurrentPoint.XYZ());
289     L1 = V1.Modulus ();
290     if (L1 > LTol)
291     {
292       //Si c'est une droite on verifie en calculant minNbPoints :
293       Standard_Boolean IsLine   = Standard_True;
294       Standard_Integer NbPoints = (minNbPnts > 3) ? minNbPnts : 3;
295       switch (C.GetType()) {
296       case GeomAbs_BSplineCurve:
297         {
298           Handle_TheBSplineCurve BS = C.BSpline() ;
299           NbPoints = Max(BS->Degree() + 1, NbPoints);
300           break;
301         }
302       case GeomAbs_BezierCurve:
303         {
304           Handle_TheBezierCurve  BZ = C.Bezier();
305           NbPoints = Max(BZ->Degree() + 1, NbPoints);
306           break;
307         }
308       default:
309       ;}
310       ////
311       Standard_Real param = 0.;
312       for (i = 1; i <= NbInterv && IsLine; ++i)
313       {
314         // Avoid usage intervals out of [firstu, lastu].
315         if ((Intervs(i+1) < firstu) ||
316             (Intervs(i)   > lastu))
317         {
318           continue;
319         }
320         // Fix border points in applicable intervals, to avoid be out of target interval.
321         if ((Intervs(i)   < firstu) &&
322             (Intervs(i+1) > firstu))
323         {
324           Intervs(i) = firstu;
325         }
326         if ((Intervs(i)   < lastu) &&
327             (Intervs(i+1) > lastu))
328         {
329           Intervs(i + 1) = lastu;
330         }
331
332         Standard_Real delta = (Intervs(i+1) - Intervs(i))/NbPoints;
333         for (j = 1; j <= NbPoints && IsLine; ++j)
334         {
335           param = Intervs(i) + j*delta;
336           D0 (C, param, MiddlePoint);
337           V2 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
338           L2 = V2.Modulus ();
339           if (L2 > LTol)
340           {
341             const Standard_Real aAngle = V2.CrossMagnitude(V1)/(L1*L2);
342             IsLine = (aAngle < ATol);
343           }
344         }
345       }
346
347       if (IsLine)
348       {
349         parameters.Clear();
350         points    .Clear();
351
352         PerformLinear(C);
353         return;
354       }
355       else
356       {
357         //c'etait une singularite on continue :
358         //Du = Dusave;
359         EvaluateDu (C, param, MiddlePoint, Du, NotDone);
360       }
361     }
362     else
363     {
364       Du = (lastu-firstu)/2.1;
365       MiddleU = firstu + Du;
366       D0 (C, MiddleU, MiddlePoint);
367       V1 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
368       L1 = V1.Modulus ();
369       if (L1 < LTol)
370       {
371         // L1 < LTol C'est une courbe de longueur nulle, calcul termine :
372         // on renvoi un segment de 2 points   (protection)
373         parameters.Append (lastu);
374         points    .Append (LastPoint);
375         return;
376       }
377     }
378   }
379
380   if (Du > Dusave) Du = Dusave;
381   else             Dusave = Du;
382
383   if (Du < uTol) {
384     Du = lastu - firstu;
385     if (Du < uTol) {
386       parameters.Append (lastu);
387       points    .Append (LastPoint);
388       return;
389     }
390   }
391
392   //Traitement normal pour une courbe
393   Standard_Boolean MorePoints = Standard_True;
394   Standard_Real U2            = firstu;   
395   Standard_Real AngleMax      = angularDeflection * 0.5;  //car on prend le point milieu
396   Standard_Integer aIdx[2] = {Intervs.Lower(), Intervs.Lower()}; // Indexes of intervals of U1 and U2, used to handle non-uniform case.
397   Standard_Boolean isNeedToCheck = Standard_False;
398   gp_Pnt aPrevPoint = points.Last();
399
400   while (MorePoints) {
401     aIdx[0] = getIntervalIdx(U1, Intervs, aIdx[0]);
402     U2 += Du;
403
404     if (U2 >= lastu) {                       //Bout de courbe
405       U2 = lastu;
406       CurrentPoint = LastPoint;
407       Du = U2-U1;
408       Dusave = Du;
409     }
410     else D0 (C, U2, CurrentPoint);           //Point suivant
411
412     Standard_Real Coef = 0.0, ACoef = 0., FCoef = 0.;
413     Standard_Boolean Correction, TooLarge, TooSmall;
414     TooLarge   = Standard_False;
415     Correction = Standard_True;
416     TooSmall = Standard_False;
417
418     while (Correction) {                     //Ajustement Du
419       if (isNeedToCheck)
420       {
421         aIdx[1] = getIntervalIdx(U2, Intervs, aIdx[0]);
422         if (aIdx[1] > aIdx[0]) // Jump to another polynom.
423         {
424           if (Du > (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3) // Set Du to the smallest value and check deflection on it.
425           {
426             Du = (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3;
427             U2 = U1 + Du;
428             if (U2 > lastu)
429               U2 = lastu;
430             D0 (C, U2, CurrentPoint);
431           }
432         }
433       }
434       MiddleU = (U1+U2)*0.5;                 //Verif / au point milieu
435       D0 (C, MiddleU, MiddlePoint);
436
437       V1 = (CurrentPoint.XYZ() - aPrevPoint.XYZ()); //Critere de fleche
438       V2 = (MiddlePoint.XYZ()  - aPrevPoint.XYZ());
439       L1 = V1.Modulus ();
440
441       FCoef = (L1 > myMinLen) ? 
442         V1.CrossMagnitude(V2)/(L1*curvatureDeflection) : 0.0;
443
444       V1 = (CurrentPoint.XYZ() - MiddlePoint.XYZ()); //Critere d'angle
445       L1 = V1.Modulus ();
446       L2 = V2.Modulus ();
447       if (L1 > myMinLen && L2 > myMinLen)
448       {
449         Standard_Real angg = V1.CrossMagnitude(V2) / (L1 * L2);
450         ACoef = angg / AngleMax;
451       }
452       else
453         ACoef = 0.0;
454
455       //On retient le plus penalisant
456       Coef = Max(ACoef, FCoef);
457
458       if (isNeedToCheck && Coef < 0.55)
459       {
460         isNeedToCheck = Standard_False;
461         Du = Dusave;
462         U2 = U1 + Du;
463         if (U2 > lastu)
464           U2 = lastu;
465         D0 (C, U2, CurrentPoint);
466         continue;
467       }
468
469       if (Coef <= 1.0) {
470         if (Abs (lastu-U2) < uTol) {
471           parameters.Append (lastu);
472           points    .Append (LastPoint);
473           MorePoints = Standard_False;
474           Correction = Standard_False;
475         }
476         else {
477           if (Coef >= 0.55 || TooLarge) { 
478             parameters.Append (U2);
479             points    .Append (CurrentPoint);
480             aPrevPoint = CurrentPoint;
481             Correction = Standard_False;
482             isNeedToCheck = Standard_True;
483           }
484           else if (TooSmall) {
485             Correction = Standard_False;
486             aPrevPoint = CurrentPoint;
487           }
488           else {
489             TooSmall = Standard_True;
490             //Standard_Real UUU2 = U2;
491             Du += Min((U2-U1)*(1.-Coef), Du*Us3);
492
493             U2 = U1 + Du;
494             if (U2 > lastu)
495               U2 = lastu;
496             D0 (C, U2, CurrentPoint);
497           }
498         }
499       }
500       else {
501
502         if (Coef >= 1.5) {
503           if (!aPrevPoint.IsEqual(points.Last(), Precision::Confusion()))
504           {
505             parameters.Append (U1);
506             points    .Append (aPrevPoint);
507           }
508           U2 = MiddleU;
509           Du  = U2-U1;
510           CurrentPoint = MiddlePoint;
511         }
512         else {
513           Du*=0.9;
514           U2 = U1 + Du;
515           D0 (C, U2, CurrentPoint);
516           TooLarge = Standard_True;
517         }
518
519       }
520     }
521     
522     Du  = U2-U1;
523
524     if (MorePoints) {
525       if (U1 > firstu) {
526         if (FCoef > ACoef) {
527           //La fleche est critere de decoupage
528           EvaluateDu (C, U2, CurrentPoint, Du, NotDone);
529           if (NotDone) {
530             Du += (Du-Dusave)*(Du/Dusave);
531             if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
532             if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
533           }
534         }
535         else {
536           //L'angle est le critere de decoupage
537           Du += (Du-Dusave)*(Du/Dusave);
538           if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
539           if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
540         }
541       }
542       
543       if (Du < uTol) {
544         Du = lastu - U2;
545         if (Du < uTol) {
546           parameters.Append (lastu);
547           points    .Append (LastPoint);
548           MorePoints = Standard_False;
549         }
550         else if (Du*Us3 > uTol) Du*=Us3;
551       }
552       U1 = U2;
553       Dusave = Du;
554     }
555   }
556   //Recalage avant dernier point :
557   i = points.Length()-1;
558 //  Real d = points (i).Distance (points (i+1));
559 // if (Abs(parameters (i) - parameters (i+1))<= 0.000001 || d < Precision::Confusion()) {
560 //    cout<<"deux points confondus"<<endl;
561 //    parameters.Remove (i+1);
562 //    points.Remove (i+1);
563 //    i--;
564 //  }
565   if (i >= 2) {
566     MiddleU = parameters (i-1);
567     MiddleU = (lastu + MiddleU)*0.5;
568     D0 (C, MiddleU, MiddlePoint);
569     parameters.SetValue (i, MiddleU);
570     points    .SetValue (i, MiddlePoint);
571   }
572
573   //-- On rajoute des points aux milieux des segments si le nombre
574   //-- mini de points n'est pas atteint
575   //--
576   Standard_Integer Nbp = points.Length();
577
578   //std::cout << "GCPnts_TangentialDeflection: Number of Points (" << Nbp << " " << minNbPnts << " )" << std::endl;
579
580   while(Nbp < minNbPnts)
581   { 
582     for (i = 2; i <= Nbp; i += 2)
583     {
584       MiddleU = (parameters.Value(i-1)+parameters.Value(i))*0.5;
585       D0 (C, MiddleU, MiddlePoint); 
586       parameters.InsertBefore(i,MiddleU);
587       points.InsertBefore(i,MiddlePoint);
588       Nbp++;
589     }
590   }
591   //Additional check for intervals
592   Standard_Real MinLen2 = myMinLen * myMinLen;
593   Standard_Integer MaxNbp = 10 * Nbp;
594   for(i = 1; i < Nbp; ++i)
595   {
596     U1 = parameters(i);
597     U2 = parameters(i + 1);
598
599     if (U2 - U1 <= uTol)
600     {
601       continue;
602     }
603
604     // Check maximal deflection on interval;
605     Standard_Real dmax = 0.;
606     Standard_Real umax = 0.;
607     Standard_Real amax = 0.;
608     EstimDefl(C, U1, U2, dmax, umax);
609     const gp_Pnt& P1 = points(i);
610     const gp_Pnt& P2 = points(i+1);
611     D0(C, umax, MiddlePoint);
612     amax = EstimAngl(P1, MiddlePoint, P2);
613     if(dmax > curvatureDeflection || amax > AngleMax)
614     {
615       if(umax - U1 > uTol && U2 - umax > uTol)
616       {
617         if (P1.SquareDistance(MiddlePoint) > MinLen2 && P2.SquareDistance(MiddlePoint) > MinLen2)
618         {
619           parameters.InsertAfter(i, umax);
620           points.InsertAfter(i, MiddlePoint);
621           ++Nbp;
622           --i; //To compensate ++i in loop header: i must point to first part of splitted interval
623           if(Nbp > MaxNbp)
624           {
625             break;
626           }
627         }
628       }
629     }
630   }
631   // 
632 }
633
634 //=======================================================================
635 //function : EstimDefl
636 //purpose  : Estimation of maximal deflection for interval [U1, U2]
637 //           
638 //=======================================================================
639 void GCPnts_TangentialDeflection::EstimDefl (const TheCurve& C,
640                             const Standard_Real U1, const Standard_Real U2, 
641                             Standard_Real& MaxDefl, Standard_Real& UMax)
642 {
643   Standard_Real Du = (lastu - firstu);
644   //
645   TheMaxCurvLinDist aFunc(C, U1, U2);
646   //
647   const Standard_Integer aNbIter = 100;
648   Standard_Real reltol = Max(1.e-3, 2.*uTol/((Abs(U1) + Abs(U2))));
649   //
650   math_BrentMinimum anOptLoc(reltol, aNbIter, uTol);
651   anOptLoc.Perform(aFunc, U1, (U1+U2)/2., U2);
652   if(anOptLoc.IsDone())
653   {
654     MaxDefl = Sqrt(-anOptLoc.Minimum());
655     UMax = anOptLoc.Location();
656     return;
657   }
658   //
659   math_Vector aLowBorder(1,1);
660   math_Vector aUppBorder(1,1);
661   math_Vector aSteps(1,1);
662   //
663   aSteps(1) = Max(0.1 * Du, 100. * uTol);
664   Standard_Integer aNbParticles = Max(8, RealToInt(32 * (U2 - U1) / Du));
665   //
666   aLowBorder(1) = U1;
667   aUppBorder(1) = U2;
668   //
669   //
670   Standard_Real aValue;
671   math_Vector aT(1,1);
672   TheMaxCurvLinDistMV aFuncMV(aFunc);
673
674   math_PSO aFinder(&aFuncMV, aLowBorder, aUppBorder, aSteps, aNbParticles); 
675   aFinder.Perform(aSteps, aValue, aT);
676   //
677   anOptLoc.Perform(aFunc, Max(aT(1) - aSteps(1), U1) , aT(1), Min(aT(1) + aSteps(1),U2));
678   if(anOptLoc.IsDone())
679   {
680     MaxDefl = Sqrt(-anOptLoc.Minimum());
681     UMax = anOptLoc.Location();
682     return;
683   }
684   MaxDefl = Sqrt(-aValue);
685   UMax = aT(1);
686   //
687 }