0022383: Four and more points at least on curve
[occt.git] / src / GCPnts / GCPnts_TangentialDeflection.gxx
1 // File:    GCPnts_TangentialDeflection.gxx
2 // Created: Fri Nov  8 11:26:11 1996
3 // Author:  Jean Claude VAUTHIER
4 //    <jcv@brunox.paris1.matra-dtv.fr>
5
6 #include <GCPnts_DeflectionType.hxx>
7 #include <Standard_ConstructionError.hxx>
8 #include <Precision.hxx>
9 #include <gp_XYZ.hxx>
10 #include <gp_Pnt.hxx>
11 #include <gp_Vec.hxx>
12 #include <gp.hxx>
13
14 #define Us3 0.3333333333333333333333333333
15
16 void GCPnts_TangentialDeflection::EvaluateDu (
17   const TheCurve& C,
18   const Standard_Real      U,
19   gp_Pnt&   P,
20   Standard_Real&     Du,
21   Standard_Boolean&  NotDone) const {
22
23   gp_Vec T, N;
24   D2 (C, U, P, T, N);
25   Standard_Real Lt   = T.Magnitude ();
26   Standard_Real LTol = Precision::Confusion ();
27   if (Lt > LTol && N.Magnitude () > LTol) {
28     Standard_Real Lc = N.CrossMagnitude (T);
29     Standard_Real Ln = Lc/Lt;
30     if (Ln > LTol) {
31       Du = sqrt (8.0 * curvatureDeflection / Ln);
32       NotDone = Standard_False;
33     }
34   }
35 }
36
37
38 //=======================================================================
39 //function : GCPnts_TangentialDeflection
40 //purpose  : 
41 //=======================================================================
42
43 GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
44  const TheCurve&        C,
45  const Standard_Real    AngularDeflection,
46  const Standard_Real    CurvatureDeflection,
47  const Standard_Integer MinimumOfPoints,
48  const Standard_Real    UTol)
49
50
51   Initialize (C,AngularDeflection,CurvatureDeflection,MinimumOfPoints,UTol); 
52 }
53
54
55 //=======================================================================
56 //function : GCPnts_TangentialDeflection
57 //purpose  : 
58 //=======================================================================
59
60 GCPnts_TangentialDeflection::GCPnts_TangentialDeflection (
61  const TheCurve&        C,
62  const Standard_Real    FirstParameter,
63  const Standard_Real    LastParameter,
64  const Standard_Real    AngularDeflection,
65  const Standard_Real    CurvatureDeflection,
66  const Standard_Integer MinimumOfPoints,
67  const Standard_Real    UTol)
68
69
70   Initialize (C, 
71         FirstParameter, 
72         LastParameter,
73         AngularDeflection, 
74         CurvatureDeflection, 
75         MinimumOfPoints,
76         UTol);
77 }
78
79
80
81 //=======================================================================
82 //function : Initialize
83 //purpose  : 
84 //=======================================================================
85
86 void GCPnts_TangentialDeflection::Initialize (
87  const TheCurve&        C, 
88  const Standard_Real    AngularDeflection, 
89  const Standard_Real    CurvatureDeflection,
90  const Standard_Integer MinimumOfPoints,
91  const Standard_Real    UTol)
92
93 {
94   Initialize (C, 
95               C.FirstParameter (), 
96               C.LastParameter (),
97               AngularDeflection, 
98               CurvatureDeflection,
99               MinimumOfPoints,
100               UTol);
101 }
102
103
104 //=======================================================================
105 //function : Initialize
106 //purpose  : 
107 //=======================================================================
108
109 void GCPnts_TangentialDeflection::Initialize (
110  const TheCurve&        C, 
111  const Standard_Real    FirstParameter, 
112  const Standard_Real    LastParameter,
113  const Standard_Real    AngularDeflection, 
114  const Standard_Real    CurvatureDeflection,
115  const Standard_Integer MinimumOfPoints,
116  const Standard_Real    UTol)
117
118 {
119   
120   Standard_ConstructionError_Raise_if (CurvatureDeflection <= Precision::Confusion () || AngularDeflection   <= Precision::Angular (), "GCPnts_TangentialDeflection::Initialize - Zero Deflection")
121
122  parameters.Clear ();
123  points    .Clear ();
124  if (FirstParameter < LastParameter) {
125    firstu = FirstParameter;
126    lastu  = LastParameter;
127  }
128  else {
129    lastu  = FirstParameter;
130    firstu = LastParameter;
131  }
132  uTol                = UTol;
133  angularDeflection   = AngularDeflection;
134  curvatureDeflection = CurvatureDeflection;
135  minNbPnts           = Max (MinimumOfPoints, 2);
136
137  switch (C.GetType()) {
138
139  case GeomAbs_Line:   
140    PerformLinear (C);
141    break;
142
143  case GeomAbs_Circle: 
144    PerformCircular (C);
145    break;
146
147  case GeomAbs_BSplineCurve:
148    {
149      Handle_TheBSplineCurve BS = C.BSpline() ;
150      if (BS->NbPoles() == 2 ) PerformLinear (C);
151      else                     PerformCurve  (C);
152      break;
153    }
154  case GeomAbs_BezierCurve:
155    {
156      Handle_TheBezierCurve  BZ = C.Bezier(); 
157      if (BZ->NbPoles() == 2) PerformLinear (C);
158      else                    PerformCurve  (C);
159      break;
160    }
161  default: PerformCurve (C);
162    
163  }
164 }
165
166
167 //=======================================================================
168 //function : PerformLinear
169 //purpose  : 
170 //=======================================================================
171
172 void GCPnts_TangentialDeflection::PerformLinear (const TheCurve& C) {
173
174   gp_Pnt P;
175   D0 (C, firstu, P);
176   parameters.Append (firstu);
177   points    .Append (P);
178   if (minNbPnts > 2) {
179     Standard_Real Du = (lastu - firstu) / minNbPnts;
180     Standard_Real U = firstu + Du;
181     for (Standard_Integer i = 2; i <= minNbPnts; i++) {
182       D0 (C, U, P);
183       parameters.Append (U);
184       points    .Append (P);
185       U += Du;
186     }
187   }
188   D0 (C, lastu, P);
189   parameters.Append (lastu);
190   points    .Append (P);
191 }
192
193
194 //=======================================================================
195 //function : PerformCircular
196 //purpose  : 
197 //=======================================================================
198
199 void GCPnts_TangentialDeflection::PerformCircular (const TheCurve& C) 
200 {
201   // akm 8/01/02 : check the radius before divide by it
202   Standard_Real dfR = C.Circle().Radius();
203   Standard_Real Du = 0.;
204   if (Abs(dfR) > Precision::Confusion())
205     Du = Max(1.0e0 - (curvatureDeflection/dfR),0.0e0) ;
206   Du  = acos (Du); Du+=Du;
207   Du               = Min (Du, angularDeflection);
208   Standard_Integer NbPoints = (Standard_Integer )((lastu - firstu) / Du);
209   NbPoints         = Max (NbPoints, minNbPnts-1);
210   Du               = (lastu - firstu) / NbPoints;
211
212   gp_Pnt P;
213   Standard_Real U = firstu;
214   for (Standard_Integer i = 1; i <= NbPoints; i++) {
215     D0 (C, U, P);
216     parameters.Append (U);
217     points    .Append (P);
218     U += Du;
219   }
220   D0 (C, lastu, P);
221   parameters.Append (lastu);
222   points    .Append (P);
223 }
224
225
226
227 //=======================================================================
228 //function : PerformCurve
229 //purpose  : On respecte ll'angle et la fleche, on peut imposer un nombre
230 //           minimum de points sur un element lineaire
231 //=======================================================================
232
233 void GCPnts_TangentialDeflection::PerformCurve (const TheCurve& C)
234
235 {
236   Standard_Integer i;       
237   gp_XYZ V1, V2;
238   gp_Pnt MiddlePoint, CurrentPoint, LastPoint;   
239   Standard_Real Du, Dusave, MiddleU, L1, L2;
240
241   Standard_Real U1       = firstu;   
242   Standard_Real LTol     = Precision::Confusion ();  //protection longueur nulle
243   Standard_Real ATol     = Precision::Angular ();    //protection angle nul
244
245   D0 (C, lastu, LastPoint);
246
247   //Initialization du calcul
248
249   Standard_Boolean NotDone = Standard_True;
250   Dusave = (lastu - firstu)*Us3;
251   Du     = Dusave;
252   EvaluateDu (C, U1, CurrentPoint, Du, NotDone);
253   parameters.Append (U1);
254   points    .Append (CurrentPoint);
255
256   if (NotDone) {
257     //C'est soit une droite, soit une singularite :
258     V1 = LastPoint.XYZ ();
259     V1.Subtract (CurrentPoint.XYZ());
260     L1 = V1.Modulus ();
261     if (L1 > LTol) {
262       //Si c'est une droite on verifie en calculant minNbPoints :
263       Standard_Boolean IsLine   = Standard_True;
264       Standard_Integer NbPoints = 3;
265       if (minNbPnts > 3) NbPoints = minNbPnts;
266       Du = (lastu-firstu)/NbPoints;
267       MiddleU = firstu + Du;
268       for (i = 2; i < NbPoints; i++) {
269         D0 (C, MiddleU, MiddlePoint);
270         V2 = MiddlePoint.XYZ();
271         V2.Subtract (CurrentPoint.XYZ());
272         L2 = V2.Modulus ();
273         if (L2 > LTol) {
274           if (((V2.CrossMagnitude (V1))/(L1*L2)) >= ATol) {
275             //C'etait une singularite
276             IsLine = Standard_False;
277             break;
278           }
279           if (minNbPnts > 2) {
280             parameters.Append (MiddleU);
281             points    .Append (MiddlePoint);
282           }
283         }
284         MiddleU += Du;
285       }
286       if (IsLine) {
287         //C'etait une droite (plusieurs poles alignes), Calcul termine :
288         parameters.Append (lastu);
289         points    .Append (LastPoint);
290         return;
291       }
292       else {
293         //c'etait une singularite on continue :
294         Standard_Integer pointsLength=points.Length ();
295         for (i = 2; i <= pointsLength; i++) {
296           points    .Remove (i);
297           parameters.Remove (i);
298           pointsLength--;
299         }
300         Du = Dusave;
301       }
302     }
303     else  {
304       
305       Du = (lastu-firstu)/2.1;
306       MiddleU = firstu + Du;
307       D0 (C, MiddleU, MiddlePoint);
308       V1 = MiddlePoint.XYZ ();
309       V1.Subtract (CurrentPoint.XYZ());
310       L1 = V1.Modulus ();
311       if (L1 < LTol) {
312         // L1 < LTol C'est une courbe de longueur nulle, calcul termine :
313         // on renvoi un segment de 2 points   (protection)
314         parameters.Append (lastu);
315         points    .Append (LastPoint);
316         return;
317       }
318     }
319   }
320
321   if (Du > Dusave) Du = Dusave;
322   else             Dusave = Du;
323
324   if (Du < uTol) {
325     Du = lastu - firstu;
326     if (Du < uTol) {
327       parameters.Append (lastu);
328       points    .Append (LastPoint);
329       return;
330     }
331   }
332
333   //Traitement normal pour une courbe
334   Standard_Boolean MorePoints = Standard_True;
335   Standard_Real U2            = firstu;   
336   Standard_Real AngleMax      = angularDeflection * 0.5;  //car on prend le point milieu
337
338   gp_Pnt aPrevPoint = points.Last();
339
340   while (MorePoints) {
341
342     U2 += Du;                             
343
344     if (U2 >= lastu) {                       //Bout de courbe
345       U2 = lastu;
346       CurrentPoint = LastPoint;
347       Du = U2-U1;
348       Dusave = Du;
349     }
350     else D0 (C, U2, CurrentPoint);           //Point suivant
351
352     Standard_Real Coef, ACoef, FCoef;
353     Standard_Boolean Correction, TooLarge, TooSmall;
354     TooLarge   = Standard_False;
355     TooSmall   = Standard_False;
356     Correction = Standard_True;
357
358     Standard_Real lastCoef = 0;
359      
360     while (Correction) {                     //Ajustement Du
361       MiddleU = (U1+U2)*0.5;                 //Verif / au point milieu
362       D0 (C, MiddleU, MiddlePoint);
363
364       V1 = CurrentPoint.XYZ ();              //Critere de fleche
365       V1.Subtract (aPrevPoint.XYZ());
366       V2 = MiddlePoint.XYZ ();
367       V2.Subtract (aPrevPoint.XYZ());
368       L1 = V1.Modulus ();
369       if (L1 > LTol) FCoef = V1.CrossMagnitude(V2)/(L1*curvatureDeflection);
370       else           FCoef = 0.0;
371
372       V1 = CurrentPoint.XYZ ();              //Critere d'angle
373       V1.Subtract (MiddlePoint.XYZ ());
374       L1 = V1.Modulus ();
375       L2 = V2.Modulus ();
376       Standard_Real angg = V1.CrossMagnitude(V2)/(L1*L2);
377       if (L1 > LTol && L2 > LTol) ACoef = angg/AngleMax;
378       else                        ACoef = 0.0;
379
380       if (ACoef >= FCoef) Coef = ACoef;      //On retient le plus penalisant
381       else                Coef = FCoef;
382
383
384       if (Coef <= 1.0) {
385         if (Abs (lastu-U2) < uTol) {
386           parameters.Append (lastu);
387           points    .Append (LastPoint);
388           MorePoints = Standard_False;
389           Correction = Standard_False;
390         }
391         else {
392           if (Coef >= 0.55 || TooLarge) { 
393             parameters.Append (U2);
394             points    .Append (CurrentPoint);
395             aPrevPoint = CurrentPoint;
396             Correction = Standard_False;
397           }
398           else if (TooSmall) {
399             Correction = Standard_False;
400             aPrevPoint = CurrentPoint;
401           }
402           else {
403             TooSmall = Standard_True;
404             lastCoef = Coef;
405             //Standard_Real UUU2 = U2;
406             Du += Min((U2-U1)*(1.-Coef), Du*Us3);
407
408             U2 = U1 + Du;
409             //if (U2 >= lastu) U2 = UUU2;
410             if (U2 >= lastu) {
411               parameters.Append (lastu);
412               points    .Append (LastPoint);
413               MorePoints = Standard_False;
414               Correction = Standard_False;
415             }
416             else D0 (C, U2, CurrentPoint);
417           }
418         }
419       }
420       else {
421
422         if (Coef >= 1.5) {
423           U2 = MiddleU;
424           CurrentPoint = MiddlePoint;
425         }
426         else {
427           Du*=0.9;
428           U2 = U1 + Du;
429           D0 (C, U2, CurrentPoint);
430           TooLarge = Standard_True;
431         }
432
433       }
434     }
435     
436     Du  = U2-U1;
437
438     if (MorePoints) {
439       if (U1 > firstu) {
440         if (FCoef > ACoef) {
441           //La fleche est critere de decoupage
442           EvaluateDu (C, U2, CurrentPoint, Du, NotDone);
443           if (NotDone) {
444             Du += (Du-Dusave)*(Du/Dusave);
445             if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
446             if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
447           }
448         }
449         else {
450           //L'angle est le critere de decoupage
451           Du += (Du-Dusave)*(Du/Dusave);
452           if (Du > 1.5 * Dusave) Du = 1.5  * Dusave;
453           if (Du < 0.75* Dusave) Du = 0.75 * Dusave;
454         }
455       }
456       
457       if (Du < uTol) {
458         Du = lastu - U2;
459         if (Du < uTol) {
460           parameters.Append (lastu);
461           points    .Append (LastPoint);
462           MorePoints = Standard_False;
463         }
464         else if (Du*Us3 > uTol) Du*=Us3;
465       }
466       U1 = U2;
467       Dusave = Du;
468     }
469   }
470   //Recalage avant dernier point :
471   i = points.Length()-1;
472 //  Real d = points (i).Distance (points (i+1));
473 // if (Abs(parameters (i) - parameters (i+1))<= 0.000001 || d < Precision::Confusion()) {
474 //    cout<<"deux points confondus"<<endl;
475 //    parameters.Remove (i+1);
476 //    points.Remove (i+1);
477 //    i--;
478 //  }
479   
480   if (i >= 2) {
481     MiddleU = parameters (i-1);
482     MiddleU = (lastu + MiddleU)*0.5;
483     D0 (C, MiddleU, MiddlePoint);
484     parameters.SetValue (i, MiddleU);
485     points    .SetValue (i, MiddlePoint);
486   }
487
488   //-- On rajoute des points aux milieux des segments si le nombre
489   //-- mini de points n'est pas atteint
490   //--
491   Standard_Integer Nbp =  points.Length();
492   Standard_Integer MinNb= (9*minNbPnts)/10;
493   //if(MinNb<4) MinNb=4;
494
495   //-- if(Nbp <  MinNb) { cout<<"\n*"; } else {  cout<<"\n."; } 
496   while(Nbp < MinNb) { 
497     //-- cout<<" \nGCPnts TangentialDeflection : Ajout de Points ("<<Nbp<<" "<<minNbPnts<<" )"<<endl;
498     for(i=2; i<=Nbp; i++) { 
499       MiddleU = (parameters.Value(i-1)+parameters.Value(i))*0.5;
500       D0 (C, MiddleU, MiddlePoint); 
501       parameters.InsertBefore(i,MiddleU);
502       points.InsertBefore(i,MiddlePoint);
503       Nbp++;
504       i++;
505     }
506   }
507 }