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