src/Blend/Blend_Walking_3.gxx

   1 //26-04-1997 modified by pmn : Initialisation plus fine de la resolution
   2
   3 Standard_Integer Blend_Walking::ArcToRecadre(const Standard_Boolean OnFirst,
   4                                              const math_Vector& sol,
   5                                              const Standard_Integer PrevIndex,
   6                                              gp_Pnt2d& lastpt2d,
   7                                              gp_Pnt2d& pt2d,
   8                                              Standard_Real& ponarc)
   9 {
  10   Standard_Integer IndexSol = 0,  nbarc = 0;
  11   Standard_Boolean ok = Standard_False;
  12   Standard_Boolean byinter = (line->NbPoints() != 0), okinter = 0;
  13   Standard_Real distmin = RealLast();
  14   Standard_Real uprev,vprev, prm, dist;
  15   Handle(TheTopolTool) Iter;
  16
  17   if (OnFirst) {
  18     if(byinter) previousP.ParametersOnS1(uprev,vprev);
  19     pt2d.SetCoord(sol(1),sol(2));
  20     Iter = recdomain1;
  21   }
  22   else {
  23     if(byinter) previousP.ParametersOnS2(uprev,vprev);
  24     pt2d.SetCoord(sol(3),sol(4));
  25     Iter = recdomain2;
  26   }
  27   lastpt2d.SetCoord(uprev,vprev);
  28   Iter->Init();
  29   while (Iter->More()) {
  30     nbarc++; ok = 0;
  31     if (OnFirst) {
  32       if(byinter) {
  33         ok = okinter = TheBlendTool::Inters(pt2d,lastpt2d,
  34                                             surf1,Iter->Value(),prm,dist);
  35       }
  36       if(!ok) ok = TheBlendTool::Project(pt2d,surf1,Iter->Value(),prm,dist);
  37     }
  38     else {
  39       if(byinter) {
  40         ok = okinter = TheBlendTool::Inters(pt2d,lastpt2d,
  41                                             surf2,Iter->Value(),prm,dist);
  42       }
  43       if(!ok) ok = TheBlendTool::Project(pt2d,surf2,Iter->Value(),prm,dist);
  44     }
  45     if (ok && (nbarc != PrevIndex) ) {
  46       if (dist<distmin || okinter) {
  47         distmin = dist;
  48         ponarc = prm;
  49         IndexSol = nbarc;
  50         if(okinter && (PrevIndex==0)) break;
  51       }
  52     }
  53     Iter->Next();
  54   }
  55   return IndexSol;
  56 }
  57
  58 Standard_Boolean Blend_Walking::Recadre(Blend_FuncInv& FuncInv,
  59                                         const Standard_Boolean OnFirst,
  60                                         const math_Vector& sol,
  61                                         math_Vector& solrst,
  62                                         Standard_Integer& Indexsol,
  63                                         Standard_Boolean& IsVtx,
  64                                         TheVertex& Vtx,
  65                                         const Standard_Real Extrap)
  66
  67 {
  68   Standard_Boolean jalons_Trouve = Standard_False;
  69   Standard_Boolean recadre = Standard_True, ok;
  70   Standard_Boolean byinter = (line->NbPoints() != 0);
  71 #ifndef DEB
  72   Standard_Integer LeJalon = 0;
  73 #else
  74   Standard_Integer LeJalon;
  75 #endif
  76   Standard_Integer nbarc;
  77   Standard_Real dist,prm,pmin, vtol;
  78   gp_Pnt2d pt2d, lastpt2d;
  79
  80   math_Vector toler(1,4),infb(1,4),supb(1,4),valsol(1,4);
  81
  82   Handle(Adaptor2d_HCurve2d) thecur;
  83   Handle(TheTopolTool) Iter;
  84
  85   if (OnFirst) Iter = recdomain1;
  86   else         Iter = recdomain2;
  87
  88   Indexsol = ArcToRecadre(OnFirst, sol, 0,
  89                           lastpt2d, pt2d, pmin);
  90   IsVtx = Standard_False;
  91   if (Indexsol == 0) {
  92     return Standard_False;
  93   }
  94
  95   Iter->Init();
  96   nbarc = 1;
  97   while (nbarc < Indexsol) {
  98     nbarc++;
  99     Iter->Next();
 100   }
 101
 102   TheArc thearc = Iter->Value();
 103
 104   if (OnFirst) {
 105     thecur = TheBlendTool::CurveOnSurf(thearc,surf1);
 106   }
 107   else {
 108     thecur = TheBlendTool::CurveOnSurf(thearc,surf2);
 109   }
 110
 111 // Le probleme a resoudre
 112   FuncInv.Set(OnFirst,thecur);
 113   FuncInv.GetBounds(infb,supb);
 114   infb(2) -= Extrap;
 115   supb(2) += Extrap;
 116
 117   FuncInv.GetTolerance(toler,tolesp/10);//Il vaut mieux garder un peu de marge
 118   math_FunctionSetRoot rsnld(FuncInv,toler,35);
 119   toler *= 10; // Mais on fait les tests correctements
 120
 121 // Calcul d'un point d'init
 122   Standard_Real ufirst,ulast;
 123   TheBlendTool::Bounds(thecur, ufirst,ulast);
 124   // Pour aider a trouver les coins singuliers on recadre eventuelement le paramtere
 125   if (Abs(pmin-ufirst) < Abs(ulast-ufirst)/1000) {
 126     pmin = ufirst;
 127   }
 128   if (Abs(pmin-ulast) < Abs(ulast-ufirst)/1000) {
 129     pmin = ulast;
 130   }
 131
 132   if (byinter) {
 133     Standard_Real lastParam = previousP.Parameter();
 134     // Verifie que le recadrage n'est pas un jalons
 135     if (jalons.Length()!=0) {
 136       Standard_Real t1, t2, t;
 137       Standard_Boolean Cherche=Standard_True;
 138       Standard_Integer ii;
 139       if (lastParam < param) {
 140         t1 = lastParam; t2 = param;
 141       }
 142       else {
 143         t1 = param; t2 = lastParam;
 144       }
 145       for (ii=1; ii<=jalons.Length() && Cherche; ii++) {
 146         t = jalons.Value(ii).Parameter();
 147         if  (((t1 < t) && (t2 > t)) || (t==param)) {
 148          jalons_Trouve = Standard_True;
 149          LeJalon = ii;
 150          Cherche = Standard_False; //Ne marche que si l'on sort simultanement
 151        }
 152         else Cherche = t < t2; // On s'arrete si t>=t2;
 153       }
 154     }
 155     if (!jalons_Trouve) {
 156       //Initialisation par Interpolation
 157       Standard_Real lambda, u, v;
 158       gp_Pnt2d Pnt, Pnt1, Pnt2;//,  POnC;
 159       thecur->D0(pmin, Pnt);
 160       if (OnFirst) {
 161         previousP.ParametersOnS2(u,v);
 162         Pnt1.SetCoord(u, v);
 163         Pnt2.SetCoord(sol(3), sol(4));
 164       }
 165       else {
 166         previousP.ParametersOnS1(u,v);
 167         Pnt1.SetCoord(u, v);
 168         Pnt2.SetCoord(sol(1), sol(2));
 169       }
 170
 171       lambda = Pnt.Distance(lastpt2d);
 172       if (lambda > 1.e-12) lambda /=  Pnt.Distance(lastpt2d) + Pnt.Distance(pt2d);
 173       else lambda = 0;
 174       solrst(1) = pmin;
 175       solrst(2) = (1-lambda)*lastParam + lambda*param;
 176       solrst(3) = (1-lambda)*Pnt1.X()  + lambda*Pnt2.X();
 177       solrst(4) = (1-lambda)*Pnt1.Y()  + lambda*Pnt2.Y();
 178     }
 179   }
 180   else { // sinon on initialise par le dernier point calcule
 181     solrst(1) = pmin;
 182     solrst(2) = param;
 183     if (OnFirst) {
 184       solrst(3) = sol(3);
 185       solrst(4) = sol(4);
 186     }
 187     else {
 188       solrst(3) = sol(1);
 189       solrst(4) = sol(2);
 190     }
 191   }
 192
 193   if (jalons_Trouve) { // On recupere le jalon
 194     Blend_Point MonJalon;
 195     Standard_Boolean periodic;
 196     Standard_Real uperiod = 0, vperiod = 0;
 197     gp_Pnt2d Pnt;
 198     Standard_Real distaux;
 199     MonJalon = jalons.Value(LeJalon);
 200     solrst(2) =  MonJalon.Parameter();
 201     if (OnFirst) {
 202       MonJalon.ParametersOnS2(solrst(3), solrst(4));
 203       periodic = (surf2->IsUPeriodic() || surf2->IsVPeriodic());
 204     }
 205     else  {
 206        MonJalon.ParametersOnS1(solrst(3), solrst(4));
 207        periodic = (surf1->IsUPeriodic() || surf1->IsVPeriodic());
 208      }
 209
 210     // Recadrage eventuelle pour le cas periodique
 211     if (periodic) {
 212       Handle(Adaptor3d_HSurface) surf;
 213       if (OnFirst) surf = surf2;
 214       else surf = surf1;
 215
 216       lastpt2d = thecur->Value(pmin);
 217
 218       if (surf->IsUPeriodic()) {
 219         uperiod =  surf->UPeriod();
 220         if (solrst(3)-lastpt2d.X() >  uperiod*0.6) solrst(3) -= uperiod;
 221         if (solrst(3)-lastpt2d.X() < -uperiod*0.6) solrst(3) += uperiod;
 222       }
 223       if (surf->IsVPeriodic()) {
 224         vperiod =  surf->VPeriod();
 225         if (solrst(4)-lastpt2d.Y() >  vperiod*0.6) solrst(4) -= vperiod;
 226         if (solrst(4)-lastpt2d.Y() < -vperiod*0.6) solrst(4) += vperiod;
 227       }
 228     }
 229
 230     // Pour le parametre sur arc il faut projeter...
 231     pt2d.SetCoord(solrst(3), solrst(4));
 232     Pnt = thecur->Value(ufirst);
 233     dist = pt2d.Distance(Pnt);
 234     solrst(1) = ufirst;
 235     Pnt = thecur->Value(ulast);
 236     distaux = pt2d.Distance(Pnt);
 237     if ( distaux < dist) {
 238       solrst(1) = ulast;
 239       dist = distaux;
 240     }
 241
 242     if (dist>Precision::PConfusion()) {
 243       prm = pmin;
 244       if (OnFirst) {
 245         ok = TheBlendTool::Project(pt2d,surf1,thearc,prm,distaux);
 246       }
 247       else {
 248         ok = TheBlendTool::Project(pt2d,surf2,thearc,prm,distaux);
 249       }
 250       if (ok && (pt2d.Distance(thecur->Value(prm)) < dist)) solrst(1) = prm;
 251       else solrst(1) = pmin;
 252     }
 253     // On verifie le jalon
 254     jalons_Trouve = (FuncInv.IsSolution(solrst,tolesp));
 255   }
 256
 257   if (!jalons_Trouve) {
 258     // Resolution...
 259     rsnld.Perform(FuncInv,solrst,infb,supb);
 260     if (!rsnld.IsDone()) {
 261       cout << "Walking::Recadre : RSNLD not done " << endl;
 262       recadre = Standard_False;
 263     }
 264     else {
 265       rsnld.Root(solrst);
 266       recadre = FuncInv.IsSolution(solrst,tolesp);
 267     }
 268   }
 269
 270   // En cas d'echecs, on regarde si un autre arc
 271   // peut faire l'affaire (cas des sorties a proximite d'un vertex)
 272   dist = (ulast - ufirst)/100;
 273   if ((!recadre) &&
 274       ((Abs(pmin-ulast) < dist) || (Abs(pmin-ufirst) < dist)) ) {
 275
 276     Indexsol =  ArcToRecadre(OnFirst, sol, Indexsol,
 277                              lastpt2d, pt2d, pmin);
 278     if (Indexsol == 0) {
 279       return Standard_False;
 280     }
 281
 282     Iter->Init();
 283     nbarc = 1;
 284     while (nbarc < Indexsol) {
 285       nbarc++;
 286       Iter->Next();
 287     }
 288     thearc = Iter->Value();
 289
 290     if (OnFirst) {
 291       thecur = TheBlendTool::CurveOnSurf(thearc,surf1);
 292     }
 293     else {
 294       thecur = TheBlendTool::CurveOnSurf(thearc,surf2);
 295     }
 296     solrst(1) = pmin;
 297     // Le probleme a resoudre
 298     FuncInv.Set(OnFirst,thecur);
 299     FuncInv.GetBounds(infb,supb);
 300     FuncInv.GetTolerance(toler,tolesp/10);//Il vaut mieux garder un peu de marge
 301     math_FunctionSetRoot rsnld(FuncInv,toler,35);
 302     toler *= 10; // Mais on fait les tests correctements
 303     // Resolution...
 304     rsnld.Perform(FuncInv,solrst,infb,supb);
 305
 306     if (!rsnld.IsDone()) {
 307       cout << "Walking::Recadre : RSNLD not done " << endl;
 308       recadre = Standard_False;
 309     }
 310     else {
 311       rsnld.Root(solrst);
 312       recadre = FuncInv.IsSolution(solrst,tolesp);
 313     }
 314   }
 315
 316   if (recadre) {
 317     // Classification topologique
 318     if (OnFirst) {
 319       thecur = TheBlendTool::CurveOnSurf(thearc,surf1);
 320     }
 321     else {
 322       thecur = TheBlendTool::CurveOnSurf(thearc,surf2);
 323     }
 324     TheBlendTool::Bounds(thecur, ufirst,ulast);
 325
 326     Iter->Initialize(thearc);
 327     Iter->InitVertexIterator();
 328     IsVtx = !Iter->MoreVertex();
 329     while (!IsVtx) {
 330       Vtx = Iter->Vertex();
 331       vtol = 0.4*Abs(ulast-ufirst); // Un majorant de la tolerance
 332       if (vtol > Max(TheBlendTool::Tolerance(Vtx,thearc), toler(1)))
 333         vtol = Max(TheBlendTool::Tolerance(Vtx,thearc), toler(1));
 334       if (Abs(TheBlendTool::Parameter(Vtx,thearc)-solrst(1)) <= vtol) {
 335         IsVtx = Standard_True; // On est dans la boule du vertex ou
 336                                // le vertex est dans la "boule" du recadrage
 337       }
 338       else {
 339         Iter->NextVertex();
 340         IsVtx = !Iter->MoreVertex();
 341       }
 342     }
 343     if (!Iter->MoreVertex()) {
 344       IsVtx = Standard_False;
 345     }
 346     return Standard_True;
 347   }
 348   return Standard_False;
 349 }
 350
 351
 352 void Blend_Walking::Transition(const Standard_Boolean OnFirst,
 353                                const TheArc& A,
 354                                const Standard_Real Param,
 355                                IntSurf_Transition& TLine,
 356                                IntSurf_Transition& TArc)
 357 {
 358   Standard_Boolean computetranstionaveclacorde = 0;
 359   gp_Vec tgline;
 360   Blend_Point prevprev;
 361
 362   if(previousP.IsTangencyPoint()){
 363     if(line->NbPoints() < 2) return;
 364     computetranstionaveclacorde = 1;
 365     if(sens < 0){
 366       prevprev = line->Point(2);
 367     }
 368     else {
 369       prevprev = line->Point(line->NbPoints() - 1);
 370     }
 371   }
 372   gp_Pnt2d p2d;
 373   gp_Vec2d dp2d;
 374
 375   gp_Pnt pbid;
 376   gp_Vec d1u,d1v,normale,tgrst;
 377   gp_Dir thenormal;
 378   CSLib_NormalStatus stat;
 379
 380   TheArcTool::D1(A,Param,p2d,dp2d);
 381   if (OnFirst) {
 382     TheSurfaceTool::D1(surf1,p2d.X(),p2d.Y(),pbid,d1u,d1v);
 383     if(!computetranstionaveclacorde) tgline = previousP.TangentOnS1();
 384     else tgline = gp_Vec(prevprev.PointOnS1(),previousP.PointOnS1());
 385   }
 386   else {
 387     TheSurfaceTool::D1(surf2,p2d.X(),p2d.Y(),pbid,d1u,d1v);
 388     if(!computetranstionaveclacorde) tgline = previousP.TangentOnS2();
 389     else tgline = gp_Vec(prevprev.PointOnS2(),previousP.PointOnS2());
 390   }
 391
 392   tgrst.SetLinearForm(dp2d.X(),d1u,dp2d.Y(),d1v);
 393
 394   CSLib::Normal(d1u, d1v, 1.e-9, stat, thenormal);
 395   if (stat ==  CSLib_Defined) normale.SetXYZ(thenormal.XYZ());
 396   else {
 397     Handle(Adaptor3d_HSurface) surf;
 398     if (OnFirst) surf = surf1;
 399     else         surf = surf2;
 400     Standard_Integer iu, iv;
 401     TColgp_Array2OfVec Der(0, 2 , 0, 2);
 402     TheSurfaceTool::D2(surf,p2d.X(),p2d.Y(),pbid, Der(1,0), Der(0,1),
 403                        Der(2,0), Der(0,2), Der(1,1));
 404     Der(2,1) = TheSurfaceTool::DN(surf, p2d.X(), p2d.Y(), 2,1);
 405     Der(1,2) = TheSurfaceTool::DN(surf,p2d.X(),p2d.Y(), 1,2);
 406     Der(2,2) = TheSurfaceTool::DN(surf,p2d.X(),p2d.Y(), 2,2);
 407     CSLib::Normal(2, Der, 1.e-9,
 408                   p2d.X(), p2d.Y(),
 409                   TheSurfaceTool::FirstUParameter(surf),
 410                   TheSurfaceTool::LastUParameter(surf),
 411                   TheSurfaceTool::FirstVParameter(surf),
 412                   TheSurfaceTool::LastVParameter(surf),
 413                   stat, thenormal, iu, iv);
 414     normale.SetXYZ(thenormal.XYZ());
 415 #if DEB
 416     if (stat == CSLib_InfinityOfSolutions)
 417       cout << "Blend_Walking::Transition : Infinite de Normal" << endl;
 418 #endif
 419   }
 420
 421   IntSurf::MakeTransition(tgline,tgrst,normale,TLine,TArc);
 422
 423 }
 424
 425
 426 void Blend_Walking::MakeExtremity(TheExtremity& Extrem,
 427                                   const Standard_Boolean OnFirst,
 428                                   const Standard_Integer Index,
 429                                   const Standard_Real Param,
 430                                   const Standard_Boolean IsVtx,
 431                                   const TheVertex& Vtx)
 432 {
 433
 434   IntSurf_Transition Tline,Tarc;
 435   Standard_Integer nbarc;
 436   Handle(TheTopolTool) Iter;
 437
 438   if (OnFirst) {
 439     Extrem.SetValue(previousP.PointOnS1(),
 440                     sol(1),sol(2),
 441                     previousP.Parameter(), tolesp);
 442     if (!previousP.IsTangencyPoint())
 443       Extrem.SetTangent(previousP.TangentOnS1());
 444     Iter = recdomain1;
 445   }
 446   else {
 447     Extrem.SetValue(previousP.PointOnS2(),
 448                     sol(3),sol(4),
 449                     previousP.Parameter(), tolesp);
 450     if (!previousP.IsTangencyPoint())
 451       Extrem.SetTangent(previousP.TangentOnS2());
 452     Iter = recdomain2;
 453   }
 454
 455   Iter->Init();
 456   nbarc = 1;
 457
 458   while (nbarc < Index) {
 459     nbarc++;
 460     Iter->Next();
 461   }
 462
 463   Transition(OnFirst,Iter->Value(),Param,Tline,Tarc);
 464   Extrem.AddArc(Iter->Value(),Param,Tline,Tarc);
 465   if (IsVtx) Extrem.SetVertex(Vtx);
 466 }
 467
 468 void  Blend_Walking::MakeSingularExtremity( TheExtremity& Extrem,
 469                                            const Standard_Boolean OnFirst,
 470                                            const TheVertex& Vtx)
 471 {
 472   IntSurf_Transition Tline,Tarc;
 473   Handle(TheTopolTool) Iter;
 474   Standard_Real prm;
 475
 476   if (OnFirst) {
 477     Iter = recdomain1;
 478     if (!previousP.IsTangencyPoint())
 479       Extrem.SetTangent(previousP.TangentOnS1());
 480   }
 481   else {
 482     if (!previousP.IsTangencyPoint())
 483       Extrem.SetTangent(previousP.TangentOnS2());
 484     Iter = recdomain2;
 485   }
 486
 487   Iter->Init();
 488   Extrem.SetVertex(Vtx);
 489   while (Iter->More()) {
 490     TheArc arc = Iter->Value();
 491     Iter->Initialize(arc);
 492     Iter->InitVertexIterator();
 493     while (Iter->MoreVertex()) {
 494       if (Iter->Identical(Vtx,Iter->Vertex())) {
 495         prm = TheBlendTool::Parameter(Vtx,arc);
 496         Transition(OnFirst,arc,prm,Tline,Tarc);
 497         Extrem.AddArc(arc,prm,Tline,Tarc);
 498       }
 499       Iter->NextVertex();
 500     }
 501     Iter->Next();
 502   }
 503 }
 504
 505
 506
 507
 508
 509
 510
 511
 512
 513
 514