b311480e |
1 | // Created on: 1997-02-10 |
2 | // Created by: Jacques GOUSSARD |
3 | // Copyright (c) 1997-1999 Matra Datavision |
4 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
5 | // |
6 | // The content of this file is subject to the Open CASCADE Technology Public |
7 | // License Version 6.5 (the "License"). You may not use the content of this file |
8 | // except in compliance with the License. Please obtain a copy of the License |
9 | // at http://www.opencascade.org and read it completely before using this file. |
10 | // |
11 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
12 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
13 | // |
14 | // The Original Code and all software distributed under the License is |
15 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
16 | // Initial Developer hereby disclaims all such warranties, including without |
17 | // limitation, any warranties of merchantability, fitness for a particular |
18 | // purpose or non-infringement. Please see the License for the specific terms |
19 | // and conditions governing the rights and limitations under the License. |
20 | |
0d969553 |
21 | |
7fd59977 |
22 | #include <BRepBlend_RstRstEvolRad.ixx> |
23 | #include <math_Gauss.hxx> |
24 | #include <math_SVD.hxx> |
25 | |
26 | #include <ElCLib.hxx> |
27 | #include <gp.hxx> |
28 | #include <BlendFunc.hxx> |
29 | #include <GeomFill.hxx> |
30 | #include <TColStd_SequenceOfReal.hxx> |
31 | #include <Standard_DomainError.hxx> |
32 | #include <Standard_NotImplemented.hxx> |
33 | #include <Precision.hxx> |
34 | |
35 | #define Eps 1.e-15 |
36 | |
37 | static void t3dto2d(Standard_Real& a, |
38 | Standard_Real& b, |
39 | const gp_Vec& A, |
40 | const gp_Vec& B, |
41 | const gp_Vec& C) |
42 | { |
43 | Standard_Real AB = A.Dot(B); |
44 | Standard_Real AC = A.Dot(C); |
45 | Standard_Real BC = B.Dot(C); |
46 | Standard_Real BB = B.Dot(B); |
47 | Standard_Real CC = C.Dot(C); |
48 | Standard_Real deno = (BB*CC-BC*BC); |
49 | a = (AB*CC-AC*BC)/deno; |
50 | b = (AC*BB-AB*BC)/deno; |
51 | } |
52 | |
53 | |
54 | static void FusionneIntervalles(const TColStd_Array1OfReal& I1, |
55 | const TColStd_Array1OfReal& I2, |
56 | TColStd_SequenceOfReal& Seq) |
57 | { |
58 | Standard_Integer ind1=1, ind2=1; |
59 | Standard_Real Epspar = Precision::PConfusion()*0.99; |
81bba717 |
60 | // supposed that the positioning works with PConfusion()/2 |
7fd59977 |
61 | Standard_Real v1, v2; |
81bba717 |
62 | // Initialisations : IND1 and IND2 point the 1st element |
63 | // of each of 2 tables to be processed. INDS points at the last |
64 | // created element of TABSOR |
7fd59977 |
65 | |
66 | |
81bba717 |
67 | //--- TABSOR is filled by parsing TABLE1 and TABLE2 simultaneously --- |
68 | //------------------ and removing multiple occurrencies ------------ |
7fd59977 |
69 | |
70 | while ((ind1<=I1.Upper()) && (ind2<=I2.Upper())) { |
71 | v1 = I1(ind1); |
72 | v2 = I2(ind2); |
73 | if (Abs(v1-v2)<= Epspar) { |
81bba717 |
74 | // elements of I1 and I2 fit here |
7fd59977 |
75 | Seq.Append((v1+v2)/2); |
76 | ind1++; |
77 | ind2++; |
78 | } |
79 | else if (v1 < v2) { |
81bba717 |
80 | // element of I1 fits here. |
7fd59977 |
81 | Seq.Append(v1); |
82 | ind1++; |
83 | } |
84 | else { |
81bba717 |
85 | // element of TABLE2 fits here. |
7fd59977 |
86 | Seq.Append(v2); |
87 | ind2++; |
88 | } |
89 | } |
90 | |
91 | if (ind1>I1.Upper()) { |
81bba717 |
92 | //----- Here I1 is exhausted, completed using the end of TABLE2 ------- |
7fd59977 |
93 | |
94 | for (; ind2<=I2.Upper(); ind2++) { |
95 | Seq.Append(I2(ind2)); |
96 | } |
97 | } |
98 | |
99 | if (ind2>I2.Upper()) { |
81bba717 |
100 | //----- Here I2 is exhausted, completed using the end of I1 ------- |
7fd59977 |
101 | |
102 | for (; ind1<=I1.Upper(); ind1++) { |
103 | Seq.Append(I1(ind1)); |
104 | } |
105 | } |
106 | } |
107 | |
108 | |
109 | |
110 | //======================================================================= |
111 | //function : BRepBlend_RstRstEvolRad |
112 | //purpose : |
113 | //======================================================================= |
114 | |
115 | BRepBlend_RstRstEvolRad::BRepBlend_RstRstEvolRad |
116 | (const Handle(Adaptor3d_HSurface)& Surf1, |
117 | const Handle(Adaptor2d_HCurve2d)& Rst1, |
118 | const Handle(Adaptor3d_HSurface)& Surf2, |
119 | const Handle(Adaptor2d_HCurve2d)& Rst2, |
120 | const Handle(Adaptor3d_HCurve)& CGuide, |
121 | const Handle(Law_Function)& Evol): |
122 | surf1(Surf1), surf2(Surf2), rst1(Rst1), rst2(Rst2), |
123 | cons1(Rst1, Surf1), cons2(Rst2, Surf2), |
124 | guide(CGuide), tguide(CGuide), |
125 | istangent(Standard_True), maxang(RealFirst()), minang(RealLast()), |
126 | distmin(RealLast()), |
127 | mySShape(BlendFunc_Rational) |
128 | { |
129 | tevol=Evol; |
130 | fevol=Evol; |
131 | } |
132 | |
133 | //======================================================================= |
134 | //function : NbVariables |
135 | //purpose : |
136 | //======================================================================= |
137 | |
138 | Standard_Integer BRepBlend_RstRstEvolRad::NbVariables() const |
139 | { |
140 | return 2; |
141 | } |
142 | |
143 | //======================================================================= |
144 | //function : NbEquations |
145 | //purpose : |
146 | //======================================================================= |
147 | |
148 | Standard_Integer BRepBlend_RstRstEvolRad::NbEquations() const |
149 | { |
150 | return 2; |
151 | } |
152 | |
153 | //======================================================================= |
154 | //function : Value |
155 | //purpose : |
156 | //======================================================================= |
157 | |
158 | Standard_Boolean BRepBlend_RstRstEvolRad::Value(const math_Vector& X, |
159 | math_Vector& F) |
160 | { |
161 | ptrst1 = cons1.Value(X(1)); |
162 | ptrst2 = cons2.Value(X(2)); |
163 | |
164 | F(1) = nplan.XYZ().Dot(ptrst1.XYZ()) + theD; |
165 | F(2) = nplan.XYZ().Dot(ptrst2.XYZ()) + theD; |
166 | |
167 | return Standard_True; |
168 | } |
169 | |
170 | //======================================================================= |
171 | //function : Derivatives |
172 | //purpose : |
173 | //======================================================================= |
174 | |
175 | Standard_Boolean BRepBlend_RstRstEvolRad::Derivatives(const math_Vector& X, |
176 | math_Matrix& D) |
177 | { |
178 | gp_Vec d11, d21; |
179 | |
180 | cons1.D1(X(1), ptrst1, d11); |
181 | cons2.D1(X(2), ptrst2, d21); |
182 | |
183 | D(1,1) = nplan.Dot(d11); |
184 | D(1,2) = 0.; |
185 | |
186 | D(2,1) = 0.; |
187 | D(2,2) = nplan.Dot(d21); |
188 | |
189 | return Standard_True; |
190 | } |
191 | |
192 | //======================================================================= |
193 | //function : Values |
194 | //purpose : |
195 | //======================================================================= |
196 | |
197 | Standard_Boolean BRepBlend_RstRstEvolRad::Values(const math_Vector& X, |
198 | math_Vector& F, |
199 | math_Matrix& D) |
200 | { |
201 | Standard_Boolean Error; |
202 | |
203 | Error = Value(X, F); |
204 | Error = Derivatives(X, D); |
205 | |
206 | return Standard_True; |
207 | } |
208 | |
209 | //======================================================================= |
210 | //function : Set |
211 | //purpose : |
212 | //======================================================================= |
213 | |
214 | void BRepBlend_RstRstEvolRad::Set(const Handle(Adaptor3d_HSurface)& SurfRef1, |
215 | const Handle(Adaptor2d_HCurve2d)& RstRef1, |
216 | const Handle(Adaptor3d_HSurface)& SurfRef2, |
217 | const Handle(Adaptor2d_HCurve2d)& RstRef2) |
218 | { |
219 | surfref1 = SurfRef1; |
220 | surfref2 = SurfRef2; |
221 | rstref1 = RstRef1; |
222 | rstref2 = RstRef2; |
223 | } |
224 | |
225 | //======================================================================= |
226 | //function : Set |
227 | //purpose : |
228 | //======================================================================= |
229 | |
230 | void BRepBlend_RstRstEvolRad::Set(const Standard_Real Param) |
231 | { |
232 | d1gui = gp_Vec(0.,0.,0.); |
233 | nplan = gp_Vec(0.,0.,0.); |
234 | tguide->D2(Param, ptgui, d1gui, d2gui); |
235 | normtg = d1gui.Magnitude(); |
236 | nplan.SetXYZ(d1gui.Normalized().XYZ()); |
237 | gp_XYZ nplanXYZ(nplan.XYZ()); |
238 | gp_XYZ ptguiXYZ(ptgui.XYZ()); |
239 | theD = nplanXYZ.Dot(ptguiXYZ) ; |
240 | theD = theD * (-1.) ; |
241 | // theD = - (nplan.XYZ().Dot(ptgui.XYZ())); |
242 | tevol->D1(Param,ray,dray); |
243 | |
244 | } |
245 | |
246 | //======================================================================= |
247 | //function : Set |
248 | //purpose : |
249 | //======================================================================= |
250 | |
251 | void BRepBlend_RstRstEvolRad::Set(const Standard_Real First, |
252 | const Standard_Real Last) |
253 | { |
254 | tguide = guide->Trim(First, Last, 1.e-12); |
255 | tevol = fevol->Trim(First,Last,1.e-12); |
256 | } |
257 | |
258 | //======================================================================= |
259 | //function : GetTolerance |
260 | //purpose : |
261 | //======================================================================= |
262 | |
263 | void BRepBlend_RstRstEvolRad::GetTolerance(math_Vector& Tolerance, |
264 | const Standard_Real Tol) const |
265 | { |
266 | Tolerance(1) = cons1.Resolution(Tol); |
267 | Tolerance(2) = cons2.Resolution(Tol); |
268 | } |
269 | |
270 | //======================================================================= |
271 | //function : GetBounds |
272 | //purpose : |
273 | //======================================================================= |
274 | |
275 | void BRepBlend_RstRstEvolRad::GetBounds(math_Vector& InfBound, |
276 | math_Vector& SupBound) const |
277 | { |
278 | InfBound(1) = cons1.FirstParameter(); |
279 | InfBound(2) = cons2.FirstParameter(); |
280 | SupBound(1) = cons1.LastParameter(); |
281 | SupBound(2) = cons2.LastParameter(); |
282 | |
283 | } |
284 | |
285 | //======================================================================= |
286 | //function : IsSolution |
287 | //purpose : |
288 | //======================================================================= |
289 | |
290 | Standard_Boolean BRepBlend_RstRstEvolRad::IsSolution(const math_Vector& Sol, |
291 | const Standard_Real Tol) |
292 | |
293 | |
294 | { |
295 | math_Vector valsol(1, 2), secmember(1, 2); |
296 | math_Matrix gradsol(1, 2, 1, 2); |
297 | |
298 | gp_Vec dnplan, d1urst1, d1vrst1, d1urst2, d1vrst2, d11, d21, temp; |
299 | gp_Pnt bid; |
300 | |
301 | Standard_Real Cosa, Sina, Angle; |
302 | |
303 | Values(Sol, valsol, gradsol); |
304 | |
305 | if (Abs(valsol(1)) <= Tol && |
306 | Abs(valsol(2)) <= Tol ) { |
307 | |
81bba717 |
308 | // Calculation of tangents |
7fd59977 |
309 | prmrst1 = Sol(1); |
310 | pt2drst1 = rst1->Value(prmrst1); |
311 | prmrst2 = Sol(2); |
312 | pt2drst2 = rst2->Value(prmrst2); |
313 | |
314 | cons1.D1(Sol(1), ptrst1, d11); |
315 | cons2.D1(Sol(2), ptrst2, d21); |
316 | |
317 | dnplan.SetLinearForm(1./normtg, d2gui, |
318 | -1./normtg * (nplan.Dot(d2gui)), nplan); |
319 | |
320 | temp.SetXYZ(ptrst1.XYZ() - ptgui.XYZ()); |
321 | secmember(1) = normtg - dnplan.Dot(temp); |
322 | |
323 | temp.SetXYZ(ptrst2.XYZ() - ptgui.XYZ()); |
324 | secmember(2) = normtg - dnplan.Dot(temp); |
325 | |
326 | math_Gauss Resol(gradsol); |
327 | |
328 | if (Resol.IsDone()) { |
329 | Resol.Solve(secmember); |
330 | istangent = Standard_False; |
331 | } |
332 | else { |
333 | math_SVD SingRS (gradsol); |
334 | if (SingRS.IsDone()) { |
335 | math_Vector DEDT(1,3); |
336 | DEDT = secmember; |
337 | SingRS.Solve(DEDT, secmember, 1.e-6); |
338 | istangent = Standard_False; |
339 | } |
340 | else istangent = Standard_True; |
341 | } |
342 | |
343 | |
344 | if (!istangent) { |
345 | tgrst1 = secmember(1) * d11; |
346 | tgrst2 = secmember(2) * d21; |
347 | |
348 | Standard_Real a, b; |
349 | surf1->D1(pt2drst1.X(), pt2drst1.Y(), bid, d1urst1, d1vrst1); |
350 | t3dto2d(a, b, tgrst1, d1urst1, d1vrst1); |
351 | tg2drst1.SetCoord(a, b); |
352 | surf2->D1(pt2drst2.X(), pt2drst2.Y(), bid, d1urst2, d1vrst2); |
353 | t3dto2d(a, b, tgrst1, d1urst2, d1vrst2); |
354 | tg2drst2.SetCoord(a, b); |
355 | } |
356 | |
357 | gp_Pnt Center; |
358 | gp_Vec NotUsed; |
359 | Standard_Boolean IsCenter; |
360 | |
361 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, NotUsed); |
362 | |
363 | if (!IsCenter) return Standard_False; |
364 | |
365 | gp_Vec n1(Center, ptrst1) , n2(Center, ptrst2); |
366 | |
367 | n1.Normalize(); |
368 | n2.Normalize(); |
369 | |
370 | Cosa = n1.Dot(n2); |
371 | Sina = nplan.Dot(n1.Crossed(n2)); |
372 | |
373 | if (choix%2 != 0) { |
0d969553 |
374 | Sina = -Sina; //nplan is changed into -nplan |
7fd59977 |
375 | } |
376 | |
377 | Angle = ACos(Cosa); |
378 | if (Sina < 0.) { |
c6541a0c |
379 | Angle = 2.*M_PI - Angle; |
7fd59977 |
380 | } |
381 | |
382 | if (Angle > maxang) {maxang = Angle;} |
383 | if (Angle < minang) {minang = Angle;} |
384 | distmin = Min( distmin, ptrst1.Distance(ptrst2)); |
385 | |
386 | return Standard_True; |
387 | } |
388 | istangent = Standard_True; |
389 | return Standard_False; |
390 | } |
391 | |
392 | //======================================================================= |
393 | //function : GetMinimalDistance |
394 | //purpose : |
395 | //======================================================================= |
396 | |
397 | Standard_Real BRepBlend_RstRstEvolRad::GetMinimalDistance() const |
398 | { |
399 | return distmin; |
400 | } |
401 | |
402 | //======================================================================= |
403 | //function : PointOnRst1 |
404 | //purpose : |
405 | //======================================================================= |
406 | |
407 | const gp_Pnt& BRepBlend_RstRstEvolRad::PointOnRst1() const |
408 | { |
409 | return ptrst1; |
410 | } |
411 | |
412 | //======================================================================= |
413 | //function : PointOnRst2 |
414 | //purpose : |
415 | //======================================================================= |
416 | |
417 | const gp_Pnt& BRepBlend_RstRstEvolRad::PointOnRst2() const |
418 | { |
419 | return ptrst2; |
420 | } |
421 | |
422 | //======================================================================= |
423 | //function : Pnt2dOnRst1 |
424 | //purpose : |
425 | //======================================================================= |
426 | |
427 | const gp_Pnt2d& BRepBlend_RstRstEvolRad::Pnt2dOnRst1() const |
428 | { |
429 | return pt2drst1; |
430 | } |
431 | |
432 | //======================================================================= |
433 | //function : Pnt2dOnRst2 |
434 | //purpose : |
435 | //======================================================================= |
436 | |
437 | const gp_Pnt2d& BRepBlend_RstRstEvolRad::Pnt2dOnRst2() const |
438 | { |
439 | return pt2drst2; |
440 | } |
441 | |
442 | //======================================================================= |
443 | //function : ParameterOnRst1 |
444 | //purpose : |
445 | //======================================================================= |
446 | |
447 | Standard_Real BRepBlend_RstRstEvolRad::ParameterOnRst1() const |
448 | { |
449 | return prmrst1; |
450 | } |
451 | |
452 | //======================================================================= |
453 | //function : ParameterOnRst2 |
454 | //purpose : |
455 | //======================================================================= |
456 | |
457 | Standard_Real BRepBlend_RstRstEvolRad::ParameterOnRst2() const |
458 | { |
459 | return prmrst2; |
460 | } |
461 | //======================================================================= |
462 | //function : IsTangencyPoint |
463 | //purpose : |
464 | //======================================================================= |
465 | |
466 | Standard_Boolean BRepBlend_RstRstEvolRad::IsTangencyPoint() const |
467 | { |
468 | return istangent; |
469 | } |
470 | |
471 | //======================================================================= |
472 | //function : TangentOnRst1 |
473 | //purpose : |
474 | //======================================================================= |
475 | |
476 | const gp_Vec& BRepBlend_RstRstEvolRad::TangentOnRst1() const |
477 | { |
478 | if (istangent) {Standard_DomainError::Raise();} |
479 | return tgrst1; |
480 | } |
481 | |
482 | //======================================================================= |
483 | //function : Tangent2dOnRst1 |
484 | //purpose : |
485 | //======================================================================= |
486 | |
487 | const gp_Vec2d& BRepBlend_RstRstEvolRad::Tangent2dOnRst1() const |
488 | { |
489 | if (istangent) {Standard_DomainError::Raise();} |
490 | return tg2drst1; |
491 | } |
492 | |
493 | //======================================================================= |
494 | //function : TangentOnRst2 |
495 | //purpose : |
496 | //======================================================================= |
497 | |
498 | const gp_Vec& BRepBlend_RstRstEvolRad::TangentOnRst2() const |
499 | { |
500 | if (istangent) {Standard_DomainError::Raise();} |
501 | return tgrst2; |
502 | } |
503 | |
504 | //======================================================================= |
505 | //function : Tangent2dOnRst2 |
506 | //purpose : |
507 | //======================================================================= |
508 | |
509 | const gp_Vec2d& BRepBlend_RstRstEvolRad::Tangent2dOnRst2() const |
510 | { |
511 | if (istangent) {Standard_DomainError::Raise();} |
512 | return tg2drst2; |
513 | } |
514 | |
515 | //======================================================================= |
516 | //function : Decroch |
517 | //purpose : |
518 | //======================================================================= |
519 | |
520 | Blend_DecrochStatus BRepBlend_RstRstEvolRad::Decroch(const math_Vector& Sol, |
521 | gp_Vec& NRst1, |
522 | gp_Vec& TgRst1, |
523 | gp_Vec& NRst2, |
524 | gp_Vec& TgRst2)const |
525 | { |
526 | gp_Vec NRst1InPlane, NRst2InPlane; |
527 | gp_Pnt PtTmp1, PtTmp2, Center; |
528 | gp_Vec d1u, d1v, centptrst, NotUsed; |
529 | Standard_Real norm, unsurnorm; |
530 | Standard_Real u,v; |
531 | |
532 | rstref1->Value(Sol(1)).Coord(u, v); |
533 | surfref1->D1(u, v,PtTmp1,d1u,d1v); |
81bba717 |
534 | // Normal to the reference surface 1 |
7fd59977 |
535 | NRst1 = d1u.Crossed(d1v); |
536 | rstref2->Value(Sol(2)).Coord(u, v); |
537 | surfref2->D1(u, v, PtTmp2, d1u, d1v); |
81bba717 |
538 | // Normal to the reference surface 2 |
7fd59977 |
539 | NRst2 = d1u.Crossed(d1v); |
540 | |
541 | Standard_Boolean IsCenter; |
542 | |
543 | IsCenter = CenterCircleRst1Rst2(PtTmp1, PtTmp2, nplan, Center, NotUsed); |
544 | |
545 | norm = nplan.Crossed(NRst1).Magnitude(); |
546 | unsurnorm = 1. / norm; |
547 | |
548 | NRst1InPlane.SetLinearForm(nplan.Dot(NRst1) * unsurnorm, nplan, -unsurnorm, NRst1); |
549 | |
550 | centptrst.SetXYZ(PtTmp1.XYZ() - Center.XYZ()); |
551 | |
552 | if (centptrst.Dot(NRst1InPlane) < 0.) NRst1InPlane.Reverse(); |
553 | |
554 | TgRst1 = nplan.Crossed(centptrst); |
555 | |
556 | norm = nplan.Crossed(NRst2).Magnitude(); |
557 | unsurnorm = 1./ norm; |
558 | NRst2InPlane.SetLinearForm(nplan.Dot(NRst2) * unsurnorm, nplan, -unsurnorm, NRst2); |
559 | centptrst.SetXYZ(PtTmp2.XYZ() - Center.XYZ()); |
560 | |
561 | |
562 | if (centptrst.Dot(NRst2InPlane) < 0.) NRst2InPlane.Reverse(); |
563 | |
564 | TgRst2 = nplan.Crossed(centptrst); |
565 | |
566 | if (choix %2 != 0) { |
567 | TgRst1.Reverse(); |
568 | TgRst2.Reverse(); |
569 | } |
570 | |
81bba717 |
571 | // Vectors are returned |
7fd59977 |
572 | if (NRst1InPlane.Dot(TgRst1) > -1.e-10) { |
573 | if (NRst2InPlane.Dot(TgRst2) < 1.e-10) { |
574 | return Blend_DecrochBoth; |
575 | } |
576 | else { |
577 | return Blend_DecrochRst1; |
578 | } |
579 | } |
580 | else { |
581 | if (NRst2InPlane.Dot(TgRst2) < 1.e-10) { |
582 | return Blend_DecrochRst2; |
583 | } |
584 | else { |
585 | return Blend_NoDecroch; |
586 | } |
587 | } |
588 | |
589 | } |
590 | |
591 | //======================================================================= |
592 | //function : Set |
593 | //purpose : |
594 | //======================================================================= |
595 | |
596 | void BRepBlend_RstRstEvolRad::Set(const Standard_Integer Choix) |
597 | { |
598 | choix = Choix; |
599 | } |
600 | |
601 | //======================================================================= |
602 | //function : Set |
603 | //purpose : |
604 | //======================================================================= |
605 | |
606 | void BRepBlend_RstRstEvolRad::Set(const BlendFunc_SectionShape TypeSection) |
607 | { |
608 | mySShape = TypeSection; |
609 | } |
610 | |
611 | |
612 | |
613 | //======================================================================= |
614 | //function : CenterCircleRst1Rst2 |
81bba717 |
615 | //purpose : Calculate the center of circle passing by two points of restrictions |
7fd59977 |
616 | //======================================================================= |
617 | Standard_Boolean BRepBlend_RstRstEvolRad::CenterCircleRst1Rst2(const gp_Pnt& PtRst1, |
618 | const gp_Pnt& PtRst2, |
619 | const gp_Vec& np, |
620 | gp_Pnt& Center, |
621 | gp_Vec& VdMed) const |
622 | { |
623 | |
624 | gp_Vec rst1rst2(PtRst1, PtRst2); |
81bba717 |
625 | gp_Vec vdmedNor; //,NRst1; vdmedNor vector director of the perpendicular bisector |
7fd59977 |
626 | Standard_Real norm2; |
81bba717 |
627 | Standard_Real Dist;// distance between the middle of PtRst1,PtRst2 and Center |
7fd59977 |
628 | |
81bba717 |
629 | // Calculate the center of the circle |
7fd59977 |
630 | VdMed = rst1rst2.Crossed(np); |
631 | norm2 = rst1rst2.SquareMagnitude(); |
632 | Dist = ray * ray - 0.25 * norm2; |
633 | |
634 | if (choix > 2) { |
635 | VdMed.Reverse(); |
636 | } |
637 | |
638 | if (Dist < - 1.E-07) return Standard_False; |
639 | |
640 | if (Dist > 1.E-07) { |
641 | Dist = sqrt(Dist); |
642 | vdmedNor = VdMed.Normalized(); |
643 | Center.SetXYZ(0.5 * rst1rst2.XYZ() + PtRst1.XYZ() + Dist * vdmedNor.XYZ()); |
644 | } |
645 | else |
646 | { |
647 | Center.SetXYZ(0.5 * rst1rst2.XYZ() + PtRst1.XYZ()); |
648 | } |
649 | |
650 | return Standard_True; |
651 | |
652 | } |
653 | |
654 | |
655 | |
656 | |
657 | |
658 | |
659 | //======================================================================= |
660 | //function : Section |
661 | //purpose : |
662 | //======================================================================= |
663 | |
664 | void BRepBlend_RstRstEvolRad::Section(const Standard_Real Param, |
665 | const Standard_Real U, |
666 | const Standard_Real V, |
667 | Standard_Real& Pdeb, |
668 | Standard_Real& Pfin, |
669 | gp_Circ& C) |
670 | { |
671 | gp_Vec ns, np, NotUsed; |
672 | gp_Pnt Center; |
673 | |
674 | tguide->D1(Param, ptgui, d1gui); |
675 | ray = tevol->Value(Param); |
676 | np = d1gui.Normalized(); |
677 | ptrst1 = cons1.Value(U); |
678 | ptrst2 = cons2.Value(V); |
679 | |
680 | Standard_Boolean IsCenter; |
681 | |
682 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, np, Center, NotUsed); |
683 | |
684 | C.SetRadius(Abs(ray)); |
685 | ns = gp_Vec(Center, ptrst1).Normalized(); |
686 | |
687 | if (choix%2 != 0) { |
688 | np.Reverse(); |
689 | } |
690 | |
691 | C.SetPosition(gp_Ax2(Center, np, ns)); |
692 | Pdeb = 0; //ElCLib::Parameter(C, pts); |
693 | Pfin = ElCLib::Parameter(C, ptrst2); |
694 | |
81bba717 |
695 | // Test negative and quasi null angles: Special case |
c6541a0c |
696 | if (Pfin > 1.5 * M_PI) { |
7fd59977 |
697 | np.Reverse(); |
698 | C.SetPosition(gp_Ax2(Center, np, ns)); |
699 | Pfin = ElCLib::Parameter(C, ptrst2); |
700 | } |
701 | if (Pfin < Precision::PConfusion()) Pfin += Precision::PConfusion(); |
702 | } |
703 | |
704 | //======================================================================= |
705 | //function : IsRational |
706 | //purpose : |
707 | //======================================================================= |
708 | |
709 | Standard_Boolean BRepBlend_RstRstEvolRad::IsRational () const |
710 | { |
711 | return (mySShape==BlendFunc_Rational || mySShape==BlendFunc_QuasiAngular); |
712 | } |
713 | |
714 | //======================================================================= |
715 | //function : GetSectionSize |
716 | //purpose : |
717 | //======================================================================= |
718 | |
719 | Standard_Real BRepBlend_RstRstEvolRad::GetSectionSize() const |
720 | { |
721 | return maxang * Abs(ray); |
722 | } |
723 | |
724 | //======================================================================= |
725 | //function : GetMinimalWeight |
726 | //purpose : |
727 | //======================================================================= |
728 | |
729 | void BRepBlend_RstRstEvolRad::GetMinimalWeight(TColStd_Array1OfReal& Weights) const |
730 | { |
731 | BlendFunc::GetMinimalWeights(mySShape, myTConv, minang, maxang, Weights ); |
81bba717 |
732 | // It is supposed that it does not depend on the Radius! |
7fd59977 |
733 | } |
734 | |
735 | //======================================================================= |
736 | //function : NbIntervals |
737 | //purpose : |
738 | //======================================================================= |
739 | |
740 | Standard_Integer BRepBlend_RstRstEvolRad::NbIntervals (const GeomAbs_Shape S) const |
741 | { |
742 | Standard_Integer Nb_Int_Courbe, Nb_Int_Loi; |
743 | Nb_Int_Courbe = guide->NbIntervals(BlendFunc::NextShape(S)); |
744 | Nb_Int_Loi = fevol->NbIntervals(S); |
745 | |
746 | if (Nb_Int_Loi==1) { |
747 | return Nb_Int_Courbe; |
748 | } |
749 | |
750 | TColStd_Array1OfReal IntC(1, Nb_Int_Courbe+1); |
751 | TColStd_Array1OfReal IntL(1, Nb_Int_Loi+1); |
752 | TColStd_SequenceOfReal Inter; |
753 | guide->Intervals(IntC, BlendFunc::NextShape(S)); |
754 | fevol->Intervals(IntL, S); |
755 | |
756 | FusionneIntervalles( IntC, IntL, Inter); |
757 | return Inter.Length()-1; |
758 | } |
759 | |
760 | //======================================================================= |
761 | //function : Intervals |
762 | //purpose : |
763 | //======================================================================= |
764 | |
765 | void BRepBlend_RstRstEvolRad::Intervals (TColStd_Array1OfReal& T, |
766 | const GeomAbs_Shape S) const |
767 | { |
768 | Standard_Integer Nb_Int_Courbe, Nb_Int_Loi; |
769 | Nb_Int_Courbe = guide->NbIntervals(BlendFunc::NextShape(S)); |
770 | Nb_Int_Loi = fevol->NbIntervals(S); |
771 | |
772 | if (Nb_Int_Loi==1) { |
773 | guide->Intervals(T, BlendFunc::NextShape(S)); |
774 | } |
775 | else { |
776 | TColStd_Array1OfReal IntC(1, Nb_Int_Courbe+1); |
777 | TColStd_Array1OfReal IntL(1, Nb_Int_Loi+1); |
778 | TColStd_SequenceOfReal Inter; |
779 | guide->Intervals(IntC, BlendFunc::NextShape(S)); |
780 | fevol->Intervals(IntL, S); |
781 | |
782 | FusionneIntervalles( IntC, IntL, Inter); |
783 | for (Standard_Integer ii=1; ii<=Inter.Length(); ii++) { |
784 | T(ii) = Inter(ii); |
785 | } |
786 | } |
787 | } |
788 | |
789 | //======================================================================= |
790 | //function : GetShape |
791 | //purpose : |
792 | //======================================================================= |
793 | |
794 | void BRepBlend_RstRstEvolRad::GetShape (Standard_Integer& NbPoles, |
795 | Standard_Integer& NbKnots, |
796 | Standard_Integer& Degree, |
797 | Standard_Integer& NbPoles2d) |
798 | { |
799 | NbPoles2d = 2; |
800 | BlendFunc::GetShape(mySShape, maxang, NbPoles, NbKnots, Degree, myTConv); |
801 | } |
802 | |
803 | //======================================================================= |
804 | //function : GetTolerance |
81bba717 |
805 | //purpose : Determine the Tolerance to be used in approximations. |
7fd59977 |
806 | //======================================================================= |
807 | |
808 | void BRepBlend_RstRstEvolRad::GetTolerance(const Standard_Real BoundTol, |
809 | const Standard_Real SurfTol, |
810 | const Standard_Real AngleTol, |
811 | math_Vector& Tol3d, |
812 | math_Vector& Tol1d) const |
813 | { |
814 | Standard_Integer low = Tol3d.Lower(), up = Tol3d.Upper(); |
815 | Standard_Real Tol; |
816 | Tol= GeomFill::GetTolerance(myTConv, minang, Abs(ray), |
817 | AngleTol, SurfTol); |
818 | Tol1d.Init(SurfTol); |
819 | Tol3d.Init(SurfTol); |
820 | Tol3d(low+1) = Tol3d(up-1) = Min(Tol, SurfTol); |
821 | Tol3d(low) = Tol3d(up) = Min(Tol, BoundTol); |
822 | } |
823 | |
824 | //======================================================================= |
825 | //function : Knots |
826 | //purpose : |
827 | //======================================================================= |
828 | |
829 | void BRepBlend_RstRstEvolRad::Knots(TColStd_Array1OfReal& TKnots) |
830 | { |
831 | GeomFill::Knots(myTConv, TKnots); |
832 | } |
833 | |
834 | //======================================================================= |
835 | //function : Mults |
836 | //purpose : |
837 | //======================================================================= |
838 | |
839 | void BRepBlend_RstRstEvolRad::Mults(TColStd_Array1OfInteger& TMults) |
840 | { |
841 | GeomFill::Mults(myTConv, TMults); |
842 | } |
843 | |
844 | //======================================================================= |
845 | //function : Section |
846 | //purpose : |
847 | //======================================================================= |
848 | |
849 | void BRepBlend_RstRstEvolRad::Section(const Blend_Point& P, |
850 | TColgp_Array1OfPnt& Poles, |
851 | TColgp_Array1OfPnt2d& Poles2d, |
852 | TColStd_Array1OfReal& Weights) |
853 | { |
854 | gp_Vec n1, n2, NotUsed; |
855 | gp_Pnt Center; |
856 | Standard_Real u, v; |
857 | |
858 | Standard_Real prm = P.Parameter(); |
859 | Standard_Integer low = Poles.Lower(); |
860 | Standard_Integer upp = Poles.Upper(); |
861 | |
862 | tguide->D1(prm,ptgui, d1gui); |
863 | ray = tevol->Value(prm); |
864 | nplan = d1gui.Normalized(); |
865 | |
866 | u = P.ParameterOnC1(); |
867 | v = P.ParameterOnC2(); |
868 | |
869 | gp_Pnt2d pt2d1 = rst1->Value(u); |
870 | gp_Pnt2d pt2d2 = rst2->Value(v); |
871 | |
872 | ptrst1 = cons1.Value(u); |
873 | ptrst2 = cons2.Value(v); |
874 | distmin = Min (distmin, ptrst1.Distance(ptrst2)); |
875 | |
876 | Poles2d(Poles2d.Lower()).SetCoord(pt2d1.X(),pt2d1.Y()); |
877 | Poles2d(Poles2d.Upper()).SetCoord(pt2d2.X(),pt2d2.Y()); |
878 | |
81bba717 |
879 | // Linear Case |
7fd59977 |
880 | if (mySShape == BlendFunc_Linear) { |
881 | Poles(low) = ptrst1; |
882 | Poles(upp) = ptrst2; |
883 | Weights(low) = 1.0; |
884 | Weights(upp) = 1.0; |
885 | return; |
886 | } |
887 | |
81bba717 |
888 | // Calculate the center of the circle |
7fd59977 |
889 | Standard_Boolean IsCenter; |
890 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, NotUsed); |
891 | |
81bba717 |
892 | // normals to the section with points |
7fd59977 |
893 | n1 = gp_Vec(Center, ptrst1).Normalized(); |
894 | n2 = gp_Vec(Center, ptrst2).Normalized(); |
895 | |
896 | if (choix%2 != 0) { |
897 | nplan.Reverse(); |
898 | } |
899 | |
900 | GeomFill::GetCircle(myTConv, |
901 | n1, n2, |
902 | nplan, ptrst1, ptrst2, |
903 | Abs(ray), Center, |
904 | Poles, Weights); |
905 | } |
906 | |
907 | //======================================================================= |
908 | //function : Section |
909 | //purpose : |
910 | //======================================================================= |
911 | |
912 | Standard_Boolean BRepBlend_RstRstEvolRad::Section(const Blend_Point& P, |
913 | TColgp_Array1OfPnt& Poles, |
914 | TColgp_Array1OfVec& DPoles, |
915 | TColgp_Array1OfPnt2d& Poles2d, |
916 | TColgp_Array1OfVec2d& DPoles2d, |
917 | TColStd_Array1OfReal& Weights, |
918 | TColStd_Array1OfReal& DWeights) |
919 | { |
920 | |
921 | gp_Vec d11, d21; |
922 | gp_Vec dnplan, d1n1, d1n2;//,np2, dnp2; |
923 | gp_Vec temp, tgct; |
924 | gp_Vec d1urst, d1vrst; |
925 | gp_Pnt Center, NotUsed; |
926 | |
927 | Standard_Real norm2, normmed, Dist; |
928 | |
929 | math_Vector sol(1, 2), valsol(1, 2), secmember(1, 2); |
930 | math_Matrix gradsol(1, 2, 1, 2); |
931 | |
932 | Standard_Real prm = P.Parameter(); |
7fd59977 |
933 | Standard_Integer low = Poles.Lower(); |
934 | Standard_Integer upp = Poles.Upper(); |
935 | Standard_Boolean istgt; |
936 | |
937 | tguide->D2(prm, ptgui, d1gui, d2gui); |
938 | tevol->D1(prm,ray,dray); |
939 | normtg = d1gui.Magnitude(); |
940 | nplan = d1gui.Normalized(); |
941 | dnplan.SetLinearForm(1./normtg, d2gui, |
942 | -1./normtg * (nplan.Dot(d2gui)), nplan); |
943 | |
944 | sol(1) = prmrst1 = P.ParameterOnC1(); |
945 | sol(2) = prmrst2 = P.ParameterOnC2(); |
946 | pt2drst1 = rst1->Value(prmrst1); |
947 | pt2drst2 = rst2->Value(prmrst2); |
948 | |
949 | Values(sol, valsol, gradsol); |
950 | |
951 | cons1.D1(sol(1), ptrst1, d11); |
952 | cons2.D1(sol(2), ptrst2, d21); |
953 | |
954 | temp.SetXYZ(ptrst1.XYZ() - ptgui.XYZ()); |
955 | secmember(1) = normtg - dnplan.Dot(temp); |
956 | |
957 | temp.SetXYZ(ptrst2.XYZ() - ptgui.XYZ()); |
958 | secmember(2) = normtg - dnplan.Dot(temp); |
959 | |
960 | math_Gauss Resol(gradsol, 1.e-9); |
961 | |
962 | if (Resol.IsDone()) { |
963 | istgt = Standard_False; |
964 | Resol.Solve(secmember); |
965 | } |
966 | else { |
967 | math_SVD SingRS (gradsol); |
968 | if (SingRS.IsDone()) { |
969 | math_Vector DEDT(1,2); |
970 | DEDT = secmember; |
971 | SingRS.Solve(DEDT, secmember, 1.e-6); |
972 | istgt = Standard_False; |
973 | } |
974 | else istgt = Standard_True; |
975 | } |
976 | |
977 | gp_Vec med; |
978 | gp_Vec rst1rst2(ptrst1, ptrst2); |
979 | Standard_Boolean IsCenter; |
980 | |
981 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, med); |
982 | if (!IsCenter) return Standard_False; |
983 | |
984 | normmed = med.Magnitude(); |
985 | med.Normalize(); |
986 | gp_Vec n1(Center, ptrst1), n2(Center, ptrst2); |
987 | |
988 | if (!istgt) { |
81bba717 |
989 | // secmember contains derivatives of parameters on curves |
990 | // corresponding to t |
7fd59977 |
991 | tgrst1 = secmember(1) * d11; |
992 | tgrst2 = secmember(2) * d21; |
993 | |
994 | gp_Vec d1rst1rst2; |
995 | |
996 | norm2 = rst1rst2.SquareMagnitude(); |
997 | d1rst1rst2 = tgrst2 - tgrst1; |
998 | Dist = ray * ray - 0.25 * norm2; |
999 | Standard_Real Invdray = dray / ray; |
1000 | |
1001 | if (Dist > 1.E-07) { |
1002 | gp_Vec d1P1P2CrosNp, dmed; |
1003 | d1P1P2CrosNp = d1rst1rst2.Crossed(nplan) + rst1rst2.Crossed(dnplan); |
81bba717 |
1004 | // derivative of the bisector |
7fd59977 |
1005 | dmed = d1P1P2CrosNp - med.Dot(d1P1P2CrosNp) * med; |
1006 | dmed /= normmed; |
1007 | Dist = sqrt(Dist); |
1008 | Standard_Real d1Dist; |
1009 | |
1010 | d1Dist = (ray * dray - 0.25 * rst1rst2.Dot(d1rst1rst2) ) / Dist; |
1011 | |
1012 | if (choix > 2) { |
1013 | dmed.Reverse(); |
1014 | } |
1015 | |
81bba717 |
1016 | // derivative of the coefficient Dist is located in dmed |
7fd59977 |
1017 | dmed.SetLinearForm(Dist, dmed, d1Dist, med); |
1018 | d1rst1rst2 *= 0.5; |
81bba717 |
1019 | // derivative of the Normal to the curve in P1 |
7fd59977 |
1020 | d1n1 = - (d1rst1rst2 + dmed + Invdray * n1) / ray; |
1021 | |
81bba717 |
1022 | // derivative of the Normal to the curve in P2 |
7fd59977 |
1023 | d1n2 = (d1rst1rst2 - dmed - Invdray * n2) / ray; |
1024 | } |
1025 | else { |
1026 | d1rst1rst2 *= 0.5; |
0d969553 |
1027 | // Normal to the curve in P1 |
7fd59977 |
1028 | d1n1 = - (d1rst1rst2 + Invdray * n1) / ray; |
1029 | |
0d969553 |
1030 | // Normal to the curve in P2 |
7fd59977 |
1031 | d1n2 = (d1rst1rst2 - Invdray * n2) / ray; |
1032 | } |
1033 | } |
1034 | |
1035 | n1.Normalize(); |
1036 | n2.Normalize(); |
1037 | |
81bba717 |
1038 | // Tops 2D |
7fd59977 |
1039 | |
1040 | Poles2d(Poles2d.Lower()).SetCoord(pt2drst1.X(), pt2drst1.Y()); |
1041 | Poles2d(Poles2d.Upper()).SetCoord(pt2drst2.X(), pt2drst2.Y()); |
1042 | if (!istgt) { |
1043 | Standard_Real a, b; |
1044 | surf1->D1(pt2drst1.X(), pt2drst1.Y(), NotUsed, d1urst, d1vrst); |
1045 | t3dto2d(a,b,tgrst1, d1urst, d1vrst); |
1046 | DPoles2d(Poles2d.Lower()).SetCoord(a, b); |
1047 | |
1048 | surf2->D1(pt2drst2.X(), pt2drst2.Y(), NotUsed, d1urst, d1vrst); |
1049 | t3dto2d(a, b, tgrst2, d1urst, d1vrst); |
1050 | DPoles2d(Poles2d.Upper()).SetCoord(a, b); |
1051 | } |
1052 | |
81bba717 |
1053 | // Linear Case |
7fd59977 |
1054 | if (mySShape == BlendFunc_Linear) { |
1055 | Poles(low) = ptrst1; |
1056 | Poles(upp) = ptrst2; |
1057 | Weights(low) = 1.0; |
1058 | Weights(upp) = 1.0; |
1059 | if (!istgt) { |
1060 | DPoles(low) = tgrst1; |
1061 | DPoles(upp) = tgrst2; |
1062 | DWeights(low) = 0.0; |
1063 | DWeights(upp) = 0.0; |
1064 | } |
1065 | return (!istgt); |
1066 | } |
1067 | |
81bba717 |
1068 | // Case of the circle |
1069 | // tangent to the center of the circle |
7fd59977 |
1070 | if (!istgt) { |
1071 | tgct.SetLinearForm(-ray, d1n1, -dray, n1, tgrst1); |
1072 | } |
1073 | |
1074 | |
1075 | if (choix%2 != 0) { |
1076 | nplan.Reverse(); |
1077 | dnplan.Reverse(); |
1078 | } |
1079 | |
1080 | if (!istgt) { |
1081 | return GeomFill::GetCircle(myTConv, |
1082 | n1, n2, |
1083 | d1n1, d1n2, |
1084 | nplan, dnplan, |
1085 | ptrst1, ptrst2, |
1086 | tgrst1, tgrst2, |
1087 | Abs(ray), dray, |
1088 | Center, tgct, |
1089 | Poles, |
1090 | DPoles, |
1091 | Weights, |
1092 | DWeights); |
1093 | } |
1094 | else { |
1095 | GeomFill::GetCircle(myTConv, |
1096 | n1, n2, |
1097 | nplan, ptrst1, ptrst2, |
1098 | Abs(ray), Center, |
1099 | Poles, Weights); |
1100 | return Standard_False; |
1101 | } |
1102 | } |
1103 | |
1104 | //======================================================================= |
1105 | //function : Section |
1106 | //purpose : |
1107 | //======================================================================= |
1108 | |
1109 | Standard_Boolean BRepBlend_RstRstEvolRad::Section |
1110 | (const Blend_Point&, |
1111 | TColgp_Array1OfPnt&, |
1112 | TColgp_Array1OfVec&, |
1113 | TColgp_Array1OfVec&, |
1114 | TColgp_Array1OfPnt2d&, |
1115 | TColgp_Array1OfVec2d&, |
1116 | TColgp_Array1OfVec2d&, |
1117 | TColStd_Array1OfReal&, |
1118 | TColStd_Array1OfReal&, |
1119 | TColStd_Array1OfReal&) |
1120 | { |
1121 | return Standard_False; |
1122 | } |
1123 | |
1124 | |
1125 | void BRepBlend_RstRstEvolRad::Resolution(const Standard_Integer IC2d, |
1126 | const Standard_Real Tol, |
1127 | Standard_Real& TolU, |
1128 | Standard_Real& TolV) const |
1129 | { |
1130 | if(IC2d == 1){ |
1131 | TolU = surf1->UResolution(Tol); |
1132 | TolV = surf1->VResolution(Tol); |
1133 | } |
1134 | else { |
1135 | TolU = surf2->UResolution(Tol); |
1136 | TolV = surf2->VResolution(Tol); |
1137 | } |
1138 | } |
1139 | |