src/Blend/Blend_Walking_4.gxx

   1 static void evalpinit(math_Vector& parinit,
   2                       const Blend_Point& previousP,
   3                       const Standard_Real parprec,
   4                       const Standard_Real param,
   5                       const math_Vector& infbound,
   6                       const math_Vector& supbound,
   7                       const Standard_Boolean classonS1,
   8                       const Standard_Boolean classonS2)
   9 {
  10   if(previousP.IsTangencyPoint()){
  11     previousP.ParametersOnS1(parinit(1),parinit(2));
  12     previousP.ParametersOnS2(parinit(3),parinit(4));
  13   }
  14   else {
  15     Standard_Real u1,v1,u2,v2;
  16     Standard_Real du1,dv1,du2,dv2;
  17     Standard_Boolean Inside=Standard_True;
  18     previousP.ParametersOnS1(u1,v1);
  19     previousP.ParametersOnS2(u2,v2);
  20     previousP.Tangent2dOnS1().Coord(du1,dv1);
  21     previousP.Tangent2dOnS2().Coord(du2,dv2);
  22     Standard_Real step = param - parprec;
  23     u1+= step*du1;
  24     v1+= step*dv1;
  25     if ( classonS1 ) {
  26       if ((u1<infbound(1)) || (u1>supbound(1))) Inside=Standard_False;
  27       if ((v1<infbound(2)) || (v1>supbound(2))) Inside=Standard_False;
  28     }
  29     u2+= step*du2;
  30     v2+= step*dv2;
  31     if ( classonS2) {
  32       if ((u2<infbound(3)) || (u2>supbound(3))) Inside=Standard_False;
  33       if ((v2<infbound(4)) || (v2>supbound(4))) Inside=Standard_False;
  34     }
  35
  36     if (Inside) {
  37       parinit(1) = u1;
  38       parinit(2) = v1;
  39       parinit(3) = u2;
  40       parinit(4) = v2;
  41     }
  42     else { // on ne joue pas au plus malin
  43       previousP.ParametersOnS1(parinit(1),parinit(2));
  44       previousP.ParametersOnS2(parinit(3),parinit(4));
  45     }
  46
  47   }
  48 }
  49
  50
  51
  52 void Blend_Walking::InternalPerform(Blend_Function& Func,
  53                                     Blend_FuncInv& FuncInv,
  54                                     const Standard_Real Bound)
  55 {
  56
  57   Standard_Real stepw = pasmax;
  58   Standard_Integer nbp = line->NbPoints();
  59   if(nbp >= 2){ //On reprend le dernier step s'il n est pas trop petit.
  60     if(sens < 0.){
  61       stepw = (line->Point(2).Parameter() - line->Point(1).Parameter());
  62     }
  63     else{
  64       stepw = (line->Point(nbp).Parameter() - line->Point(nbp - 1).Parameter());
  65     }
  66     stepw = Max(stepw,100.*tolgui);
  67   }
  68   Standard_Real parprec = param;
  69
  70   if (sens*(parprec - Bound) >= -tolgui) {
  71     return;
  72   }
  73   Blend_Status State = Blend_OnRst12;
  74   TopAbs_State situ1 =TopAbs_IN,situ2=TopAbs_IN;
  75   Standard_Real w1,w2;
  76   Standard_Integer Index1,Index2,nbarc;
  77   Standard_Boolean Arrive,recad1,recad2, control;
  78   Standard_Boolean Isvtx1,Isvtx2,echecrecad;
  79   gp_Pnt2d p2d;
  80   math_Vector tolerance(1,4),infbound(1,4),supbound(1,4),parinit(1,4);
  81   math_Vector solrst1(1,4),solrst2(1,4);
  82   TheVertex Vtx1,Vtx2;
  83   TheExtremity Ext1,Ext2;
  84
  85   //IntSurf_Transition Tline,Tarc;
  86
  87   Func.GetTolerance(tolerance,tolesp);
  88   Func.GetBounds(infbound,supbound);
  89
  90   math_FunctionSetRoot rsnld(Func,tolerance,30);
  91   parinit = sol;
  92
  93   Arrive = Standard_False;
  94   param = parprec + sens*stepw;
  95   if(sens *(param - Bound) > 0.) {
  96     stepw = sens*(Bound - parprec)*0.5;
  97     param = parprec + sens*stepw;
  98   }
  99
 100   evalpinit(parinit,previousP,parprec,param,
 101             infbound,supbound, clasonS1, clasonS2);
 102
 103   while (!Arrive) {
 104
 105 #ifdef DEB
 106     sectioncalculee = 0;
 107     nbcomputedsection++;
 108 #endif
 109     Standard_Boolean bonpoint = 1;
 110     Func.Set(param);
 111     rsnld.Perform(Func,parinit,infbound,supbound);
 112
 113     if (!rsnld.IsDone()) {
 114       State = Blend_StepTooLarge;
 115       bonpoint = 0;
 116     }
 117     else {
 118       rsnld.Root(sol);
 119
 120       if(clasonS1) situ1 = domain1->Classify(gp_Pnt2d(sol(1),sol(2)),
 121                                              Min(tolerance(1),tolerance(2)),0);
 122       else situ1 = TopAbs_IN;
 123       if(clasonS2) situ2 = domain2->Classify(gp_Pnt2d(sol(3),sol(4)),
 124                                              Min(tolerance(3),tolerance(4)),0);
 125       else situ2 = TopAbs_IN;
 126     }
 127     if(bonpoint && line->NbPoints() == 1 && (situ1 != TopAbs_IN || situ2 != TopAbs_IN)){
 128       State = Blend_StepTooLarge;
 129       bonpoint = 0;
 130     }
 131     if(bonpoint){
 132       w1 = w2 = Bound;
 133       recad1 = Standard_False;
 134       recad2 = Standard_False;
 135       echecrecad = Standard_False;
 136       control = Standard_False;
 137
 138       if (situ1 == TopAbs_OUT || situ1 == TopAbs_ON) {
 139         // pb inverse sur surf1
 140         //Si le recadrage s'effectue dans le sens de la progression a une tolerance pres,
 141         //on a pris la mauvaise solution.
 142         recad1 = Recadre(FuncInv,Standard_True,
 143                          sol,solrst1,Index1,Isvtx1,Vtx1);
 144
 145         if (recad1) {
 146           Standard_Real wtemp;
 147           wtemp  = solrst1(2);
 148           if ((param - wtemp)/sens>= -10*tolesp){
 149             w1 = solrst1(2);
 150             control = Standard_True;
 151           }
 152           else {
 153             echecrecad = Standard_True;
 154             recad1 = Standard_False;
 155             State = Blend_StepTooLarge;
 156             bonpoint = 0;
 157             stepw = stepw/2.;
 158           }
 159         }
 160         else {
 161           echecrecad = Standard_True;
 162         }
 163       }
 164       if (situ2 == TopAbs_OUT || situ2 == TopAbs_ON) {
 165         // pb inverse sur surf2
 166         //Si le recadrage s'effectue dans le sens de la progression a une tolerance pres,
 167         //on a pris la mauvaise solution.
 168         recad2 = Recadre(FuncInv,Standard_False,
 169                          sol,solrst2,Index2,Isvtx2,Vtx2);
 170
 171         if (recad2) {
 172           Standard_Real wtemp;
 173           wtemp = solrst2(2);
 174           if ((param - wtemp)/sens>= -10*tolesp){
 175             w2 = solrst2(2);
 176             control = Standard_True;
 177           }
 178           else {
 179             echecrecad = Standard_True;
 180             recad2 = Standard_False;
 181             State = Blend_StepTooLarge;
 182             bonpoint = 0;
 183             stepw = stepw/2.;
 184           }
 185         }
 186         else {
 187           echecrecad = Standard_True;
 188         }
 189       }
 190
 191       // Que faut il controler
 192       if (recad1 && recad2) {
 193           if (Abs(w1-w2) <= 10*tolgui) {
 194           // pas besoin de controler les recadrage
 195           // Le control pouvant se planter (cf model blend10)
 196           // La tolerance est choisie grossse afin, de permetre au
 197           // cheminement suivant, de poser quelques sections ...
 198           control = Standard_False;
 199           }
 200           else if (sens*(w1-w2) < 0.) {
 201             //sol sur 1 ?
 202             recad2 = Standard_False;
 203           }
 204           else {
 205             //sol sur 2 ?
 206             recad1 = Standard_False;
 207           }
 208         }
 209
 210       // Controle effectif des recadrage
 211       if (control) {
 212         TopAbs_State situ;
 213         if (recad1 && clasonS2) {
 214           situ = recdomain2->Classify(gp_Pnt2d(solrst1(3),solrst1(4)),
 215                                       Min(tolerance(3),tolerance(4)));
 216           if (situ == TopAbs_OUT) {
 217             recad1 = Standard_False;
 218             echecrecad = Standard_True;
 219           }
 220         }
 221         else if (recad2 && clasonS1) {
 222           situ = recdomain1->Classify(gp_Pnt2d(solrst2(3),solrst2(4)),
 223                                       Min(tolerance(1),tolerance(1)));
 224           if (situ == TopAbs_OUT) {
 225             recad2 = Standard_False;
 226             echecrecad = Standard_True;
 227           }
 228         }
 229       }
 230
 231       if(recad1 || recad2) echecrecad = Standard_False;
 232
 233       if (!echecrecad) {
 234         if (recad1 && recad2) {
 235           //sol sur 1 et 2 a la fois
 236           // On passe par les arcs , pour ne pas avoir de probleme
 237           // avec les surfaces periodiques.
 238           State = Blend_OnRst12;
 239           param =  (w1+w2)/2;
 240           p2d = TheArcTool::Value(recdomain1->Value(),solrst1(1));
 241           sol(1) = p2d.X();
 242           sol(2) = p2d.Y();
 243           p2d = TheArcTool::Value(recdomain2->Value(),solrst2(1));
 244           sol(3) = p2d.X();
 245           sol(4) = p2d.Y();
 246         }
 247         else if (recad1) {
 248           // sol sur 1
 249           State = Blend_OnRst1;
 250           param = w1;
 251           recdomain1->Init();
 252           nbarc = 1;
 253           while (nbarc < Index1) {
 254             nbarc++;
 255             recdomain1->Next();
 256           }
 257           p2d = TheArcTool::Value(recdomain1->Value(),solrst1(1));
 258           sol(1) = p2d.X();
 259           sol(2) = p2d.Y();
 260           sol(3) = solrst1(3);
 261           sol(4) = solrst1(4);
 262         }
 263         else if (recad2) {
 264           //sol sur 2
 265           State = Blend_OnRst2;
 266           param = w2;
 267
 268           recdomain2->Init();
 269           nbarc = 1;
 270           while (nbarc < Index2) {
 271             nbarc++;
 272             recdomain2->Next();
 273           }
 274           p2d = TheArcTool::Value(recdomain2->Value(),solrst2(1));
 275           sol(1) = solrst2(3);
 276           sol(2) = solrst2(4);
 277           sol(3) = p2d.X();
 278           sol(4) = p2d.Y();
 279         }
 280         else {
 281           State = Blend_OK;
 282         }
 283
 284         Standard_Boolean testdefl = 1;
 285 #ifdef DEB
 286         testdefl = !Blend_GetcontextNOTESTDEFL();
 287 #endif
 288         if (recad1 || recad2) {
 289           Func.Set(param);
 290           // Il vaut mieux un pas non orthodoxe que pas de recadrage!! PMN
 291           State = TestArret(Func, State,
 292                             (testdefl && (Abs(stepw) > 3*tolgui)),
 293                             Standard_False, Standard_True);
 294         }
 295         else {
 296           State = TestArret(Func, State, testdefl);
 297         }
 298       }
 299       else {
 300         // Ou bien le pas max est mal regle. On divise.
 301 //      if(line->NbPoints() == 1) State = Blend_StepTooLarge;
 302         if (stepw > 2*tolgui) State = Blend_StepTooLarge;
 303         // Sinon echec recadrage. On sort avec PointsConfondus
 304         else {
 305 #if DEB
 306           cout << "Echec recadrage" << endl;
 307 #endif
 308           State = Blend_SamePoints;
 309         }
 310       }
 311     }
 312
 313 #ifdef DEB
 314     if (Blend_GettraceDRAWSECT()){
 315       Drawsect(surf1,surf2,sol,param,Func, State);
 316     }
 317 #endif
 318     switch (State) {
 319     case Blend_OK :
 320       {
 321         // Mettre a jour la ligne.
 322         if (sens>0.) {
 323           line->Append(previousP);
 324         }
 325         else {
 326           line->Prepend(previousP);
 327         }
 328
 329         parprec = param;
 330
 331         if (param == Bound) {
 332           Arrive = Standard_True;
 333           Ext1.SetValue(previousP.PointOnS1(),
 334                         sol(1),sol(2),
 335                         previousP.Parameter(), tolesp);
 336           Ext2.SetValue(previousP.PointOnS2(),
 337                         sol(3),sol(4),
 338                         previousP.Parameter(), tolesp);
 339           if (!previousP.IsTangencyPoint()) {
 340             Ext1.SetTangent(previousP.TangentOnS1());
 341             Ext2.SetTangent(previousP.TangentOnS2());
 342           }
 343
 344           // Indiquer que fin sur Bound.
 345         }
 346         else {
 347           param = param + sens*stepw;
 348           if (sens*(param - Bound) > - tolgui) {
 349             param = Bound;
 350           }
 351         }
 352         evalpinit(parinit,previousP,parprec,param,
 353                   infbound,supbound, clasonS1, clasonS2);
 354       }
 355       break;
 356
 357     case Blend_StepTooLarge :
 358       {
 359         stepw = stepw/2.;
 360         if (Abs(stepw) < tolgui) {
 361           Ext1.SetValue(previousP.PointOnS1(),
 362                         sol(1),sol(2),
 363                         previousP.Parameter(),tolesp);
 364           Ext2.SetValue(previousP.PointOnS2(),
 365                         sol(3),sol(4),
 366                         previousP.Parameter(),tolesp);
 367           if (!previousP.IsTangencyPoint()) {
 368             Ext1.SetTangent(previousP.TangentOnS1());
 369             Ext2.SetTangent(previousP.TangentOnS2());
 370           }
 371           Arrive = Standard_True;
 372           if (line->NbPoints()>=2) {
 373             // Indiquer qu on s arrete en cours de cheminement
 374           }
 375 //        else {
 376 //          line->Clear();
 377 //        }
 378         }
 379         else {
 380           param = parprec + sens*stepw;  // on ne risque pas de depasser Bound.
 381           evalpinit(parinit,previousP,parprec,param,
 382                     infbound,supbound, clasonS1, clasonS2);
 383         }
 384       }
 385       break;
 386
 387     case Blend_StepTooSmall :
 388       {
 389         // Mettre a jour la ligne.
 390         if (sens>0.) {
 391           line->Append(previousP);
 392         }
 393         else {
 394           line->Prepend(previousP);
 395         }
 396
 397         parprec = param;
 398
 399         stepw = Min(1.5*stepw,pasmax);
 400         if (param == Bound) {
 401           Arrive = Standard_True;
 402           Ext1.SetValue(previousP.PointOnS1(),
 403                         sol(1),sol(2),
 404                         previousP.Parameter(),tolesp);
 405           Ext2.SetValue(previousP.PointOnS2(),
 406                         sol(3),sol(4),
 407                         previousP.Parameter(),tolesp);
 408           if (!previousP.IsTangencyPoint()) {
 409             Ext1.SetTangent(previousP.TangentOnS1());
 410             Ext2.SetTangent(previousP.TangentOnS2());
 411           }
 412           // Indiquer que fin sur Bound.
 413         }
 414         else {
 415           param = param + sens*stepw;
 416           if (sens*(param - Bound) > - tolgui) {
 417             param = Bound;
 418           }
 419         }
 420         evalpinit(parinit,previousP,parprec,param,
 421                   infbound,supbound, clasonS1, clasonS2);
 422       }
 423       break;
 424
 425     case Blend_OnRst1  :
 426       {
 427         if (sens>0.) {
 428           line->Append(previousP);
 429         }
 430         else {
 431           line->Prepend(previousP);
 432         }
 433         MakeExtremity(Ext1,Standard_True,Index1,
 434                       solrst1(1),Isvtx1,Vtx1);
 435         // On blinde le cas singulier ou un des recadrage a planter
 436         if (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2*tolesp)) {
 437           Ext2.SetValue(previousP.PointOnS1(),
 438                         sol(3),sol(4),tolesp);
 439           if (Isvtx1) MakeSingularExtremity(Ext2, Standard_False, Vtx1);
 440         }
 441         else {
 442           Ext2.SetValue(previousP.PointOnS2(),
 443                         sol(3),sol(4),
 444                         previousP.Parameter(),tolesp);
 445         }
 446         Arrive = Standard_True;
 447       }
 448       break;
 449
 450     case Blend_OnRst2  :
 451       {
 452         if (sens>0.) {
 453           line->Append(previousP);
 454         }
 455         else {
 456           line->Prepend(previousP);
 457         }
 458         // On blinde le cas singulier ou un des recadrage a plante
 459         if (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2*tolesp)) {
 460           Ext1.SetValue(previousP.PointOnS2(),
 461                         sol(1),sol(2),tolesp);
 462           if (Isvtx2) MakeSingularExtremity(Ext1, Standard_True, Vtx2);
 463         }
 464         else {
 465           Ext1.SetValue(previousP.PointOnS1(),
 466                         sol(1),sol(2),
 467                         previousP.Parameter(),tolesp);
 468         }
 469         MakeExtremity(Ext2,Standard_False,Index2,
 470                       solrst2(1),Isvtx2,Vtx2);
 471         Arrive = Standard_True;
 472       }
 473       break;
 474
 475
 476     case Blend_OnRst12 :
 477       {
 478         if (sens>0.) {
 479           line->Append(previousP);
 480         }
 481         else {
 482           line->Prepend(previousP);
 483         }
 484
 485         if ( (Isvtx1 != Isvtx2) &&
 486             (previousP.PointOnS1().IsEqual(previousP.PointOnS2(), 2*tolesp)) ) {
 487           // On blinde le cas singulier ou un seul recadrage
 488           // est reconnu comme vertex.
 489           if (Isvtx1) {
 490             Isvtx2 = Standard_True;
 491             Vtx2 = Vtx1;
 492           }
 493           else {
 494             Isvtx1 = Standard_True;
 495             Vtx1 = Vtx2;
 496           }
 497         }
 498
 499         MakeExtremity(Ext1,Standard_True,Index1,
 500                       solrst1(1),Isvtx1,Vtx1);
 501         MakeExtremity(Ext2,Standard_False,Index2,
 502                       solrst2(1),Isvtx2,Vtx2);
 503         Arrive = Standard_True;
 504       }
 505       break;
 506
 507     case Blend_SamePoints :
 508       {
 509         // On arrete
 510 #if DEB
 511         cout << " Points confondus dans le cheminement" << endl;
 512 #endif
 513         Ext1.SetValue(previousP.PointOnS1(),
 514                       sol(1),sol(2),
 515                       previousP.Parameter(),tolesp);
 516         Ext2.SetValue(previousP.PointOnS2(),
 517                       sol(3),sol(4),
 518                       previousP.Parameter(),tolesp);;
 519         if (!previousP.IsTangencyPoint()) {
 520           Ext1.SetTangent(previousP.TangentOnS1());
 521           Ext2.SetTangent(previousP.TangentOnS2());
 522         }
 523         Arrive = Standard_True;
 524       }
 525       break;
 526 #ifndef DEB
 527     default:
 528       break;
 529 #endif
 530     }
 531     if (Arrive) {
 532       if (sens > 0.) {
 533         line->SetEndPoints(Ext1,Ext2);
 534       }
 535       else {
 536         line->SetStartPoints(Ext1,Ext2);
 537
 538       }
 539     }
 540
 541   }
 542
 543 }
 544
 545
 546
 547
 548
 549
 550
 551
 552