c32e4a08083c97cec0a2b3aa4fc76a2cd7791c87
[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     Standard_Real Du = (lastu - firstu) / minNbPnts;
201     Standard_Real U = firstu + Du;
202     for (Standard_Integer i = 2; i <= minNbPnts; i++) {
203       D0 (C, U, P);
204       parameters.Append (U);
205       points    .Append (P);
206       U += Du;
207     }
208   }
209   D0 (C, lastu, P);
210   parameters.Append (lastu);
211   points    .Append (P);
212 }
213
214 //=======================================================================
215 //function : PerformCircular
216 //purpose  : 
217 //=======================================================================
218
219 void GCPnts_TangentialDeflection::PerformCircular (const TheCurve& C) 
220 {
221   // akm 8/01/02 : check the radius before divide by it
222   Standard_Real dfR = C.Circle().Radius();
223   Standard_Real Du = GCPnts_TangentialDeflection::ArcAngularStep(
224     dfR, curvatureDeflection, angularDeflection, myMinLen);
225     
226   const Standard_Real aDiff = lastu - firstu;
227   // Round up number of points to satisfy curvatureDeflection more precisely
228   Standard_Integer NbPoints = (Standard_Integer)Min(Ceiling(aDiff / Du), 1.0e+6);
229   NbPoints = Max(NbPoints, minNbPnts - 1);
230   Du       = aDiff / NbPoints;
231
232   gp_Pnt P;
233   Standard_Real U = firstu;
234   for (Standard_Integer i = 1; i <= NbPoints; i++)
235   {
236     D0 (C, U, P);
237     parameters.Append (U);
238     points    .Append (P);
239     U += Du;
240   }
241   D0 (C, lastu, P);
242   parameters.Append (lastu);
243   points    .Append (P);
244 }
245
246
247 //=======================================================================
248 //function : PerformCurve
249 //purpose  : On respecte ll'angle et la fleche, on peut imposer un nombre
250 //           minimum de points sur un element lineaire
251 //=======================================================================
252 void GCPnts_TangentialDeflection::PerformCurve (const TheCurve& C)
253
254 {
255   Standard_Integer i, j;       
256   gp_XYZ V1, V2;
257   gp_Pnt MiddlePoint, CurrentPoint, LastPoint;   
258   Standard_Real Du, Dusave, MiddleU, L1, L2;
259
260   Standard_Real U1       = firstu;   
261   Standard_Real LTol     = Precision::Confusion ();  //protection longueur nulle
262   Standard_Real ATol     = 1.e-2 * angularDeflection;
263   if(ATol > 1.e-2)
264     ATol = 1.e-2;
265   else if(ATol < 1.e-7)
266     ATol = 1.e-7;
267
268   D0 (C, lastu, LastPoint);
269
270   //Initialization du calcul
271
272   Standard_Boolean NotDone = Standard_True;
273   Dusave = (lastu - firstu)*Us3;
274   Du     = Dusave;
275   EvaluateDu (C, U1, CurrentPoint, Du, NotDone);
276   parameters.Append (U1);
277   points    .Append (CurrentPoint);
278
279   // Used to detect "isLine" current bspline and in Du computation in general handling.
280   Standard_Integer NbInterv = C.NbIntervals(GeomAbs_CN);
281   TColStd_Array1OfReal Intervs(1, NbInterv+1);
282   C.Intervals(Intervs, GeomAbs_CN);
283
284   if (NotDone || Du > 5. * Dusave) {
285     //C'est soit une droite, soit une singularite :
286     V1 = (LastPoint.XYZ() - CurrentPoint.XYZ());
287     L1 = V1.Modulus ();
288     if (L1 > LTol)
289     {
290       //Si c'est une droite on verifie en calculant minNbPoints :
291       Standard_Boolean IsLine   = Standard_True;
292       Standard_Integer NbPoints = (minNbPnts > 3) ? minNbPnts : 3;
293       switch (C.GetType()) {
294       case GeomAbs_BSplineCurve:
295         {
296           Handle_TheBSplineCurve BS = C.BSpline() ;
297           NbPoints = Max(BS->Degree() + 1, NbPoints);
298           break;
299         }
300       case GeomAbs_BezierCurve:
301         {
302           Handle_TheBezierCurve  BZ = C.Bezier();
303           NbPoints = Max(BZ->Degree() + 1, NbPoints);
304           break;
305         }
306       default:
307       ;}
308       ////
309       Standard_Real param = 0.;
310       for (i = 1; i <= NbInterv && IsLine; ++i)
311       {
312         // Avoid usage intervals out of [firstu, lastu].
313         if ((Intervs(i+1) < firstu) ||
314             (Intervs(i)   > lastu))
315         {
316           continue;
317         }
318         // Fix border points in applicable intervals, to avoid be out of target interval.
319         if ((Intervs(i)   < firstu) &&
320             (Intervs(i+1) > firstu))
321         {
322           Intervs(i) = firstu;
323         }
324         if ((Intervs(i)   < lastu) &&
325             (Intervs(i+1) > lastu))
326         {
327           Intervs(i + 1) = lastu;
328         }
329
330         Standard_Real delta = (Intervs(i+1) - Intervs(i))/NbPoints;
331         for (j = 1; j <= NbPoints && IsLine; ++j)
332         {
333           param = Intervs(i) + j*delta;
334           D0 (C, param, MiddlePoint);
335           V2 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
336           L2 = V2.Modulus ();
337           if (L2 > LTol)
338           {
339             const Standard_Real aAngle = V2.CrossMagnitude(V1)/(L1*L2);
340             IsLine = (aAngle < ATol);
341           }
342         }
343       }
344
345       if (IsLine)
346       {
347         parameters.Clear();
348         points    .Clear();
349
350         PerformLinear(C);
351         return;
352       }
353       else
354       {
355         //c'etait une singularite on continue :
356         //Du = Dusave;
357         EvaluateDu (C, param, MiddlePoint, Du, NotDone);
358       }
359     }
360     else
361     {
362       Du = (lastu-firstu)/2.1;
363       MiddleU = firstu + Du;
364       D0 (C, MiddleU, MiddlePoint);
365       V1 = (MiddlePoint.XYZ() - CurrentPoint.XYZ());
366       L1 = V1.Modulus ();
367       if (L1 < LTol)
368       {
369         // L1 < LTol C'est une courbe de longueur nulle, calcul termine :
370         // on renvoi un segment de 2 points   (protection)
371         parameters.Append (lastu);
372         points    .Append (LastPoint);
373         return;
374       }
375     }
376   }
377
378   if (Du > Dusave) Du = Dusave;
379   else             Dusave = Du;
380
381   if (Du < uTol) {
382     Du = lastu - firstu;
383     if (Du < uTol) {
384       parameters.Append (lastu);
385       points    .Append (LastPoint);
386       return;
387     }
388   }
389
390   //Traitement normal pour une courbe
391   Standard_Boolean MorePoints = Standard_True;
392   Standard_Real U2            = firstu;   
393   Standard_Real AngleMax      = angularDeflection * 0.5;  //car on prend le point milieu
394   Standard_Integer aIdx[2] = {Intervs.Lower(), Intervs.Lower()}; // Indexes of intervals of U1 and U2, used to handle non-uniform case.
395   Standard_Boolean isNeedToCheck = Standard_False;
396   gp_Pnt aPrevPoint = points.Last();
397
398   while (MorePoints) {
399     aIdx[0] = getIntervalIdx(U1, Intervs, aIdx[0]);
400     U2 += Du;
401
402     if (U2 >= lastu) {                       //Bout de courbe
403       U2 = lastu;
404       CurrentPoint = LastPoint;
405       Du = U2-U1;
406       Dusave = Du;
407     }
408     else D0 (C, U2, CurrentPoint);           //Point suivant
409
410     Standard_Real Coef = 0.0, ACoef = 0., FCoef = 0.;
411     Standard_Boolean Correction, TooLarge, TooSmall;
412     TooLarge   = Standard_False;
413     Correction = Standard_True;
414     TooSmall = Standard_False;
415
416     while (Correction) {                     //Ajustement Du
417       if (isNeedToCheck)
418       {
419         aIdx[1] = getIntervalIdx(U2, Intervs, aIdx[0]);
420         if (aIdx[1] > aIdx[0]) // Jump to another polynom.
421         {
422           if (Du > (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3) // Set Du to the smallest value and check deflection on it.
423           {
424             Du = (Intervs(aIdx[0] + 1) - Intervs(aIdx[0]) ) * Us3;
425             U2 = U1 + Du;
426             if (U2 > lastu)
427               U2 = lastu;
428             D0 (C, U2, CurrentPoint);
429           }
430         }
431       }
432       MiddleU = (U1+U2)*0.5;                 //Verif / au point milieu
433       D0 (C, MiddleU, MiddlePoint);
434
435       V1 = (CurrentPoint.XYZ() - aPrevPoint.XYZ()); //Critere de fleche
436       V2 = (MiddlePoint.XYZ()  - aPrevPoint.XYZ());
437       L1 = V1.Modulus ();
438
439       FCoef = (L1 > myMinLen) ? 
440         V1.CrossMagnitude(V2)/(L1*curvatureDeflection) : 0.0;
441
442       V1 = (CurrentPoint.XYZ() - MiddlePoint.XYZ()); //Critere d'angle
443       L1 = V1.Modulus ();
444       L2 = V2.Modulus ();
445       if (L1 > myMinLen && L2 > myMinLen)
446       {
447         Standard_Real angg = V1.CrossMagnitude(V2) / (L1 * L2);
448         ACoef = angg / AngleMax;
449       }
450       else
451         ACoef = 0.0;
452
453       //On retient le plus penalisant
454       Coef = Max(ACoef, FCoef);
455
456       if (isNeedToCheck && Coef < 0.55)
457       {
458         isNeedToCheck = Standard_False;
459         Du = Dusave;
460         U2 = U1 + Du;
461         if (U2 > lastu)
462           U2 = lastu;
463         D0 (C, U2, CurrentPoint);
464         continue;
465       }
466
467       if (Coef <= 1.0) {
468         if (Abs (lastu-U2) < uTol) {
469           parameters.Append (lastu);
470           points    .Append (LastPoint);
471           MorePoints = Standard_False;
472           Correction = Standard_False;
473         }
474         else {
475           if (Coef >= 0.55 || TooLarge) { 
476             parameters.Append (U2);
477             points    .Append (CurrentPoint);
478             aPrevPoint = CurrentPoint;
479             Correction = Standard_False;
480             isNeedToCheck = Standard_True;
481           }
482           else if (TooSmall) {
483             Correction = Standard_False;
484             aPrevPoint = CurrentPoint;
485           }
486           else {
487             TooSmall = Standard_True;
488             //Standard_Real UUU2 = U2;
489             Du += Min((U2-U1)*(1.-Coef), Du*Us3);
490
491             U2 = U1 + Du;
492             if (U2 > lastu)
493               U2 = lastu;
494             D0 (C, U2, CurrentPoint);
495           }
496         }
497       }
498       else {
499
500         if (Coef >= 1.5) {
501           if (!aPrevPoint.IsEqual(points.Last(), Precision::Confusion()))
502           {
503             parameters.Append (U1);
504             points    .Append (aPrevPoint);
505           }
506           U2 = MiddleU;
507           Du  = U2-U1;
508           CurrentPoint = MiddlePoint;
509         }
510         else {
511           Du*=0.9;
512           U2 = U1 + Du;
513           D0 (C, U2, CurrentPoint);
514           TooLarge = Standard_True;
515         }
516
517       }
518     }
519     
520     Du  = U2-U1;
521
522     if (MorePoints) {
523       if (U1 > firstu) {
524         if (FCoef > ACoef) {
525           //La fleche est critere de decoupage
526           EvaluateDu (C, U2, CurrentPoint, Du, NotDone);
527           if (NotDone) {
528             Du += (Du-Dusave)*(Du/Dusave);
529             if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
530             if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
531           }
532         }
533         else {
534           //L'angle est le critere de decoupage
535           Du += (Du-Dusave)*(Du/Dusave);
536           if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
537           if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
538         }
539       }
540       
541       if (Du < uTol) {
542         Du = lastu - U2;
543         if (Du < uTol) {
544           parameters.Append (lastu);
545           points    .Append (LastPoint);
546           MorePoints = Standard_False;
547         }
548         else if (Du*Us3 > uTol) Du*=Us3;
549       }
550       U1 = U2;
551       Dusave = Du;
552     }
553   }
554   //Recalage avant dernier point :
555   i = points.Length()-1;
556 //  Real d = points (i).Distance (points (i+1));
557 // if (Abs(parameters (i) - parameters (i+1))<= 0.000001 || d < Precision::Confusion()) {
558 //    cout<<"deux points confondus"<<endl;
559 //    parameters.Remove (i+1);
560 //    points.Remove (i+1);
561 //    i--;
562 //  }
563   if (i >= 2) {
564     MiddleU = parameters (i-1);
565     MiddleU = (lastu + MiddleU)*0.5;
566     D0 (C, MiddleU, MiddlePoint);
567     parameters.SetValue (i, MiddleU);
568     points    .SetValue (i, MiddlePoint);
569   }
570
571   //-- On rajoute des points aux milieux des segments si le nombre
572   //-- mini de points n'est pas atteint
573   //--
574   Standard_Integer Nbp =  points.Length();
575   Standard_Integer MinNb= (9*minNbPnts)/10;
576   //if(MinNb<4) MinNb=4;
577
578   //-- if(Nbp <  MinNb) { cout<<"\n*"; } else {  cout<<"\n."; } 
579   while(Nbp < MinNb) { 
580     //-- cout<<" \nGCPnts TangentialDeflection : Ajout de Points ("<<Nbp<<" "<<minNbPnts<<" )"<<endl;
581     for (i = 2; i <= Nbp; i += 2) {
582       MiddleU = (parameters.Value(i-1)+parameters.Value(i))*0.5;
583       D0 (C, MiddleU, MiddlePoint); 
584       parameters.InsertBefore(i,MiddleU);
585       points.InsertBefore(i,MiddlePoint);
586       Nbp++;
587     }
588   }
589   //Additional check for intervals
590   Standard_Real MinLen2 = myMinLen * myMinLen;
591   Standard_Integer MaxNbp = 10 * Nbp;
592   for(i = 1; i < Nbp; ++i)
593   {
594     U1 = parameters(i);
595     U2 = parameters(i + 1);
596
597     if (U2 - U1 <= uTol)
598     {
599       continue;
600     }
601
602     // Check maximal deflection on interval;
603     Standard_Real dmax = 0.;
604     Standard_Real umax = 0.;
605     Standard_Real amax = 0.;
606     EstimDefl(C, U1, U2, dmax, umax);
607     const gp_Pnt& P1 = points(i);
608     const gp_Pnt& P2 = points(i+1);
609     D0(C, umax, MiddlePoint);
610     amax = EstimAngl(P1, MiddlePoint, P2);
611     if(dmax > curvatureDeflection || amax > AngleMax)
612     {
613       if(umax - U1 > uTol && U2 - umax > uTol)
614       {
615         if (P1.SquareDistance(MiddlePoint) > MinLen2 && P2.SquareDistance(MiddlePoint) > MinLen2)
616         {
617           parameters.InsertAfter(i, umax);
618           points.InsertAfter(i, MiddlePoint);
619           ++Nbp;
620           --i; //To compensate ++i in loop header: i must point to first part of splitted interval
621           if(Nbp > MaxNbp)
622           {
623             break;
624           }
625         }
626       }
627     }
628   }
629   // 
630 }
631
632 //=======================================================================
633 //function : EstimDefl
634 //purpose  : Estimation of maximal deflection for interval [U1, U2]
635 //           
636 //=======================================================================
637 void GCPnts_TangentialDeflection::EstimDefl (const TheCurve& C,
638                             const Standard_Real U1, const Standard_Real U2, 
639                             Standard_Real& MaxDefl, Standard_Real& UMax)
640 {
641   Standard_Real Du = (lastu - firstu);
642   //
643   TheMaxCurvLinDist aFunc(C, U1, U2);
644   //
645   const Standard_Integer aNbIter = 100;
646   Standard_Real reltol = Max(1.e-3, 2.*uTol/((Abs(U1) + Abs(U2))));
647   //
648   math_BrentMinimum anOptLoc(reltol, aNbIter, uTol);
649   anOptLoc.Perform(aFunc, U1, (U1+U2)/2., U2);
650   if(anOptLoc.IsDone())
651   {
652     MaxDefl = Sqrt(-anOptLoc.Minimum());
653     UMax = anOptLoc.Location();
654     return;
655   }
656   //
657   math_Vector aLowBorder(1,1);
658   math_Vector aUppBorder(1,1);
659   math_Vector aSteps(1,1);
660   //
661   aSteps(1) = Max(0.1 * Du, 100. * uTol);
662   Standard_Integer aNbParticles = Max(8, RealToInt(32 * (U2 - U1) / Du));
663   //
664   aLowBorder(1) = U1;
665   aUppBorder(1) = U2;
666   //
667   //
668   Standard_Real aValue;
669   math_Vector aT(1,1);
670   TheMaxCurvLinDistMV aFuncMV(aFunc);
671
672   math_PSO aFinder(&aFuncMV, aLowBorder, aUppBorder, aSteps, aNbParticles); 
673   aFinder.Perform(aSteps, aValue, aT);
674   //
675   anOptLoc.Perform(aFunc, Max(aT(1) - aSteps(1), U1) , aT(1), Min(aT(1) + aSteps(1),U2));
676   if(anOptLoc.IsDone())
677   {
678     MaxDefl = Sqrt(-anOptLoc.Minimum());
679     UMax = anOptLoc.Location();
680     return;
681   }
682   MaxDefl = Sqrt(-aValue);
683   UMax = aT(1);
684   //
685 }