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