| 1 | // Copyright (c) 1995-1999 Matra Datavision |
| 2 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
| 3 | // |
| 4 | // The content of this file is subject to the Open CASCADE Technology Public |
| 5 | // License Version 6.5 (the "License"). You may not use the content of this file |
| 6 | // except in compliance with the License. Please obtain a copy of the License |
| 7 | // at http://www.opencascade.org and read it completely before using this file. |
| 8 | // |
| 9 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
| 10 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
| 11 | // |
| 12 | // The Original Code and all software distributed under the License is |
| 13 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
| 14 | // Initial Developer hereby disclaims all such warranties, including without |
| 15 | // limitation, any warranties of merchantability, fitness for a particular |
| 16 | // purpose or non-infringement. Please see the License for the specific terms |
| 17 | // and conditions governing the rights and limitations under the License. |
| 18 | |
| 19 | IntWalk_StatusDeflection IntWalk_IWalking::TestDeflection |
| 20 | (TheIWFunction& sp, |
| 21 | const Standard_Boolean Finished, |
| 22 | const math_Vector& UV, |
| 23 | const IntWalk_StatusDeflection StatusPrecedent, |
| 24 | Standard_Integer& NbDivision, |
| 25 | Standard_Real& Step, |
| 26 | const Standard_Integer StepSign) |
| 27 | { |
| 28 | // Verification du pas d avancement, ET recalcul de ce pas : |
| 29 | // |
| 30 | // 1)test point confondu |
| 31 | // si oui les autres tests ne sont pas faits |
| 32 | // 2)test angle 3d trop grand |
| 33 | // si oui on divise le pas, on sort |
| 34 | // angle3d = angle ((point precedent, point calcule), |
| 35 | // tangente precedente) |
| 36 | // 3)verification du pas d avancement en 2d |
| 37 | // 4)test point confondu |
| 38 | // 5)test angle 2d trop grand |
| 39 | // 6)test point de tangence |
| 40 | // si oui on sort |
| 41 | // 7)calcul de la tangente en u,v de la section |
| 42 | // 8)test angle 3d trop grand |
| 43 | // angle3d = angle ((point precedent, point calcule), |
| 44 | // nouvelle tangente) |
| 45 | // 9)test angle 2d trop grand |
| 46 | //10)test de changement de rive(depasser le point de tangence sans le savoir) |
| 47 | //11)calcul du pas d avancement en fonction de la fleche |
| 48 | //12)ajustement du pas en fonction des pas precedents |
| 49 | |
| 50 | IntWalk_StatusDeflection Status = IntWalk_OK; |
| 51 | |
| 52 | static const Standard_Real CosRef3D = 0.98;// regle par tests dans U4 |
| 53 | // correspond a 11.478 d |
| 54 | static const Standard_Real CosRef2D = 0.88; // correspond a 25 d |
| 55 | static const Standard_Integer MaxDivision = 60; // nombre maxi de division |
| 56 | // du pas a cause de |
| 57 | // l angle trop grand en 2d (U4) |
| 58 | //--------------------------------------------------------------------------------- |
| 59 | //-- lbr le 4 Avril 95 : On peut se trouver ds le cas ou Status renvoie point |
| 60 | //-- confondus si epsilon est assez grand (1e-11) . Dans ce cas on boucle |
| 61 | //-- sans jamais changer les valeurs envoyees a Rsnld. |
| 62 | static Standard_Integer NbPointsConfondusConsecutifs = 0 ; |
| 63 | static Standard_Integer EpsilonSembleTropGrand = 0 ; |
| 64 | //--------------------------------------------------------------------------------- |
| 65 | Standard_Real Paramu, Paramv, StepU,StepV; |
| 66 | Standard_Real Cosi, Cosi2, Norme; |
| 67 | |
| 68 | gp_Vec Corde(previousPoint.Value(), sp.Point()); |
| 69 | |
| 70 | Norme = Corde.SquareMagnitude(); |
| 71 | // if (Norme <= epsilon*epsilon) { |
| 72 | if ((++NbPointsConfondusConsecutifs < 10) && (Norme <= epsilon)) { // le carre est deja pris dans le constructeur |
| 73 | Status = IntWalk_PointConfondu; |
| 74 | if (StatusPrecedent == IntWalk_PasTropGrand) { |
| 75 | return IntWalk_ArretSurPointPrecedent; |
| 76 | } |
| 77 | |
| 78 | if(++EpsilonSembleTropGrand > 5 && NbPointsConfondusConsecutifs == 8) { //-- Provisoire |
| 79 | if(epsilon>0.00000000001) epsilon*=0.5; //-- Provisoire |
| 80 | EpsilonSembleTropGrand = 0; //-- Provisoire |
| 81 | } |
| 82 | } |
| 83 | else { |
| 84 | NbPointsConfondusConsecutifs = 0; //-- Provisoire |
| 85 | EpsilonSembleTropGrand = 0; //-- Provisoire |
| 86 | if(Norme<1e-16) Norme = 1e-16; //-- Provisoire |
| 87 | |
| 88 | Cosi = Corde * previousd3d; |
| 89 | if (Cosi*StepSign < 0.) { // angle 3d > pi/2 !!!! |
| 90 | Cosi2 = 0.; |
| 91 | } |
| 92 | else { |
| 93 | Cosi2 = Cosi * Cosi / previousd3d.SquareMagnitude() / Norme; |
| 94 | } |
| 95 | if (Cosi2 < CosRef3D) { //angle 3d trop grand |
| 96 | Step = Step /2.0; |
| 97 | StepU = Abs(Step*previousd2d.X()); |
| 98 | StepV = Abs(Step*previousd2d.Y()); |
| 99 | if (StepU < tolerance(1) && StepV < tolerance(2)) |
| 100 | Status = IntWalk_ArretSurPointPrecedent; |
| 101 | else |
| 102 | Status = IntWalk_PasTropGrand; |
| 103 | return Status; |
| 104 | } |
| 105 | } |
| 106 | |
| 107 | if (!reversed) { |
| 108 | previousPoint.ParametersOnS2(Paramu, Paramv); |
| 109 | } |
| 110 | else { |
| 111 | previousPoint.ParametersOnS1(Paramu, Paramv); |
| 112 | } |
| 113 | Standard_Real Du = UV(1) - Paramu; |
| 114 | Standard_Real Dv = UV(2) - Paramv; |
| 115 | Standard_Real Duv = Du * Du + Dv * Dv; |
| 116 | if (Abs(Du) < tolerance(1) && Abs(Dv) < tolerance(2)) |
| 117 | return IntWalk_ArretSurPointPrecedent; //point confondu 2d |
| 118 | Cosi = StepSign * (Du * previousd2d.X() + |
| 119 | Dv * previousd2d.Y()); |
| 120 | if (Cosi < 0 && Status == IntWalk_PointConfondu) |
| 121 | return IntWalk_ArretSurPointPrecedent; // on sort car retour arriere |
| 122 | // avec point confondu |
| 123 | |
| 124 | |
| 125 | if (sp.IsTangent()) |
| 126 | return IntWalk_ArretSurPoint; |
| 127 | |
| 128 | //si au cours du cheminement on a subdivise plus de MaxDivision pour chaque |
| 129 | //pas precedent,anomalie sur le carreau;on ne fait plus rien (experience U4) |
| 130 | |
| 131 | if (NbDivision < MaxDivision && |
| 132 | Status != IntWalk_PointConfondu && |
| 133 | StatusPrecedent!= IntWalk_PointConfondu ) { |
| 134 | Cosi2 = Cosi * Cosi / Duv; |
| 135 | if (Cosi2 < CosRef2D || Cosi < 0 ) { |
| 136 | Step = Step / 2.0; |
| 137 | StepU = Abs(Step*previousd2d.X()); |
| 138 | StepV = Abs(Step*previousd2d.Y()); |
| 139 | |
| 140 | if (StepU < tolerance(1) && StepV < tolerance(2)) |
| 141 | Status = IntWalk_ArretSurPointPrecedent; |
| 142 | else |
| 143 | Status = IntWalk_PasTropGrand; |
| 144 | NbDivision = NbDivision + 1; |
| 145 | return Status; |
| 146 | } |
| 147 | |
| 148 | Cosi = Corde * sp.Direction3d(); |
| 149 | Cosi2 = Cosi * Cosi / sp.Direction3d().SquareMagnitude() / Norme; |
| 150 | if (Cosi2 < CosRef3D ){ //angle 3d trop grand |
| 151 | Step = Step / 2.; |
| 152 | StepU = Abs(Step*previousd2d.X()); |
| 153 | StepV = Abs(Step*previousd2d.Y()); |
| 154 | if (StepU < tolerance(1) && StepV < tolerance(2)) |
| 155 | Status = IntWalk_ArretSurPoint; |
| 156 | else |
| 157 | Status = IntWalk_PasTropGrand; |
| 158 | return Status; |
| 159 | } |
| 160 | Cosi = Du * sp.Direction2d().X() + |
| 161 | Dv * sp.Direction2d().Y(); |
| 162 | Cosi2 = Cosi * Cosi / Duv; |
| 163 | if (Cosi2 < CosRef2D || |
| 164 | sp.Direction2d() * previousd2d < 0) { |
| 165 | //angle 2d trop grand ou changement de rive |
| 166 | Step = Step / 2.; |
| 167 | StepU = Abs(Step*previousd2d.X()); |
| 168 | StepV = Abs(Step*previousd2d.Y()); |
| 169 | if (StepU < tolerance(1) && StepV < tolerance(2)) |
| 170 | Status = IntWalk_ArretSurPointPrecedent; |
| 171 | else |
| 172 | Status = IntWalk_PasTropGrand; |
| 173 | return Status; |
| 174 | } |
| 175 | } |
| 176 | |
| 177 | if (!Finished) { |
| 178 | |
| 179 | if (Status == IntWalk_PointConfondu) { |
| 180 | StepU = Min(Abs(1.5 * Du),pas*(UM-Um)); |
| 181 | StepV = Min(Abs(1.5 * Dv),pas*(VM-Vm)); |
| 182 | |
| 183 | Standard_Real d2dx = Abs(previousd2d.X()); |
| 184 | Standard_Real d2dy = Abs(previousd2d.Y()); |
| 185 | |
| 186 | if (d2dx < tolerance(1)) { |
| 187 | Step = StepV/d2dy; |
| 188 | } |
| 189 | else if (d2dy < tolerance(2)) { |
| 190 | Step = StepU/d2dx; |
| 191 | } |
| 192 | else { |
| 193 | Step = Min(StepU/d2dx,StepV/d2dy); |
| 194 | } |
| 195 | |
| 196 | |
| 197 | |
| 198 | } |
| 199 | else { |
| 200 | // on estime la fleche courante. |
| 201 | // si fleche/2<=flechecourante<= fleche on considere que le critere est |
| 202 | // respecte. |
| 203 | // sinon ajuster le pas en fonction du pas precedent |
| 204 | |
| 205 | /* |
| 206 | Standard_Real Dist = Sqrt(Norme)/3.; |
| 207 | TColgp_Array1OfPnt Poles(1,4); |
| 208 | gp_Pnt POnCurv,Milieu; |
| 209 | Poles(1) = previousPoint.Value(); |
| 210 | Poles(4) = sp.Point(); |
| 211 | Poles(2) = Poles(1).XYZ() + |
| 212 | StepSign * Dist* previousd3d.Normalized().XYZ(); |
| 213 | Poles(3) = Poles(4).XYZ() - |
| 214 | StepSign * Dist*sp.Direction3d().Normalized().XYZ(); |
| 215 | BzCLib::PntPole(0.5,Poles,POnCurv); |
| 216 | Milieu = (Poles(1).XYZ() + Poles(4).XYZ())*0.5; |
| 217 | // FlecheCourante = Milieu.Distance(POnCurv); |
| 218 | Standard_Real FlecheCourante = Milieu.SquareDistance(POnCurv); |
| 219 | */ |
| 220 | |
| 221 | // Calcul direct : |
| 222 | // POnCurv=(((p1+p2)/2.+(p2+p3)/2.)/2. + ((p2+p3)/2.+(p3+P4)/2.)/2.)/2. |
| 223 | // soit POnCurv = p1/8. + 3.p2/8. + 3.p3/8. + p4/8. |
| 224 | // Or p2 = p1 + lambda*d1 et p3 = p4 - lambda*d4 |
| 225 | // Donc POnCurv = (p1 + p4)/2. + 3.*(lambda d1 - lambda d4)/8. |
| 226 | // On calcule l'ecart avec (p1+p4)/2. . Il faut donc juste calculer |
| 227 | // la norme (au carre) de 3.*lambda (d1 - d4)/8. |
| 228 | // soit la norme de : |
| 229 | // 3.*(Sqrt(Norme)/3.)*StepSign*(d1-d4)/8. |
| 230 | // ce qui fait, en prenant le carre : |
| 231 | // Norme * (d1-d4).SquareMagnitude()/64. |
| 232 | |
| 233 | Standard_Real FlecheCourante = |
| 234 | (previousd3d.Normalized().XYZ()-sp.Direction3d().Normalized().XYZ()).SquareModulus()*Norme/64.; |
| 235 | |
| 236 | |
| 237 | // if (FlecheCourante <= 0.5*fleche) { |
| 238 | if (FlecheCourante <= 0.25*fleche*fleche) { |
| 239 | |
| 240 | Standard_Real d2dx = Abs(sp.Direction2d().X()); |
| 241 | Standard_Real d2dy = Abs(sp.Direction2d().Y()); |
| 242 | |
| 243 | StepU = Min(Abs(1.5*Du),pas*(UM-Um)); |
| 244 | StepV = Min(Abs(1.5*Dv),pas*(VM-Vm)); |
| 245 | |
| 246 | if (d2dx < tolerance(1)) { |
| 247 | Step = StepV/d2dy; |
| 248 | } |
| 249 | else if (d2dy < tolerance(2)) { |
| 250 | Step = StepU/d2dx; |
| 251 | } |
| 252 | else { |
| 253 | Step = Min(StepU/d2dx,StepV/d2dy); |
| 254 | } |
| 255 | |
| 256 | } |
| 257 | else { |
| 258 | // if (FlecheCourante > fleche) { // pas trop grand |
| 259 | if (FlecheCourante > fleche*fleche) { // pas trop grand |
| 260 | Step = Step /2.; |
| 261 | Status = IntWalk_PasTropGrand; |
| 262 | } |
| 263 | else { |
| 264 | Standard_Real d2dx = Abs(sp.Direction2d().X()); |
| 265 | Standard_Real d2dy = Abs(sp.Direction2d().Y()); |
| 266 | |
| 267 | StepU = Min(Abs(1.5*Du),pas*(UM-Um)); |
| 268 | StepV = Min(Abs(1.5*Dv),pas*(VM-Vm)); |
| 269 | |
| 270 | if (d2dx < tolerance(1)) { |
| 271 | Step = Min(Step,StepV/d2dy); |
| 272 | } |
| 273 | else if (d2dy < tolerance(2)) { |
| 274 | Step = Min(Step,StepU/d2dx); |
| 275 | } |
| 276 | else { |
| 277 | Step = Min(Step,Min(StepU/d2dx,StepV/d2dy)); |
| 278 | } |
| 279 | } |
| 280 | } |
| 281 | } |
| 282 | } |
| 283 | return Status; |
| 284 | } |
| 285 | |
| 286 | |
| 287 | |
| 288 | |