7fd59977 |
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 | |