b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
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 |
973c2be1 |
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. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | //26-04-1997 modified by pmn : Initialisation plus fine de la resolution |
16 | |
17 | Standard_Integer Blend_Walking::ArcToRecadre(const Standard_Boolean OnFirst, |
75259fc5 |
18 | const math_Vector& theSol, |
7fd59977 |
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(); |
1d47d8d0 |
28 | Standard_Real uprev = 0.,vprev = 0., prm = 0., dist = 0.; |
7fd59977 |
29 | Handle(TheTopolTool) Iter; |
30 | |
31 | if (OnFirst) { |
32 | if(byinter) previousP.ParametersOnS1(uprev,vprev); |
75259fc5 |
33 | pt2d.SetCoord(theSol(1),theSol(2)); |
7fd59977 |
34 | Iter = recdomain1; |
35 | } |
36 | else { |
37 | if(byinter) previousP.ParametersOnS2(uprev,vprev); |
75259fc5 |
38 | pt2d.SetCoord(theSol(3),theSol(4)); |
7fd59977 |
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, |
75259fc5 |
74 | const math_Vector& theSol, |
7fd59977 |
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); |
7fd59977 |
85 | Standard_Integer LeJalon = 0; |
1d47d8d0 |
86 | |
7fd59977 |
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 | |
75259fc5 |
99 | Indexsol = ArcToRecadre(OnFirst, theSol, 0, |
7fd59977 |
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); |
75259fc5 |
174 | Pnt2.SetCoord(theSol(3), theSol(4)); |
7fd59977 |
175 | } |
176 | else { |
177 | previousP.ParametersOnS1(u,v); |
178 | Pnt1.SetCoord(u, v); |
75259fc5 |
179 | Pnt2.SetCoord(theSol(1), theSol(2)); |
7fd59977 |
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) { |
75259fc5 |
195 | solrst(3) = theSol(3); |
196 | solrst(4) = theSol(4); |
7fd59977 |
197 | } |
198 | else { |
75259fc5 |
199 | solrst(3) = theSol(1); |
200 | solrst(4) = theSol(2); |
7fd59977 |
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()) { |
0797d9d3 |
272 | #ifdef OCCT_DEBUG |
7fd59977 |
273 | cout << "Walking::Recadre : RSNLD not done " << endl; |
aefdc31b |
274 | #endif |
7fd59977 |
275 | recadre = Standard_False; |
276 | } |
277 | else { |
278 | rsnld.Root(solrst); |
279 | recadre = FuncInv.IsSolution(solrst,tolesp); |
280 | } |
281 | } |
282 | |
283 | // En cas d'echecs, on regarde si un autre arc |
284 | // peut faire l'affaire (cas des sorties a proximite d'un vertex) |
285 | dist = (ulast - ufirst)/100; |
286 | if ((!recadre) && |
287 | ((Abs(pmin-ulast) < dist) || (Abs(pmin-ufirst) < dist)) ) { |
288 | |
75259fc5 |
289 | Indexsol = ArcToRecadre(OnFirst, theSol, Indexsol, |
7fd59977 |
290 | lastpt2d, pt2d, pmin); |
291 | if (Indexsol == 0) { |
292 | return Standard_False; |
293 | } |
294 | |
295 | Iter->Init(); |
296 | nbarc = 1; |
297 | while (nbarc < Indexsol) { |
298 | nbarc++; |
299 | Iter->Next(); |
300 | } |
301 | thearc = Iter->Value(); |
302 | |
303 | if (OnFirst) { |
304 | thecur = TheBlendTool::CurveOnSurf(thearc,surf1); |
305 | } |
306 | else { |
307 | thecur = TheBlendTool::CurveOnSurf(thearc,surf2); |
308 | } |
309 | solrst(1) = pmin; |
310 | // Le probleme a resoudre |
311 | FuncInv.Set(OnFirst,thecur); |
312 | FuncInv.GetBounds(infb,supb); |
313 | FuncInv.GetTolerance(toler,tolesp/10);//Il vaut mieux garder un peu de marge |
51740958 |
314 | math_FunctionSetRoot aRsnld(FuncInv,toler,35); |
7fd59977 |
315 | toler *= 10; // Mais on fait les tests correctements |
316 | // Resolution... |
51740958 |
317 | aRsnld.Perform(FuncInv,solrst,infb,supb); |
7fd59977 |
318 | |
51740958 |
319 | if (!aRsnld.IsDone()) { |
0797d9d3 |
320 | #ifdef OCCT_DEBUG |
7fd59977 |
321 | cout << "Walking::Recadre : RSNLD not done " << endl; |
aefdc31b |
322 | #endif |
7fd59977 |
323 | recadre = Standard_False; |
324 | } |
325 | else { |
51740958 |
326 | aRsnld.Root(solrst); |
7fd59977 |
327 | recadre = FuncInv.IsSolution(solrst,tolesp); |
328 | } |
329 | } |
330 | |
331 | if (recadre) { |
332 | // Classification topologique |
333 | if (OnFirst) { |
334 | thecur = TheBlendTool::CurveOnSurf(thearc,surf1); |
335 | } |
336 | else { |
337 | thecur = TheBlendTool::CurveOnSurf(thearc,surf2); |
338 | } |
339 | TheBlendTool::Bounds(thecur, ufirst,ulast); |
340 | |
341 | Iter->Initialize(thearc); |
342 | Iter->InitVertexIterator(); |
343 | IsVtx = !Iter->MoreVertex(); |
344 | while (!IsVtx) { |
345 | Vtx = Iter->Vertex(); |
346 | vtol = 0.4*Abs(ulast-ufirst); // Un majorant de la tolerance |
347 | if (vtol > Max(TheBlendTool::Tolerance(Vtx,thearc), toler(1))) |
348 | vtol = Max(TheBlendTool::Tolerance(Vtx,thearc), toler(1)); |
349 | if (Abs(TheBlendTool::Parameter(Vtx,thearc)-solrst(1)) <= vtol) { |
350 | IsVtx = Standard_True; // On est dans la boule du vertex ou |
351 | // le vertex est dans la "boule" du recadrage |
352 | } |
353 | else { |
354 | Iter->NextVertex(); |
355 | IsVtx = !Iter->MoreVertex(); |
356 | } |
357 | } |
358 | if (!Iter->MoreVertex()) { |
359 | IsVtx = Standard_False; |
360 | } |
361 | return Standard_True; |
362 | } |
363 | return Standard_False; |
364 | } |
365 | |
366 | |
367 | void Blend_Walking::Transition(const Standard_Boolean OnFirst, |
368 | const TheArc& A, |
369 | const Standard_Real Param, |
370 | IntSurf_Transition& TLine, |
371 | IntSurf_Transition& TArc) |
372 | { |
373 | Standard_Boolean computetranstionaveclacorde = 0; |
374 | gp_Vec tgline; |
375 | Blend_Point prevprev; |
376 | |
377 | if(previousP.IsTangencyPoint()){ |
378 | if(line->NbPoints() < 2) return; |
379 | computetranstionaveclacorde = 1; |
380 | if(sens < 0){ |
381 | prevprev = line->Point(2); |
382 | } |
383 | else { |
384 | prevprev = line->Point(line->NbPoints() - 1); |
385 | } |
386 | } |
387 | gp_Pnt2d p2d; |
388 | gp_Vec2d dp2d; |
389 | |
390 | gp_Pnt pbid; |
391 | gp_Vec d1u,d1v,normale,tgrst; |
392 | gp_Dir thenormal; |
393 | CSLib_NormalStatus stat; |
394 | |
395 | TheArcTool::D1(A,Param,p2d,dp2d); |
396 | if (OnFirst) { |
397 | TheSurfaceTool::D1(surf1,p2d.X(),p2d.Y(),pbid,d1u,d1v); |
398 | if(!computetranstionaveclacorde) tgline = previousP.TangentOnS1(); |
399 | else tgline = gp_Vec(prevprev.PointOnS1(),previousP.PointOnS1()); |
400 | } |
401 | else { |
402 | TheSurfaceTool::D1(surf2,p2d.X(),p2d.Y(),pbid,d1u,d1v); |
403 | if(!computetranstionaveclacorde) tgline = previousP.TangentOnS2(); |
404 | else tgline = gp_Vec(prevprev.PointOnS2(),previousP.PointOnS2()); |
405 | } |
406 | |
407 | tgrst.SetLinearForm(dp2d.X(),d1u,dp2d.Y(),d1v); |
408 | |
409 | CSLib::Normal(d1u, d1v, 1.e-9, stat, thenormal); |
410 | if (stat == CSLib_Defined) normale.SetXYZ(thenormal.XYZ()); |
411 | else { |
412 | Handle(Adaptor3d_HSurface) surf; |
413 | if (OnFirst) surf = surf1; |
414 | else surf = surf2; |
415 | Standard_Integer iu, iv; |
416 | TColgp_Array2OfVec Der(0, 2 , 0, 2); |
417 | TheSurfaceTool::D2(surf,p2d.X(),p2d.Y(),pbid, Der(1,0), Der(0,1), |
418 | Der(2,0), Der(0,2), Der(1,1)); |
419 | Der(2,1) = TheSurfaceTool::DN(surf, p2d.X(), p2d.Y(), 2,1); |
420 | Der(1,2) = TheSurfaceTool::DN(surf,p2d.X(),p2d.Y(), 1,2); |
421 | Der(2,2) = TheSurfaceTool::DN(surf,p2d.X(),p2d.Y(), 2,2); |
422 | CSLib::Normal(2, Der, 1.e-9, |
423 | p2d.X(), p2d.Y(), |
424 | TheSurfaceTool::FirstUParameter(surf), |
425 | TheSurfaceTool::LastUParameter(surf), |
426 | TheSurfaceTool::FirstVParameter(surf), |
427 | TheSurfaceTool::LastVParameter(surf), |
428 | stat, thenormal, iu, iv); |
429 | normale.SetXYZ(thenormal.XYZ()); |
0797d9d3 |
430 | #ifdef OCCT_DEBUG |
7fd59977 |
431 | if (stat == CSLib_InfinityOfSolutions) |
432 | cout << "Blend_Walking::Transition : Infinite de Normal" << endl; |
433 | #endif |
434 | } |
435 | |
436 | IntSurf::MakeTransition(tgline,tgrst,normale,TLine,TArc); |
437 | |
438 | } |
439 | |
440 | |
441 | void Blend_Walking::MakeExtremity(TheExtremity& Extrem, |
442 | const Standard_Boolean OnFirst, |
443 | const Standard_Integer Index, |
444 | const Standard_Real Param, |
445 | const Standard_Boolean IsVtx, |
446 | const TheVertex& Vtx) |
447 | { |
448 | |
449 | IntSurf_Transition Tline,Tarc; |
450 | Standard_Integer nbarc; |
451 | Handle(TheTopolTool) Iter; |
452 | |
453 | if (OnFirst) { |
454 | Extrem.SetValue(previousP.PointOnS1(), |
455 | sol(1),sol(2), |
456 | previousP.Parameter(), tolesp); |
457 | if (!previousP.IsTangencyPoint()) |
458 | Extrem.SetTangent(previousP.TangentOnS1()); |
459 | Iter = recdomain1; |
460 | } |
461 | else { |
462 | Extrem.SetValue(previousP.PointOnS2(), |
463 | sol(3),sol(4), |
464 | previousP.Parameter(), tolesp); |
465 | if (!previousP.IsTangencyPoint()) |
466 | Extrem.SetTangent(previousP.TangentOnS2()); |
467 | Iter = recdomain2; |
468 | } |
469 | |
470 | Iter->Init(); |
471 | nbarc = 1; |
472 | |
473 | while (nbarc < Index) { |
474 | nbarc++; |
475 | Iter->Next(); |
476 | } |
477 | |
478 | Transition(OnFirst,Iter->Value(),Param,Tline,Tarc); |
479 | Extrem.AddArc(Iter->Value(),Param,Tline,Tarc); |
480 | if (IsVtx) Extrem.SetVertex(Vtx); |
481 | } |
482 | |
483 | void Blend_Walking::MakeSingularExtremity( TheExtremity& Extrem, |
484 | const Standard_Boolean OnFirst, |
485 | const TheVertex& Vtx) |
486 | { |
487 | IntSurf_Transition Tline,Tarc; |
488 | Handle(TheTopolTool) Iter; |
489 | Standard_Real prm; |
490 | |
491 | if (OnFirst) { |
492 | Iter = recdomain1; |
493 | if (!previousP.IsTangencyPoint()) |
494 | Extrem.SetTangent(previousP.TangentOnS1()); |
495 | } |
496 | else { |
497 | if (!previousP.IsTangencyPoint()) |
498 | Extrem.SetTangent(previousP.TangentOnS2()); |
499 | Iter = recdomain2; |
500 | } |
501 | |
502 | Iter->Init(); |
503 | Extrem.SetVertex(Vtx); |
504 | while (Iter->More()) { |
505 | TheArc arc = Iter->Value(); |
506 | Iter->Initialize(arc); |
507 | Iter->InitVertexIterator(); |
508 | while (Iter->MoreVertex()) { |
509 | if (Iter->Identical(Vtx,Iter->Vertex())) { |
510 | prm = TheBlendTool::Parameter(Vtx,arc); |
511 | Transition(OnFirst,arc,prm,Tline,Tarc); |
512 | Extrem.AddArc(arc,prm,Tline,Tarc); |
513 | } |
514 | Iter->NextVertex(); |
515 | } |
516 | Iter->Next(); |
517 | } |
518 | } |
519 | |
520 | |
521 | |
522 | |
523 | |
524 | |
525 | |
526 | |
527 | |
528 | |
529 | |