fa7e62e888de7a53861a5b8f2d38e5fa2d848a25
[occt.git] / src / IntWalk / IntWalk_IWalking_5.gxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and/or modify it under
7 // the terms of the GNU Lesser General Public License version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 namespace {
16   static const Standard_Real CosRef3D = 0.98;// rule by tests in U4 
17                                              // correspond to  11.478 d
18   static const Standard_Real CosRef2D = 0.88; // correspond to 25 d
19   static const Standard_Integer MaxDivision = 60;  // max number of step division 
20                                                    // because the angle is too great in 2d (U4)
21 }
22
23 IntWalk_StatusDeflection IntWalk_IWalking::TestDeflection
24   (TheIWFunction& sp,
25    const Standard_Boolean Finished,
26    const math_Vector& UV,
27    const IntWalk_StatusDeflection StatusPrecedent,
28    Standard_Integer& NbDivision,
29    Standard_Real& Step,
30    const Standard_Integer StepSign)
31 {
32   // Check the step of advancement, AND recalculate this step :
33   //
34   // 1) test point confused
35   //     if yes other tests are not done
36   // 2) test angle 3d too great
37   //     if yes divide the step and leave
38   //     angle3d = angle ((previous point, calculated point),
39   //                       previous tangent)
40   // 3) check step of advancement in 2d
41   // 4) test point confused
42   // 5) test angle 2d too great
43   // 6) test point of tangency 
44   //     if yes leave
45   // 7) calculate the tangent by u,v of the section 
46   // 8) test angle 3d too great  
47   //     angle3d = angle ((previous point, calculated point),  
48   //                       new tangent)
49   // 9) test angle 2d too great
50   //10) test change of side (pass the tangent point not knowing it)
51   //11) calculate the step of advancement depending on the vector
52   //12) adjust the step depending on the previous steps 
53   
54   IntWalk_StatusDeflection Status = IntWalk_OK;
55
56   //---------------------------------------------------------------------------------
57   //-- lbr le 4 Avril 95 : it is possible that the status returns points confused
58   //-- if epsilon is great enough (1e-11). In this case one loops 
59   //-- without ever changing the values sent to Rsnld. 
60   //---------------------------------------------------------------------------------
61   Standard_Real Paramu, Paramv, StepU,StepV;
62   Standard_Real Cosi, Cosi2, Norme;
63
64   gp_Vec Corde(previousPoint.Value(), sp.Point());
65
66   Norme = Corde.SquareMagnitude();
67 //  if (Norme <= epsilon*epsilon) {
68   if ((++NbPointsConfondusConsecutifs < 10) && (Norme <= epsilon)) { // the square is already taken in the constructor
69     Status = IntWalk_PointConfondu;
70     if (StatusPrecedent == IntWalk_PasTropGrand) {
71       return IntWalk_ArretSurPointPrecedent;
72     }
73
74     if(++EpsilonSembleTropGrand > 5   &&  NbPointsConfondusConsecutifs == 8)  {     //--    Temporary 
75       if(epsilon>0.00000000001) epsilon*=0.5;                                       //--    Temporary
76       EpsilonSembleTropGrand = 0;                                                   //--    Temporary 
77     }
78   }
79   else {
80     NbPointsConfondusConsecutifs = 0;   //--    Temporary
81     EpsilonSembleTropGrand = 0;         //--    Temporary
82     if(Norme<1e-16) Norme = 1e-16;      //--    Temporary 
83     
84     Cosi = Corde * previousd3d;
85     if (Cosi*StepSign < 0.) { // angle 3d > pi/2 !!!!
86       Cosi2 = 0.;
87     }
88     else {
89       Cosi2 = Cosi * Cosi / previousd3d.SquareMagnitude() / Norme;
90     }
91     if (Cosi2 < CosRef3D) { //angle 3d too great
92       Step = Step /2.0;
93       StepU = Abs(Step*previousd2d.X());
94       StepV = Abs(Step*previousd2d.Y());
95       if (StepU < tolerance(1) && StepV < tolerance(2)) 
96         Status = IntWalk_ArretSurPointPrecedent;
97       else 
98         Status = IntWalk_PasTropGrand;
99       return Status;
100     }
101   }
102
103   if (!reversed) {
104     previousPoint.ParametersOnS2(Paramu, Paramv);
105   }
106   else {
107     previousPoint.ParametersOnS1(Paramu, Paramv);
108   }
109   Standard_Real Du = UV(1) - Paramu;
110   Standard_Real Dv = UV(2) - Paramv;
111   Standard_Real Duv = Du * Du + Dv * Dv;
112   if (Abs(Du) < tolerance(1) && Abs(Dv) < tolerance(2))
113     return IntWalk_ArretSurPointPrecedent; //confused point 2d
114   Cosi = StepSign * (Du * previousd2d.X() + 
115                      Dv * previousd2d.Y());
116   if (Cosi < 0 && Status == IntWalk_PointConfondu) 
117     return IntWalk_ArretSurPointPrecedent; // leave as step back  
118                                            // with confused point
119
120
121   if (sp.IsTangent()) 
122     return IntWalk_ArretSurPoint;       
123
124 //if during routing one has subdivided more than  MaxDivision for each
125 //previous step, bug on the square; do nothing (experience U4)
126
127   if (NbDivision < MaxDivision && 
128       Status != IntWalk_PointConfondu && 
129       StatusPrecedent!= IntWalk_PointConfondu ) {
130     Cosi2 = Cosi * Cosi / Duv;
131     if (Cosi2 < CosRef2D || Cosi < 0  ) {
132       Step = Step / 2.0;
133       StepU = Abs(Step*previousd2d.X());
134       StepV = Abs(Step*previousd2d.Y());
135
136       if (StepU < tolerance(1) && StepV < tolerance(2))
137         Status = IntWalk_ArretSurPointPrecedent;
138       else 
139         Status = IntWalk_PasTropGrand;
140       NbDivision = NbDivision + 1;
141       return Status;
142     }
143
144     Cosi = Corde * sp.Direction3d(); 
145     Cosi2 = Cosi * Cosi / sp.Direction3d().SquareMagnitude() / Norme;
146     if (Cosi2 < CosRef3D ){ //angle 3d too great
147       Step = Step / 2.;
148       StepU = Abs(Step*previousd2d.X());
149       StepV = Abs(Step*previousd2d.Y());
150       if (StepU < tolerance(1) && StepV < tolerance(2))
151         Status = IntWalk_ArretSurPoint;
152       else 
153         Status = IntWalk_PasTropGrand;
154       return Status;
155     }
156     Cosi = Du * sp.Direction2d().X() + 
157            Dv * sp.Direction2d().Y();
158     Cosi2 = Cosi * Cosi / Duv;
159     if (Cosi2 < CosRef2D || 
160         sp.Direction2d() * previousd2d < 0) {
161       //angle 2d too great or change the side       
162       Step  = Step / 2.;
163       StepU = Abs(Step*previousd2d.X());
164       StepV = Abs(Step*previousd2d.Y());
165       if (StepU < tolerance(1) && StepV < tolerance(2))
166         Status = IntWalk_ArretSurPointPrecedent;
167       else 
168         Status = IntWalk_PasTropGrand;
169       return Status;
170     }
171   }
172
173   if (!Finished) {
174
175     if (Status == IntWalk_PointConfondu) {
176       StepU = Min(Abs(1.5 * Du),pas*(UM-Um));
177       StepV = Min(Abs(1.5 * Dv),pas*(VM-Vm));
178
179       Standard_Real d2dx = Abs(previousd2d.X()); 
180       Standard_Real d2dy = Abs(previousd2d.Y()); 
181
182       if (d2dx < tolerance(1)) {
183         Step = StepV/d2dy;
184       }
185       else if (d2dy < tolerance(2)) {
186         Step = StepU/d2dx;
187       }
188       else {
189         Step = Min(StepU/d2dx,StepV/d2dy);
190       }
191
192
193
194     }
195     else {
196 //   estimate the current vector.
197 //   if vector/2<=current vector<= vector it is considered that the criterion
198 //   is observed.
199 //   otherwise adjust the step depending on the previous step 
200
201 /*
202       Standard_Real Dist = Sqrt(Norme)/3.;
203       TColgp_Array1OfPnt Poles(1,4);
204       gp_Pnt POnCurv,Milieu;
205       Poles(1) = previousPoint.Value();
206       Poles(4) = sp.Point();
207       Poles(2) = Poles(1).XYZ() + 
208         StepSign * Dist* previousd3d.Normalized().XYZ();
209       Poles(3) = Poles(4).XYZ() - 
210         StepSign * Dist*sp.Direction3d().Normalized().XYZ();
211       BzCLib::PntPole(0.5,Poles,POnCurv);
212       Milieu = (Poles(1).XYZ() + Poles(4).XYZ())*0.5;
213 //      FlecheCourante = Milieu.Distance(POnCurv);
214       Standard_Real FlecheCourante = Milieu.SquareDistance(POnCurv);
215 */
216
217       // Direct calculation : 
218       // POnCurv=(((p1+p2)/2.+(p2+p3)/2.)/2. + ((p2+p3)/2.+(p3+P4)/2.)/2.)/2.
219       // either POnCurv = p1/8. + 3.p2/8. + 3.p3/8. + p4/8.
220       // Or p2 = p1 + lambda*d1 et p3 = p4 - lambda*d4
221       // So POnCurv = (p1 + p4)/2. + 3.*(lambda d1 - lambda d4)/8.
222       // Calculate the deviation with (p1+p4)/2. . So it is just necessary to calculate
223       // the norm (square) of 3.*lambda (d1 - d4)/8.
224       // either the norm of :
225       //    3.*(Sqrt(Norme)/3.)*StepSign*(d1-d4)/8.
226       // which produces, takin the square :
227       //         Norme * (d1-d4).SquareMagnitude()/64.
228
229       Standard_Real FlecheCourante = 
230         (previousd3d.Normalized().XYZ()-sp.Direction3d().Normalized().XYZ()).SquareModulus()*Norme/64.;
231
232   
233 //      if (FlecheCourante <= 0.5*fleche) {
234       if (FlecheCourante <= 0.25*fleche*fleche) {
235
236         Standard_Real d2dx = Abs(sp.Direction2d().X()); 
237         Standard_Real d2dy = Abs(sp.Direction2d().Y()); 
238
239         StepU = Min(Abs(1.5*Du),pas*(UM-Um));
240         StepV = Min(Abs(1.5*Dv),pas*(VM-Vm));
241
242         if (d2dx < tolerance(1)) {
243           Step = StepV/d2dy;
244         }
245         else if (d2dy < tolerance(2)) {
246           Step = StepU/d2dx;
247         }
248         else {
249           Step = Min(StepU/d2dx,StepV/d2dy);
250         }
251         
252       }
253       else {
254 //      if (FlecheCourante > fleche) {  // step too great
255         if (FlecheCourante > fleche*fleche) {  // step too great
256           Step = Step /2.;
257           StepU = Abs(Step*previousd2d.X());
258           StepV = Abs(Step*previousd2d.Y());
259           if (StepU < tolerance(1) && StepV < tolerance(2)) 
260             Status = IntWalk_ArretSurPointPrecedent;
261           else 
262             Status = IntWalk_PasTropGrand;
263         }
264         else {
265           Standard_Real d2dx = Abs(sp.Direction2d().X()); 
266           Standard_Real d2dy = Abs(sp.Direction2d().Y()); 
267
268           StepU = Min(Abs(1.5*Du),pas*(UM-Um));
269           StepV = Min(Abs(1.5*Dv),pas*(VM-Vm));
270
271           if (d2dx < tolerance(1)) {
272             Step = Min(Step,StepV/d2dy);
273           }
274           else if (d2dy < tolerance(2)) {
275             Step = Min(Step,StepU/d2dx);
276           }
277           else {
278             Step = Min(Step,Min(StepU/d2dx,StepV/d2dy));
279           }
280         }
281       }
282     }
283   }
284   return Status;     
285 }
286
287
288
289