b311480e |
1 | // Created on: 1993-12-20 |
2 | // Created by: Jacques GOUSSARD |
3 | // Copyright (c) 1993-1999 Matra Datavision |
973c2be1 |
4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
5 | // |
973c2be1 |
6 | // This file is part of Open CASCADE Technology software library. |
b311480e |
7 | // |
d5f74e42 |
8 | // This library is free software; you can redistribute it and/or modify it under |
9 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
10 | // by the Free Software Foundation, with special exception defined in the file |
11 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
12 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
13 | // |
973c2be1 |
14 | // Alternatively, this file may be used under the terms of Open CASCADE |
15 | // commercial license or contractual agreement. |
7fd59977 |
16 | |
7fd59977 |
17 | |
42cf5bc1 |
18 | #include <Adaptor3d_HCurve.hxx> |
19 | #include <Adaptor3d_HSurface.hxx> |
20 | #include <Blend_Point.hxx> |
7fd59977 |
21 | #include <BlendFunc.hxx> |
42cf5bc1 |
22 | #include <BlendFunc_EvolRad.hxx> |
23 | #include <CSLib.hxx> |
24 | #include <CSLib_NormalStatus.hxx> |
25 | #include <ElCLib.hxx> |
7fd59977 |
26 | #include <GeomFill.hxx> |
42cf5bc1 |
27 | #include <gp.hxx> |
28 | #include <gp_Circ.hxx> |
29 | #include <gp_Pnt.hxx> |
30 | #include <gp_Vec.hxx> |
31 | #include <gp_Vec2d.hxx> |
32 | #include <Law_Function.hxx> |
33 | #include <math_Gauss.hxx> |
34 | #include <math_Matrix.hxx> |
35 | #include <math_SVD.hxx> |
36 | #include <Precision.hxx> |
7fd59977 |
37 | #include <Standard_Boolean.hxx> |
38 | #include <Standard_DomainError.hxx> |
39 | #include <Standard_NotImplemented.hxx> |
7fd59977 |
40 | #include <TColgp_Array2OfVec.hxx> |
42cf5bc1 |
41 | #include <TColStd_SequenceOfReal.hxx> |
7fd59977 |
42 | |
43 | #define Eps 1.e-15 |
44 | |
45 | static void FusionneIntervalles(const TColStd_Array1OfReal& I1, |
46 | const TColStd_Array1OfReal& I2, |
47 | TColStd_SequenceOfReal& Seq) |
48 | { |
49 | Standard_Integer ind1=1, ind2=1; |
50 | Standard_Real Epspar = Precision::PConfusion()*0.99; |
81bba717 |
51 | // supposed that positioning works with PConfusion()/2 |
7fd59977 |
52 | Standard_Real v1, v2; |
81bba717 |
53 | // Initialisation : IND1 and IND2 point at the 1st element |
54 | // of each of 2 tables to be processed. INDS points at the last |
55 | // element of TABSOR |
7fd59977 |
56 | |
57 | |
81bba717 |
58 | //--- TABSOR is filled by parsing TABLE1 and TABLE2 simultaneously --- |
59 | //------------------ by removing multiple occurrencies ------------ |
7fd59977 |
60 | |
61 | while ((ind1<=I1.Upper()) && (ind2<=I2.Upper())) { |
62 | v1 = I1(ind1); |
63 | v2 = I2(ind2); |
64 | if (Abs(v1-v2)<= Epspar) { |
81bba717 |
65 | // Here elements of I1 and I2 are suitable. |
7fd59977 |
66 | Seq.Append((v1+v2)/2); |
67 | ind1++; |
68 | ind2++; |
69 | } |
70 | else if (v1 < v2) { |
81bba717 |
71 | // Here the element of I1 is suitable. |
7fd59977 |
72 | Seq.Append(v1); |
73 | ind1++; |
74 | } |
75 | else { |
81bba717 |
76 | // Here the element of TABLE2 is suitable. |
7fd59977 |
77 | Seq.Append(v2); |
78 | ind2++; |
79 | } |
80 | } |
81 | |
82 | if (ind1>I1.Upper()) { |
81bba717 |
83 | //----- Here I1 is empty, to be completed with the end of TABLE2 ------- |
7fd59977 |
84 | |
85 | for (; ind2<=I2.Upper(); ind2++) { |
86 | Seq.Append(I2(ind2)); |
87 | } |
88 | } |
89 | |
90 | if (ind2>I2.Upper()) { |
81bba717 |
91 | //----- Here I2 is empty, to be completed with the end of I1 ------- |
7fd59977 |
92 | |
93 | for (; ind1<=I1.Upper(); ind1++) { |
94 | Seq.Append(I1(ind1)); |
95 | } |
96 | } |
97 | } |
98 | |
99 | |
100 | //======================================================================= |
101 | //function : BlendFunc_EvolRad |
102 | //purpose : |
103 | //======================================================================= |
104 | |
105 | BlendFunc_EvolRad::BlendFunc_EvolRad(const Handle(Adaptor3d_HSurface)& S1, |
106 | const Handle(Adaptor3d_HSurface)& S2, |
107 | const Handle(Adaptor3d_HCurve)& C, |
108 | const Handle(Law_Function)& Law) |
109 | : |
110 | surf1(S1),surf2(S2), |
111 | curv(C), tcurv(C), |
112 | istangent(Standard_True), |
113 | xval(1,4), |
114 | E(1,4), DEDX(1,4,1,4), DEDT(1,4), |
115 | D2EDX2(4,4,4), |
116 | D2EDXDT(1,4,1,4), D2EDT2(1,4), |
117 | minang(RealLast()), maxang(RealFirst()), |
118 | lengthmin(RealLast()), |
119 | lengthmax(RealFirst()), |
120 | distmin(RealLast()), |
121 | mySShape(BlendFunc_Rational) |
122 | { |
123 | fevol = Law; |
124 | tevol = Law; |
125 | |
81bba717 |
126 | // Initialisaton of cash control variables. |
7fd59977 |
127 | tval = -9.876e100; |
128 | xval.Init(-9.876e100); |
129 | myXOrder = -1; |
130 | myTOrder = -1; |
131 | } |
132 | |
133 | //======================================================================= |
134 | //function : NbEquations |
135 | //purpose : |
136 | //======================================================================= |
137 | |
138 | Standard_Integer BlendFunc_EvolRad::NbEquations () const |
139 | { |
140 | return 4; |
141 | } |
142 | |
143 | |
144 | //======================================================================= |
145 | //function : Set |
146 | //purpose : |
147 | //======================================================================= |
148 | |
149 | void BlendFunc_EvolRad::Set(const Standard_Integer Choix) |
150 | { |
151 | choix = Choix; |
152 | switch (choix) { |
153 | case 1: |
154 | case 2: |
155 | { |
156 | sg1 = -1.; |
157 | sg2 = -1.; |
158 | } |
159 | break; |
160 | case 3: |
161 | case 4: |
162 | { |
163 | sg1 = 1.; |
164 | sg2 = -1.; |
165 | } |
166 | break; |
167 | case 5: |
168 | case 6: |
169 | { |
170 | sg1 = 1.; |
171 | sg2 = 1.; |
172 | } |
173 | break; |
174 | case 7: |
175 | case 8: |
176 | { |
177 | sg1 = -1.; |
178 | sg2 = 1.; |
179 | } |
180 | break; |
181 | default: |
182 | sg1 = sg2 = -1.; |
183 | } |
184 | } |
185 | |
186 | //======================================================================= |
187 | //function : Set |
188 | //purpose : |
189 | //======================================================================= |
190 | |
191 | void BlendFunc_EvolRad::Set(const BlendFunc_SectionShape TypeSection) |
192 | { |
193 | mySShape = TypeSection; |
194 | } |
195 | |
196 | |
197 | //======================================================================= |
198 | //function : ComputeValues |
81bba717 |
199 | //purpose : OBLIGATORY passage for all computations |
200 | // This method manages the positioning on Surfaces and Curves |
201 | // Partial calculation of equations and their derivatives |
202 | // Storage of some intermediary results in fields to be |
203 | // used in other methods. |
7fd59977 |
204 | //======================================================================= |
205 | |
206 | Standard_Boolean BlendFunc_EvolRad::ComputeValues(const math_Vector& X, |
207 | const Standard_Integer Order, |
208 | const Standard_Boolean byParam, |
209 | const Standard_Real Param) |
210 | { |
81bba717 |
211 | // static declaration to avoid systematic realloc |
7fd59977 |
212 | |
213 | static gp_Vec d3u1,d3v1,d3uuv1,d3uvv1,d3u2,d3v2,d3uuv2,d3uvv2; |
214 | static gp_Vec d1gui, d2gui, d3gui; |
215 | static gp_Pnt ptgui; |
216 | static Standard_Real invnormtg, dinvnormtg; |
217 | Standard_Real T = Param, aux; |
218 | |
81bba717 |
219 | // Case of implicit parameter |
7fd59977 |
220 | if ( !byParam) { T = param;} |
221 | |
81bba717 |
222 | // The work is done already? |
7fd59977 |
223 | Standard_Boolean lX_OK = (Order<=myXOrder); |
224 | Standard_Integer ii; |
225 | for (ii=1; ((ii<=X.Length()) && lX_OK); ii++) { |
226 | lX_OK = ( X(ii) == xval(ii) ); |
227 | } |
228 | |
229 | Standard_Boolean t_OK =( (T == tval) |
230 | && ((Order<=myTOrder)||(!byParam)) ); |
231 | |
232 | if (lX_OK && (t_OK) ) { |
233 | return Standard_True; |
234 | } |
235 | |
81bba717 |
236 | // Processing of t |
7fd59977 |
237 | if (!t_OK) { |
238 | tval = T; |
239 | if (byParam) { myTOrder = Order;} |
240 | else { myTOrder = 0;} |
81bba717 |
241 | //----- Positioning on the curve and the law---------------- |
7fd59977 |
242 | switch (myTOrder) { |
243 | case 0 : |
244 | { |
245 | tcurv->D1(T, ptgui, d1gui); |
246 | nplan = d1gui.Normalized(); |
247 | ray = tevol->Value(T); |
248 | } |
249 | break; |
250 | |
251 | case 1 : |
252 | { |
253 | tcurv->D2(T,ptgui,d1gui,d2gui); |
254 | nplan = d1gui.Normalized(); |
255 | invnormtg = ((Standard_Real) 1 ) / d1gui.Magnitude(); |
256 | dnplan.SetLinearForm(invnormtg, d2gui, |
257 | -invnormtg*(nplan.Dot(d2gui)), nplan); |
258 | |
259 | tevol->D1(T, ray, dray); |
260 | } |
261 | break; |
262 | case 2 : |
263 | { |
264 | tcurv->D3(T,ptgui,d1gui,d2gui,d3gui); |
265 | nplan = d1gui.Normalized(); |
266 | invnormtg = ((Standard_Real) 1 ) / d1gui.Magnitude(); |
267 | dnplan.SetLinearForm(invnormtg, d2gui, |
268 | -invnormtg*(nplan.Dot(d2gui)), nplan); |
269 | dinvnormtg = - nplan.Dot(d2gui)*invnormtg*invnormtg; |
270 | d2nplan.SetLinearForm(invnormtg, d3gui, |
271 | dinvnormtg, d2gui); |
272 | aux = dinvnormtg*(nplan.Dot(d2gui)) + invnormtg*( dnplan.Dot(d2gui) |
273 | + nplan.Dot(d3gui) ); |
274 | d2nplan.SetLinearForm(-invnormtg*(nplan.Dot(d2gui)), dnplan, |
275 | -aux, nplan, d2nplan ); |
276 | |
277 | tevol->D2(T, ray, dray, d2ray); |
278 | break; |
279 | } |
280 | default: |
281 | return Standard_False; |
282 | } |
283 | } |
284 | |
81bba717 |
285 | // Processing of X |
7fd59977 |
286 | if (!lX_OK) { |
287 | xval = X; |
288 | myXOrder = Order; |
81bba717 |
289 | //-------------- Positioning on surfaces ----------------- |
7fd59977 |
290 | switch (myXOrder) { |
291 | case 0 : |
292 | { |
293 | surf1->D1(X(1),X(2),pts1,d1u1,d1v1); |
294 | nsurf1 = d1u1.Crossed(d1v1); |
295 | surf2->D1(X(3),X(4),pts2,d1u2,d1v2); |
296 | nsurf2 = d1u2.Crossed(d1v2); |
297 | break; |
298 | } |
299 | case 1 : |
300 | { |
301 | surf1->D2(X(1),X(2),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1); |
302 | nsurf1 = d1u1.Crossed(d1v1); |
303 | dns1u1 = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1)); |
304 | dns1v1 = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1)); |
305 | |
306 | surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2); |
307 | nsurf2 = d1u2.Crossed(d1v2); |
308 | dns1u2 = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2)); |
309 | dns1v2 = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2)); |
310 | |
311 | break; |
312 | } |
313 | case 2 : |
314 | { |
315 | surf1->D3(X(1),X(2),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1,d3u1,d3v1,d3uuv1,d3uvv1); |
316 | nsurf1 = d1u1.Crossed(d1v1); |
317 | surf2->D3(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2,d3u2,d3v2,d3uuv2,d3uvv2); |
318 | nsurf2 = d1u2.Crossed(d1v2); |
319 | break; |
320 | } |
321 | default: |
322 | return Standard_False; |
323 | } |
81bba717 |
324 | // Case of degenerated surfaces |
7fd59977 |
325 | if (nsurf1.Magnitude() < Eps ) { |
326 | // gp_Vec normal; |
327 | gp_Pnt2d P(X(1), X(2)); |
328 | if (Order == 0) BlendFunc::ComputeNormal(surf1, P, nsurf1); |
329 | else BlendFunc::ComputeDNormal(surf1, P, nsurf1, dns1u1, dns1v1); |
330 | } |
331 | if (nsurf2.Magnitude() < Eps) { |
332 | // gp_Vec normal; |
333 | gp_Pnt2d P(X(3), X(4)); |
334 | if (Order==0) BlendFunc::ComputeNormal(surf2, P, nsurf2); |
335 | else BlendFunc::ComputeDNormal(surf2, P, nsurf2, dns1u2, dns1v2); |
336 | } |
337 | } |
338 | |
81bba717 |
339 | // -------------------- Positioning of order 0 --------------------- |
7fd59977 |
340 | Standard_Real invnorm1, invnorm2, ndotns1, ndotns2, theD; |
341 | Standard_Real ray1 = sg1*ray; |
342 | Standard_Real ray2 = sg2*ray; |
343 | gp_Vec ncrossns1,ncrossns2,resul,temp, n1, n2; |
344 | |
345 | theD = - (nplan.XYZ().Dot(ptgui.XYZ())); |
346 | |
347 | E(1) = (nplan.X() * (pts1.X() + pts2.X()) + |
348 | nplan.Y() * (pts1.Y() + pts2.Y()) + |
349 | nplan.Z() * (pts1.Z() + pts2.Z())) /2 + theD; |
350 | |
351 | ncrossns1 = nplan.Crossed(nsurf1); |
352 | ncrossns2 = nplan.Crossed(nsurf2); |
353 | invnorm1 = ncrossns1.Magnitude(); |
354 | invnorm2 = ncrossns2.Magnitude(); |
355 | |
356 | if (invnorm1 > Eps) invnorm1 = ((Standard_Real) 1) /invnorm1; |
357 | else { |
81bba717 |
358 | invnorm1 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
359 | #ifdef OCCT_DEBUG |
04232180 |
360 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
361 | #endif |
362 | } |
363 | if (invnorm2 > Eps) invnorm2 = ((Standard_Real) 1) /invnorm2; |
364 | else { |
81bba717 |
365 | invnorm2 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
366 | #ifdef OCCT_DEBUG |
04232180 |
367 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
368 | #endif |
369 | } |
370 | |
371 | ndotns1 = nplan.Dot(nsurf1); |
372 | ndotns2 = nplan.Dot(nsurf2); |
373 | |
374 | n1.SetLinearForm(ndotns1,nplan,-1.,nsurf1); |
375 | n1.Multiply(invnorm1); |
376 | n2.SetLinearForm(ndotns2,nplan,-1.,nsurf2); |
377 | n2.Multiply(invnorm2); |
378 | |
379 | resul.SetLinearForm(ray1, n1, |
380 | -ray2, n2, |
381 | gp_Vec(pts2,pts1)); |
382 | |
383 | E(2) = resul.X(); |
384 | E(3) = resul.Y(); |
385 | E(4) = resul.Z(); |
386 | |
81bba717 |
387 | // -------------------- Positioning of order 1 --------------------- |
7fd59977 |
388 | if (Order >= 1) { |
389 | Standard_Real grosterme, cube, carre; |
390 | |
391 | DEDX(1,1) = nplan.Dot(d1u1)/2; |
392 | DEDX(1,2) = nplan.Dot(d1v1)/2; |
393 | DEDX(1,3) = nplan.Dot(d1u2)/2; |
394 | DEDX(1,4) = nplan.Dot(d1v2)/2; |
395 | |
396 | cube =invnorm1*invnorm1*invnorm1; |
81bba717 |
397 | // Derived compared to u1 |
7fd59977 |
398 | grosterme = - ncrossns1.Dot(nplan.Crossed(dns1u1))*cube; |
399 | dndu1.SetLinearForm( grosterme*ndotns1 |
400 | + invnorm1*nplan.Dot(dns1u1), nplan, |
401 | - grosterme, nsurf1, |
402 | - invnorm1, dns1u1 ); |
403 | |
404 | resul.SetLinearForm(ray1, dndu1, d1u1); |
405 | DEDX(2,1) = resul.X(); |
406 | DEDX(3,1) = resul.Y(); |
407 | DEDX(4,1) = resul.Z(); |
408 | |
81bba717 |
409 | // Derived compared to v1 |
7fd59977 |
410 | grosterme = - ncrossns1.Dot(nplan.Crossed(dns1v1))*cube; |
411 | dndv1.SetLinearForm( grosterme*ndotns1 |
412 | +invnorm1*nplan.Dot(dns1v1), nplan, |
413 | -grosterme, nsurf1, |
414 | -invnorm1, dns1v1); |
415 | |
416 | resul.SetLinearForm(ray1, dndv1, d1v1); |
417 | DEDX(2,2) = resul.X(); |
418 | DEDX(3,2) = resul.Y(); |
419 | DEDX(4,2) = resul.Z(); |
420 | |
421 | cube = invnorm2*invnorm2*invnorm2; |
422 | // Derivee par rapport a u2 |
423 | grosterme = - ncrossns2.Dot(nplan.Crossed(dns1u2))*cube; |
424 | dndu2.SetLinearForm( grosterme*ndotns2 |
425 | +invnorm2*nplan.Dot(dns1u2), nplan, |
426 | -grosterme, nsurf2, |
427 | -invnorm2, dns1u2); |
428 | |
429 | resul.SetLinearForm(-ray2, dndu2, -1, d1u2); |
430 | DEDX(2,3) = resul.X(); |
431 | DEDX(3,3) = resul.Y(); |
432 | DEDX(4,3) = resul.Z(); |
433 | |
81bba717 |
434 | // Derived compared to v2 |
7fd59977 |
435 | grosterme = -ncrossns2.Dot(nplan.Crossed(dns1v2))*cube; |
436 | dndv2.SetLinearForm( grosterme*ndotns2 |
437 | +invnorm2*nplan.Dot(dns1v2), nplan, |
438 | -grosterme, nsurf2, |
439 | -invnorm2 , dns1v2); |
440 | |
441 | resul.SetLinearForm(-ray2,dndv2, -1, d1v2); |
442 | DEDX(2,4) = resul.X(); |
443 | DEDX(3,4) = resul.Y(); |
444 | DEDX(4,4) = resul.Z(); |
445 | |
446 | if (byParam) { |
447 | temp.SetXYZ( (pts1.XYZ()+pts2.XYZ())/2 - ptgui.XYZ()); |
81bba717 |
448 | // Derived from n1 compared to w |
7fd59977 |
449 | grosterme = ncrossns1.Dot(dnplan.Crossed(nsurf1))*invnorm1*invnorm1; |
450 | dn1w.SetLinearForm((dnplan.Dot(nsurf1)-grosterme*ndotns1)*invnorm1, nplan, |
451 | ndotns1*invnorm1,dnplan, |
452 | grosterme*invnorm1,nsurf1); |
453 | |
81bba717 |
454 | // Derived from n2 compared to w |
7fd59977 |
455 | grosterme = ncrossns2.Dot(dnplan.Crossed(nsurf2))*invnorm2*invnorm2; |
456 | dn2w.SetLinearForm((dnplan.Dot(nsurf2)-grosterme*ndotns2)*invnorm2,nplan, |
457 | ndotns2*invnorm2,dnplan, |
458 | grosterme*invnorm2,nsurf2); |
459 | |
460 | |
461 | DEDT(1) = dnplan.Dot(temp) - 1./invnormtg ; |
462 | temp.SetLinearForm(ray2, dn2w, sg2*dray, n2); |
463 | resul.SetLinearForm(ray1, dn1w, |
464 | sg1*dray, n1, |
465 | -1, temp); |
466 | DEDT(2) = resul.X(); |
467 | DEDT(3) = resul.Y(); |
468 | DEDT(4) = resul.Z(); |
469 | } |
81bba717 |
470 | // ------ Positioning of order 2 ----------------------------- |
7fd59977 |
471 | if (Order == 2) { |
472 | // gp_Vec d2ndu1, d2ndu2, d2ndv1, d2ndv2, d2nduv1, d2nduv2; |
473 | gp_Vec d2ns1u1, d2ns1u2, d2ns1v1, d2ns1v2, d2ns1uv1, d2ns1uv2; |
474 | Standard_Real uterm, vterm, smallterm, p1, p2, p12; |
475 | Standard_Real DPrim, DSecn; |
476 | D2EDX2.Init(0); |
477 | |
478 | D2EDX2(1, 1, 1) = nplan.Dot(d2u1)/2; |
479 | D2EDX2(1, 2, 1) = D2EDX2(1, 1, 2) = nplan.Dot(d2uv1)/2; |
480 | D2EDX2(1, 2, 2) = nplan.Dot(d2v1)/2; |
481 | |
482 | D2EDX2(1, 3, 3) = nplan.Dot(d2u2)/2; |
483 | D2EDX2(1, 4, 3) = D2EDX2(1, 3, 4) = nplan.Dot(d2uv2)/2; |
484 | D2EDX2(1, 4, 4) = nplan.Dot(d2v2)/2; |
485 | // ================ |
486 | // == Surface 1 == |
487 | // ================ |
488 | carre = invnorm1*invnorm1; |
489 | cube = carre*invnorm1; |
81bba717 |
490 | // Derived double compared to u1 |
491 | // Derived from the norm |
7fd59977 |
492 | d2ns1u1.SetLinearForm(1, d3u1.Crossed(d1v1), |
493 | 2, d2u1.Crossed(d2uv1), |
494 | 1, d1u1.Crossed(d3uuv1)); |
495 | DPrim = ncrossns1.Dot(nplan.Crossed(dns1u1)); |
496 | smallterm = - 2*DPrim*cube; |
497 | DSecn = ncrossns1.Dot(nplan.Crossed(d2ns1u1)) |
498 | + (nplan.Crossed(dns1u1)).SquareMagnitude(); |
499 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
500 | |
501 | temp.SetLinearForm( grosterme*ndotns1, nplan, |
502 | -grosterme , nsurf1); |
503 | p1 = nplan.Dot(dns1u1); |
504 | p2 = nplan.Dot(d2ns1u1); |
505 | d2ndu1.SetLinearForm( invnorm1*p2 |
506 | +smallterm*p1, nplan, |
507 | -smallterm, dns1u1, |
508 | -invnorm1, d2ns1u1); |
509 | d2ndu1 += temp; |
510 | resul.SetLinearForm(ray1, d2ndu1, d2u1); |
511 | D2EDX2(2,1,1) = resul.X(); |
512 | D2EDX2(3,1,1) = resul.Y(); |
513 | D2EDX2(4,1,1) = resul.Z(); |
514 | |
81bba717 |
515 | // Derived double compared to u1, v1 |
516 | // Derived from the norm |
7fd59977 |
517 | d2ns1uv1 = (d3uuv1.Crossed(d1v1)) |
518 | + (d2u1 .Crossed(d2v1)) |
519 | + (d1u1 .Crossed(d3uvv1)); |
520 | uterm = ncrossns1.Dot(nplan.Crossed(dns1u1)); |
521 | vterm = ncrossns1.Dot(nplan.Crossed(dns1v1)); |
522 | DSecn = (nplan.Crossed(dns1v1)).Dot(nplan.Crossed(dns1u1)) |
523 | + ncrossns1.Dot(nplan.Crossed(d2ns1uv1)); |
524 | grosterme = (3*uterm*vterm*carre-DSecn)*cube; |
81bba717 |
525 | uterm *= -cube; //and only now |
7fd59977 |
526 | vterm *= -cube; |
527 | |
528 | p1 = nplan.Dot(dns1u1); |
529 | p2 = nplan.Dot(dns1v1); |
530 | temp.SetLinearForm( grosterme*ndotns1, nplan, |
531 | - grosterme, nsurf1, |
532 | - invnorm1, d2ns1uv1); |
533 | d2nduv1.SetLinearForm( invnorm1*nplan.Dot(d2ns1uv1) |
534 | + uterm*p2 |
535 | + vterm*p1, nplan, |
536 | - uterm, dns1v1, |
537 | - vterm, dns1u1); |
538 | |
539 | d2nduv1 += temp; |
540 | resul.SetLinearForm(ray1, d2nduv1, d2uv1); |
541 | |
542 | D2EDX2(2,2,1) = D2EDX2(2,1,2) = resul.X(); |
543 | D2EDX2(3,2,1) = D2EDX2(3,1,2) = resul.Y(); |
544 | D2EDX2(4,2,1) = D2EDX2(4,1,2) = resul.Z(); |
545 | |
81bba717 |
546 | // Derived double compared to v1 |
547 | // Derived from the norm |
7fd59977 |
548 | d2ns1v1.SetLinearForm(1, d1u1.Crossed(d3v1), |
549 | 2, d2uv1.Crossed(d2v1), |
550 | 1, d3uvv1.Crossed(d1v1)); |
551 | DPrim = ncrossns1.Dot(nplan.Crossed(dns1v1)); |
552 | smallterm = - 2*DPrim*cube; |
553 | DSecn = ncrossns1.Dot(nplan.Crossed(d2ns1v1)) |
554 | + (nplan.Crossed(dns1v1)).SquareMagnitude(); |
555 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
556 | |
557 | p1 = nplan.Dot(dns1v1); |
558 | p2 = nplan.Dot(d2ns1v1); |
559 | temp.SetLinearForm( grosterme*ndotns1, nplan, |
560 | -grosterme , nsurf1); |
561 | d2ndv1.SetLinearForm( invnorm1*p2 |
562 | +smallterm*p1, nplan, |
563 | -smallterm, dns1v1, |
564 | -invnorm1, d2ns1v1); |
565 | d2ndv1 += temp; |
566 | resul.SetLinearForm(ray1, d2ndv1, d2v1); |
567 | |
568 | D2EDX2(2,2,2) = resul.X(); |
569 | D2EDX2(3,2,2) = resul.Y(); |
570 | D2EDX2(4,2,2) = resul.Z(); |
571 | // ================ |
572 | // == Surface 2 == |
573 | // ================ |
574 | carre = invnorm2*invnorm2; |
575 | cube = carre*invnorm2; |
81bba717 |
576 | // Derived double compared to u2 |
577 | // Derived from the norm |
7fd59977 |
578 | d2ns1u2.SetLinearForm(1, d3u2.Crossed(d1v2), |
579 | 2, d2u2.Crossed(d2uv2), |
580 | 1, d1u2.Crossed(d3uuv2)); |
581 | DPrim = ncrossns2.Dot(nplan.Crossed(dns1u2)); |
582 | smallterm = - 2*DPrim*cube; |
583 | DSecn = ncrossns2.Dot(nplan.Crossed(d2ns1u2)) |
584 | + (nplan.Crossed(dns1u2)).SquareMagnitude(); |
585 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
586 | |
587 | temp.SetLinearForm( grosterme*ndotns2, nplan, |
588 | -grosterme , nsurf2); |
589 | p1 = nplan.Dot(dns1u2); |
590 | p2 = nplan.Dot(d2ns1u2); |
591 | d2ndu2.SetLinearForm( invnorm2*p2 |
592 | +smallterm*p1, nplan, |
593 | -smallterm, dns1u2, |
594 | -invnorm2, d2ns1u2); |
595 | d2ndu2 += temp; |
596 | resul.SetLinearForm(-ray2, d2ndu2, -1, d2u2); |
597 | D2EDX2(2,3,3) = resul.X(); |
598 | D2EDX2(3,3,3) = resul.Y(); |
599 | D2EDX2(4,3,3) = resul.Z(); |
600 | |
81bba717 |
601 | // Derived double compared to u2, v2 |
602 | // Derived from the norm |
7fd59977 |
603 | d2ns1uv2 = (d3uuv2.Crossed(d1v2)) |
604 | + (d2u2 .Crossed(d2v2)) |
605 | + (d1u2 .Crossed(d3uvv2)); |
606 | uterm = ncrossns2.Dot(nplan.Crossed(dns1u2)); |
607 | vterm = ncrossns2.Dot(nplan.Crossed(dns1v2)); |
608 | DSecn = (nplan.Crossed(dns1v2)).Dot(nplan.Crossed(dns1u2)) |
609 | + ncrossns2.Dot(nplan.Crossed(d2ns1uv2)); |
610 | grosterme = (3*uterm*vterm*carre-DSecn)*cube; |
81bba717 |
611 | uterm *= -cube; //and only now |
7fd59977 |
612 | vterm *= -cube; |
613 | |
614 | p1 = nplan.Dot(dns1u2); |
615 | p2 = nplan.Dot(dns1v2); |
616 | temp.SetLinearForm( grosterme*ndotns2, nplan, |
617 | - grosterme, nsurf2, |
618 | - invnorm2, d2ns1uv2); |
619 | d2nduv2.SetLinearForm( invnorm2*nplan.Dot(d2ns1uv2) |
620 | + uterm*p2 |
621 | + vterm*p1, nplan, |
622 | - uterm, dns1v2, |
623 | - vterm, dns1u2); |
624 | |
625 | d2nduv2 += temp; |
626 | resul.SetLinearForm(-ray2, d2nduv2, -1, d2uv2); |
627 | |
628 | D2EDX2(2,4,3) = D2EDX2(2,3,4) = resul.X(); |
629 | D2EDX2(3,4,3) = D2EDX2(3,3,4) = resul.Y(); |
630 | D2EDX2(4,4,3) = D2EDX2(4,3,4) = resul.Z(); |
631 | |
81bba717 |
632 | // Derived double compared to v2 |
633 | // Derived from the norm |
7fd59977 |
634 | d2ns1v2.SetLinearForm(1, d1u2.Crossed(d3v2), |
635 | 2, d2uv2.Crossed(d2v2), |
636 | 1, d3uvv2.Crossed(d1v2)); |
637 | DPrim = ncrossns2.Dot(nplan.Crossed(dns1v2)); |
638 | smallterm = - 2*DPrim*cube; |
639 | DSecn = ncrossns2.Dot(nplan.Crossed(d2ns1v2)) |
640 | + (nplan.Crossed(dns1v2)).SquareMagnitude(); |
641 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
642 | |
643 | p1 = nplan.Dot(dns1v2); |
644 | p2 = nplan.Dot(d2ns1v2); |
645 | temp.SetLinearForm( grosterme*ndotns2, nplan, |
646 | -grosterme , nsurf2); |
647 | d2ndv2.SetLinearForm( invnorm2*p2 |
648 | +smallterm*p1, nplan, |
649 | -smallterm, dns1v2, |
650 | -invnorm2, d2ns1v2); |
651 | d2ndv2 += temp; |
652 | resul.SetLinearForm(-ray2, d2ndv2, -1, d2v2); |
653 | |
654 | D2EDX2(2,4,4) = resul.X(); |
655 | D2EDX2(3,4,4) = resul.Y(); |
656 | D2EDX2(4,4,4) = resul.Z(); |
657 | |
658 | if (byParam) { |
659 | Standard_Real tterm; |
81bba717 |
660 | // ---------- Double Derivation on t, X -------------------------- |
7fd59977 |
661 | D2EDXDT(1,1) = dnplan.Dot(d1u1)/2; |
662 | D2EDXDT(1,2) = dnplan.Dot(d1v1)/2; |
663 | D2EDXDT(1,3) = dnplan.Dot(d1u2)/2; |
664 | D2EDXDT(1,4) = dnplan.Dot(d1v2)/2; |
665 | |
666 | carre = invnorm1*invnorm1; |
667 | cube = carre*invnorm1; |
81bba717 |
668 | //--> Derived compared to u1 and t |
7fd59977 |
669 | tterm = ncrossns1.Dot(dnplan.Crossed(nsurf1)); |
670 | smallterm = - tterm*cube; |
81bba717 |
671 | // Derived from the norm |
7fd59977 |
672 | uterm = ncrossns1.Dot(nplan. Crossed(dns1u1)); |
673 | DSecn = (nplan.Crossed(dns1u1)).Dot(dnplan.Crossed(nsurf1)) |
674 | + ncrossns1.Dot(dnplan.Crossed(dns1u1)); |
675 | grosterme = (3*uterm*tterm*carre - DSecn) * cube; |
676 | uterm *= -cube; |
677 | |
678 | p1 = dnplan.Dot(nsurf1); |
679 | p2 = nplan. Dot(dns1u1); |
680 | p12 = dnplan.Dot(dns1u1); |
681 | |
682 | d2ndtu1.SetLinearForm( invnorm1*p12 |
683 | + smallterm*p2 |
684 | + uterm*p1 |
685 | + grosterme*ndotns1, nplan, |
686 | invnorm1*p2 |
687 | + uterm*ndotns1, dnplan, |
688 | - smallterm, dns1u1); |
689 | d2ndtu1 -= grosterme*nsurf1; |
690 | |
691 | resul.SetLinearForm(ray1, d2ndtu1, sg1*dray, dndu1) ; |
692 | D2EDXDT(2,1) = resul.X(); |
693 | D2EDXDT(3,1) = resul.Y(); |
694 | D2EDXDT(4,1) = resul.Z(); |
695 | |
81bba717 |
696 | //--> Derived compared to v1 and t |
697 | // Derived from the norm |
7fd59977 |
698 | uterm = ncrossns1.Dot(nplan. Crossed(dns1v1)); |
699 | DSecn = (nplan. Crossed(dns1v1)).Dot(dnplan.Crossed(nsurf1)) |
700 | + ncrossns1.Dot(dnplan.Crossed(dns1v1)); |
701 | grosterme = (3*uterm*tterm*carre - DSecn) * cube; |
702 | uterm *= -cube; |
703 | |
704 | p1 = dnplan.Dot(nsurf1); |
705 | p2 = nplan. Dot(dns1v1); |
706 | p12 = dnplan.Dot(dns1v1); |
707 | d2ndtv1.SetLinearForm( invnorm1*p12 |
708 | + uterm*p1 |
709 | + smallterm*p2 |
710 | + grosterme*ndotns1, nplan, |
711 | invnorm1*p2 |
712 | + uterm*ndotns1, dnplan, |
713 | - smallterm , dns1v1); |
714 | d2ndtv1 -= grosterme*nsurf1; |
715 | |
716 | resul.SetLinearForm(ray1, d2ndtv1, sg1*dray, dndv1) ; |
717 | D2EDXDT(2,2) = resul.X(); |
718 | D2EDXDT(3,2) = resul.Y(); |
719 | D2EDXDT(4,2) = resul.Z(); |
720 | |
721 | carre = invnorm2*invnorm2; |
722 | cube = carre*invnorm2; |
81bba717 |
723 | //--> Derived compared to u2 and t |
7fd59977 |
724 | tterm = ncrossns2.Dot(dnplan.Crossed(nsurf2)); |
725 | smallterm = -tterm*cube; |
81bba717 |
726 | // Derived from the norm |
7fd59977 |
727 | uterm = ncrossns2.Dot(nplan. Crossed(dns1u2)); |
728 | DSecn = (nplan. Crossed(dns1u2)).Dot(dnplan.Crossed(nsurf2)) |
729 | + ncrossns2.Dot(dnplan.Crossed(dns1u2)); |
730 | grosterme = (3*uterm*tterm*carre - DSecn) * cube; |
731 | uterm *= -cube; |
732 | |
733 | p1 = dnplan.Dot(nsurf2); |
734 | p2 = nplan. Dot(dns1u2); |
735 | p12 = dnplan.Dot(dns1u2); |
736 | |
737 | d2ndtu2.SetLinearForm( invnorm2*p12 |
738 | + smallterm*p2 |
739 | + uterm*p1 |
740 | + grosterme*ndotns2, nplan, |
741 | invnorm2*p2 |
742 | + uterm*ndotns2, dnplan, |
743 | -smallterm , dns1u2); |
744 | d2ndtu2 -= grosterme*nsurf2; |
745 | |
746 | resul.SetLinearForm( - ray2, d2ndtu2, -sg2*dray, dndu2); |
747 | D2EDXDT(2,3) = resul.X(); |
748 | D2EDXDT(3,3) = resul.Y(); |
749 | D2EDXDT(4,3) = resul.Z(); |
750 | |
81bba717 |
751 | //--> Derived compared to v2 and t |
752 | // Derived from the norm |
7fd59977 |
753 | uterm = ncrossns2.Dot(nplan. Crossed(dns1v2)); |
754 | DSecn = (nplan.Crossed(dns1v2)).Dot(dnplan.Crossed(nsurf2)) |
755 | + ncrossns2.Dot(dnplan.Crossed(dns1v2)); |
756 | grosterme = (3*uterm*tterm*carre - DSecn) * cube; |
757 | uterm *= - cube; |
758 | |
759 | p1 = dnplan.Dot(nsurf2); |
760 | p2 = nplan. Dot(dns1v2); |
761 | p12 = dnplan.Dot(dns1v2); |
762 | |
763 | d2ndtv2.SetLinearForm( invnorm2*p12 |
764 | + smallterm*p2 |
765 | + uterm*p1 |
766 | + grosterme*ndotns2, nplan, |
767 | invnorm2*p2 |
768 | + uterm*ndotns2, dnplan, |
769 | -smallterm , dns1v2); |
770 | d2ndtv2 -= grosterme*nsurf2; |
771 | |
772 | resul.SetLinearForm( - ray2, d2ndtv2, -sg2*dray, dndv2); |
773 | D2EDXDT(2,4) = resul.X(); |
774 | D2EDXDT(3,4) = resul.Y(); |
775 | D2EDXDT(4,4) = resul.Z(); |
776 | |
777 | |
81bba717 |
778 | // ---------- Double derivation on t ----------------------------- |
779 | // Derived from n1 compared to w |
7fd59977 |
780 | carre = invnorm1*invnorm1; |
781 | cube = carre*invnorm1; |
81bba717 |
782 | // Derived from the norm |
7fd59977 |
783 | DPrim = ncrossns1.Dot(dnplan.Crossed(nsurf1)); |
784 | smallterm = - 2*DPrim*cube; |
785 | DSecn = (dnplan.Crossed(nsurf1)).SquareMagnitude() |
786 | + ncrossns1.Dot(d2nplan.Crossed(nsurf1)); |
787 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
788 | |
789 | p1 = dnplan. Dot(nsurf1); |
790 | p2 = d2nplan.Dot(nsurf1); |
791 | |
792 | temp.SetLinearForm( grosterme*ndotns1, nplan, |
793 | -grosterme , nsurf1); |
794 | d2n1w.SetLinearForm( smallterm*p1 |
795 | + invnorm1*p2, nplan, |
796 | smallterm*ndotns1 |
797 | + 2*invnorm1*p1, dnplan, |
798 | ndotns1*invnorm1, d2nplan); |
799 | d2n1w += temp; |
800 | |
81bba717 |
801 | // Derived from n2 compared to w |
7fd59977 |
802 | carre = invnorm2*invnorm2; |
803 | cube = carre*invnorm2; |
81bba717 |
804 | // Derived from the norm |
7fd59977 |
805 | DPrim = ncrossns2.Dot(dnplan.Crossed(nsurf2)); |
806 | smallterm = - 2*DPrim*cube; |
807 | DSecn = (dnplan.Crossed(nsurf2)).SquareMagnitude() |
808 | + ncrossns2.Dot(d2nplan.Crossed(nsurf2)); |
809 | grosterme = (3*DPrim*DPrim*carre - DSecn) * cube; |
810 | |
811 | p1 = dnplan. Dot(nsurf2); |
812 | p2 = d2nplan.Dot(nsurf2); |
813 | |
814 | temp.SetLinearForm( grosterme*ndotns2, nplan, |
815 | -grosterme , nsurf2); |
816 | d2n2w.SetLinearForm( smallterm*p1 |
817 | + invnorm2*p2, nplan, |
818 | smallterm*ndotns2 |
819 | + 2*invnorm2*p1, dnplan, |
820 | ndotns2*invnorm2, d2nplan); |
821 | d2n2w += temp; |
822 | |
823 | temp.SetXYZ( (pts1.XYZ()+pts2.XYZ())/2 - ptgui.XYZ()); |
824 | D2EDT2(1) = d2nplan.Dot(temp) - 2*dnplan.Dot(d1gui) - nplan.Dot(d2gui); |
825 | |
826 | resul.SetLinearForm(ray1, d2n1w, |
827 | 2*sg1*dray, dn1w, |
828 | sg1*d2ray , n1); |
829 | temp.SetLinearForm(ray2, d2n2w, |
830 | 2*sg2*dray, dn2w, |
831 | sg2*d2ray , n2); |
832 | resul -= temp; |
833 | |
834 | D2EDT2(2) = resul.X(); |
835 | D2EDT2(3) = resul.Y(); |
836 | D2EDT2(4) = resul.Z(); |
837 | } |
838 | } |
839 | } |
840 | return Standard_True; |
841 | } |
842 | |
843 | //======================================================================= |
844 | //function : Set |
845 | //purpose : |
846 | //======================================================================= |
847 | |
848 | void BlendFunc_EvolRad::Set(const Standard_Real Param) |
849 | { |
850 | param = Param; |
851 | } |
852 | |
853 | |
854 | //======================================================================= |
855 | //function : Set |
81bba717 |
856 | //purpose : Segments curve in its useful part. |
857 | // Small precision is taken at random |
7fd59977 |
858 | //======================================================================= |
859 | |
860 | void BlendFunc_EvolRad::Set(const Standard_Real First, |
861 | const Standard_Real Last) |
862 | { |
863 | tcurv = curv->Trim(First, Last, 1.e-12); |
864 | tevol = fevol->Trim(First, Last, 1.e-12); |
865 | } |
866 | |
867 | //======================================================================= |
868 | //function : GetTolerance |
869 | //purpose : |
870 | //======================================================================= |
871 | |
872 | void BlendFunc_EvolRad::GetTolerance(math_Vector& Tolerance, |
873 | const Standard_Real Tol) const |
874 | { |
875 | Tolerance(1) = surf1->UResolution(Tol); |
876 | Tolerance(2) = surf1->VResolution(Tol); |
877 | Tolerance(3) = surf2->UResolution(Tol); |
878 | Tolerance(4) = surf2->VResolution(Tol); |
879 | } |
880 | |
881 | |
882 | //======================================================================= |
883 | //function : GetBounds |
884 | //purpose : |
885 | //======================================================================= |
886 | |
887 | void BlendFunc_EvolRad::GetBounds(math_Vector& InfBound, |
888 | math_Vector& SupBound) const |
889 | { |
890 | InfBound(1) = surf1->FirstUParameter(); |
891 | InfBound(2) = surf1->FirstVParameter(); |
892 | InfBound(3) = surf2->FirstUParameter(); |
893 | InfBound(4) = surf2->FirstVParameter(); |
894 | SupBound(1) = surf1->LastUParameter(); |
895 | SupBound(2) = surf1->LastVParameter(); |
896 | SupBound(3) = surf2->LastUParameter(); |
897 | SupBound(4) = surf2->LastVParameter(); |
898 | |
899 | for(Standard_Integer i = 1; i <= 4; i++){ |
900 | if(!Precision::IsInfinite(InfBound(i)) && |
901 | !Precision::IsInfinite(SupBound(i))) { |
902 | Standard_Real range = (SupBound(i) - InfBound(i)); |
903 | InfBound(i) -= range; |
904 | SupBound(i) += range; |
905 | } |
906 | } |
907 | } |
908 | |
909 | |
910 | //======================================================================= |
911 | //function : IsSolution |
912 | //purpose : |
913 | //======================================================================= |
914 | |
915 | Standard_Boolean BlendFunc_EvolRad::IsSolution(const math_Vector& Sol, |
916 | const Standard_Real Tol) |
917 | |
918 | |
919 | { |
920 | Standard_Real norm, Cosa, Sina, Angle; |
921 | Standard_Boolean Ok=Standard_True; |
922 | |
923 | Ok = ComputeValues(Sol, 1, Standard_True, param); |
924 | |
925 | if (Abs(E(1)) <= Tol && |
926 | E(2)*E(2) + E(3)*E(3) + E(4)*E(4) <= Tol*Tol) { |
927 | |
81bba717 |
928 | // ns1, ns2, np are copied locally to avoid crushing the fields ! |
7fd59977 |
929 | gp_Vec ns1, ns2, np; |
930 | ns1 = nsurf1; |
931 | ns2 = nsurf2; |
932 | np = nplan; |
933 | |
934 | norm = nplan.Crossed(ns1).Magnitude(); |
935 | if (norm < Eps) { |
81bba717 |
936 | norm = 1; // Unsatisfactory, but it is not necessary to stop |
7fd59977 |
937 | } |
938 | ns1.SetLinearForm(nplan.Dot(ns1)/norm,nplan, -1./norm, ns1); |
939 | |
940 | norm = nplan.Crossed(ns2).Magnitude(); |
941 | if (norm < Eps) { |
81bba717 |
942 | norm = 1; // Unsatisfactory, but it is not necessary to stop |
7fd59977 |
943 | } |
944 | ns2.SetLinearForm(nplan.Dot(ns2)/norm,nplan, -1./norm, ns2); |
945 | |
946 | Standard_Real maxpiv = 1.e-14; |
947 | math_Gauss Resol(DEDX,maxpiv); |
948 | istangent = Standard_False; |
949 | if (Resol.IsDone()) { |
950 | math_Vector controle(1,4),solution(1,4), tolerances(1,4); |
951 | GetTolerance(tolerances,Tol); |
952 | Resol.Solve(-DEDT,solution); |
953 | controle = DEDT.Added(DEDX.Multiplied(solution)); |
954 | if(Abs(controle(1)) > tolerances(1) || |
955 | Abs(controle(2)) > tolerances(2) || |
956 | Abs(controle(3)) > tolerances(3) || |
957 | Abs(controle(4)) > tolerances(4)){ |
0797d9d3 |
958 | #ifdef OCCT_DEBUG |
04232180 |
959 | std::cout<<"Cheminement : echec calcul des derivees"<<std::endl; |
7fd59977 |
960 | #endif |
961 | istangent = Standard_True; |
962 | } |
963 | if(!istangent){ |
964 | tg1.SetLinearForm(solution(1),d1u1,solution(2),d1v1); |
965 | tg2.SetLinearForm(solution(3),d1u2,solution(4),d1v2); |
966 | tg12d.SetCoord(solution(1),solution(2)); |
967 | tg22d.SetCoord(solution(3),solution(4)); |
968 | } |
969 | } |
970 | else { |
971 | istangent = Standard_True; |
972 | } |
81bba717 |
973 | // update of maxang |
7fd59977 |
974 | |
975 | if (sg1 > 0.) { // sg1*ray |
976 | ns1.Reverse(); |
977 | } |
978 | if (sg2 > 0.) { // sg2*ray |
979 | ns2.Reverse(); |
980 | } |
981 | Cosa = ns1.Dot(ns2); |
982 | Sina = nplan.Dot(ns1.Crossed(ns2)); |
983 | if (choix%2 != 0) { |
81bba717 |
984 | Sina = -Sina; //nplan is changed into -nplan |
7fd59977 |
985 | } |
986 | |
987 | if(Cosa > 1.) {Cosa = 1.; Sina = 0.;} |
988 | Angle = ACos(Cosa); |
81bba717 |
989 | // Reframing on ]-pi/2, 3pi/2] |
7fd59977 |
990 | if (Sina <0.) { |
991 | if (Cosa > 0.) Angle = -Angle; |
c6541a0c |
992 | else Angle = 2.*M_PI - Angle; |
7fd59977 |
993 | } |
994 | |
995 | if (Abs(Angle)>maxang) {maxang = Abs(Angle);} |
996 | if (Abs(Angle)<minang) {minang = Abs(Angle);} |
997 | if (Abs(Angle*ray) < lengthmin) { lengthmin = Abs(Angle*ray);} |
998 | if (Abs(Angle*ray) > lengthmax) { lengthmax = Abs(Angle*ray);} |
999 | distmin = Min(distmin, pts1.Distance(pts2)); |
1000 | |
1001 | return Ok; |
1002 | } |
1003 | istangent = Standard_True; |
1004 | return Standard_False; |
1005 | } |
1006 | |
1007 | //======================================================================= |
1008 | //function : GetMinimalDistance |
1009 | //purpose : |
1010 | //======================================================================= |
1011 | |
1012 | Standard_Real BlendFunc_EvolRad::GetMinimalDistance() const |
1013 | { |
1014 | return distmin; |
1015 | } |
1016 | |
1017 | //======================================================================= |
1018 | //function : Value |
1019 | //purpose : |
1020 | //======================================================================= |
1021 | |
1022 | Standard_Boolean BlendFunc_EvolRad::Value(const math_Vector& X, |
1023 | math_Vector& F) |
1024 | { |
1025 | Standard_Boolean Ok; |
1026 | Ok = ComputeValues(X, 0); |
1027 | F = E; |
1028 | return Ok; |
1029 | } |
1030 | |
1031 | |
1032 | |
1033 | //======================================================================= |
1034 | //function : Derivatives |
1035 | //purpose : |
1036 | //======================================================================= |
1037 | |
1038 | Standard_Boolean BlendFunc_EvolRad::Derivatives(const math_Vector& X, |
1039 | math_Matrix& D) |
1040 | { |
1041 | Standard_Boolean Ok; |
1042 | Ok = ComputeValues(X, 1); |
1043 | D = DEDX; |
1044 | return Ok; |
1045 | } |
1046 | |
1047 | |
1048 | Standard_Boolean BlendFunc_EvolRad::Values(const math_Vector& X, |
1049 | math_Vector& F, |
1050 | math_Matrix& D) |
1051 | { |
1052 | Standard_Boolean Ok; |
1053 | Ok = ComputeValues(X, 1); |
1054 | F = E; |
1055 | D = DEDX; |
1056 | return Ok; |
1057 | } |
1058 | |
1059 | |
1060 | |
1061 | //======================================================================= |
1062 | //function : Tangent |
1063 | //purpose : |
1064 | //======================================================================= |
1065 | |
1066 | void BlendFunc_EvolRad::Tangent(const Standard_Real U1, |
1067 | const Standard_Real V1, |
1068 | const Standard_Real U2, |
1069 | const Standard_Real V2, |
1070 | gp_Vec& TgF, |
1071 | gp_Vec& TgL, |
1072 | gp_Vec& NmF, |
1073 | gp_Vec& NmL) const |
1074 | { |
1075 | gp_Pnt Center; |
1076 | gp_Vec ns1; |
1077 | Standard_Real invnorm1; |
1078 | |
1079 | if ((U1!=xval(1)) || (V1!=xval(2)) || |
1080 | (U2!=xval(3)) || (V2!=xval(4))) { |
1081 | gp_Vec d1u,d1v; |
1082 | gp_Pnt bid; |
0797d9d3 |
1083 | #ifdef OCCT_DEBUG |
04232180 |
1084 | std::cout << " erreur de tengent !!!!!!!!!!!!!!!!!!!!" << std::endl; |
7fd59977 |
1085 | #endif |
1086 | surf1->D1(U1,V1,bid,d1u,d1v); |
1087 | NmF = ns1 = d1u.Crossed(d1v); |
1088 | surf2->D1(U2,V2,bid,d1u,d1v); |
1089 | NmL = d1u.Crossed(d1v); |
1090 | } |
1091 | else { |
1092 | NmF = ns1 = nsurf1; |
1093 | NmL = nsurf2; |
1094 | } |
1095 | |
1096 | invnorm1 = nplan.Crossed(ns1).Magnitude(); |
1097 | if (invnorm1 < Eps) invnorm1 = 1; |
1098 | else invnorm1 = 1. / invnorm1; |
1099 | ns1.SetLinearForm(nplan.Dot(ns1)*invnorm1,nplan, -invnorm1,ns1); |
1100 | |
1101 | Center.SetXYZ(pts1.XYZ()+sg1*ray*ns1.XYZ()); |
1102 | |
1103 | TgF = nplan.Crossed(gp_Vec(Center,pts1)); |
1104 | TgL = nplan.Crossed(gp_Vec(Center,pts2)); |
1105 | if (choix%2 == 1) { |
1106 | TgF.Reverse(); |
1107 | TgL.Reverse(); |
1108 | } |
1109 | } |
1110 | |
1111 | //======================================================================= |
1112 | //function : TwistOnS1 |
1113 | //purpose : |
1114 | //======================================================================= |
1115 | |
1116 | Standard_Boolean BlendFunc_EvolRad::TwistOnS1() const |
1117 | { |
9775fa61 |
1118 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1119 | return tg1.Dot(nplan) < 0.; |
1120 | } |
1121 | |
1122 | //======================================================================= |
1123 | //function : TwistOnS2 |
1124 | //purpose : |
1125 | //======================================================================= |
1126 | |
1127 | Standard_Boolean BlendFunc_EvolRad::TwistOnS2() const |
1128 | { |
9775fa61 |
1129 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1130 | return tg2.Dot(nplan) < 0.; |
1131 | } |
1132 | |
1133 | //======================================================================= |
1134 | //function : Section |
1135 | //purpose : |
1136 | //======================================================================= |
1137 | |
1138 | void BlendFunc_EvolRad::Section(const Standard_Real Param, |
1139 | const Standard_Real U1, |
1140 | const Standard_Real V1, |
1141 | const Standard_Real U2, |
1142 | const Standard_Real V2, |
1143 | Standard_Real& Pdeb, |
1144 | Standard_Real& Pfin, |
1145 | gp_Circ& C) |
1146 | { |
1147 | gp_Pnt Center; |
1148 | gp_Vec ns1,np; |
1149 | |
1150 | math_Vector X(1,4); |
1151 | X(1) = U1; X(2) = V1; X(3) = U2; X(4) = V2; |
1152 | Standard_Real prm = Param; |
7fd59977 |
1153 | |
96a95605 |
1154 | ComputeValues(X, 0, Standard_True, prm); |
7fd59977 |
1155 | |
1156 | ns1 = nsurf1; |
1157 | np = nplan; |
1158 | |
1159 | Standard_Real norm1; |
1160 | norm1 = nplan.Crossed(ns1).Magnitude(); |
1161 | if (norm1 < Eps) { |
81bba717 |
1162 | norm1 = 1; // Unsatisfactory, but it is not necessary to stop |
7fd59977 |
1163 | } |
1164 | ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1); |
1165 | |
1166 | Center.SetXYZ(pts1.XYZ()+sg1*ray*ns1.XYZ()); |
1167 | |
81bba717 |
1168 | // ns1 is oriented from the center to pts1 |
7fd59977 |
1169 | if (sg1 > 0.) { |
1170 | ns1.Reverse(); |
1171 | } |
1172 | if (choix%2 != 0) { |
1173 | np.Reverse(); |
1174 | } |
1175 | C.SetRadius(Abs(ray)); |
1176 | C.SetPosition(gp_Ax2(Center,np,ns1)); |
1177 | Pdeb = 0.; |
1178 | Pfin = ElCLib::Parameter(C,pts2); |
81bba717 |
1179 | // Test of negative and almost null angles : Single Case |
c6541a0c |
1180 | if (Pfin>1.5*M_PI) { |
7fd59977 |
1181 | np.Reverse(); |
1182 | C.SetPosition(gp_Ax2(Center,np,ns1)); |
1183 | Pfin = ElCLib::Parameter(C,pts2); |
1184 | } |
1185 | if (Pfin < Precision::PConfusion()) Pfin += Precision::PConfusion(); |
1186 | } |
1187 | |
1188 | |
1189 | //======================================================================= |
1190 | //function : PointOnS1 |
1191 | //purpose : |
1192 | //======================================================================= |
1193 | |
1194 | const gp_Pnt& BlendFunc_EvolRad::PointOnS1 () const |
1195 | { |
1196 | return pts1; |
1197 | } |
1198 | |
1199 | |
1200 | //======================================================================= |
1201 | //function : PointOnS2 |
1202 | //purpose : |
1203 | //======================================================================= |
1204 | |
1205 | const gp_Pnt& BlendFunc_EvolRad::PointOnS2 () const |
1206 | { |
1207 | return pts2; |
1208 | } |
1209 | |
1210 | |
1211 | //======================================================================= |
1212 | //function : IsTangencyPoint |
1213 | //purpose : |
1214 | //======================================================================= |
1215 | |
1216 | Standard_Boolean BlendFunc_EvolRad::IsTangencyPoint () const |
1217 | { |
1218 | return istangent; |
1219 | } |
1220 | |
1221 | |
1222 | //======================================================================= |
1223 | //function : TangentOnS1 |
1224 | //purpose : |
1225 | //======================================================================= |
1226 | |
1227 | const gp_Vec& BlendFunc_EvolRad::TangentOnS1 () const |
1228 | { |
9775fa61 |
1229 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1230 | return tg1; |
1231 | } |
1232 | |
1233 | |
1234 | //======================================================================= |
1235 | //function : TangentOnS2 |
1236 | //purpose : |
1237 | //======================================================================= |
1238 | |
1239 | const gp_Vec& BlendFunc_EvolRad::TangentOnS2 () const |
1240 | { |
9775fa61 |
1241 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1242 | return tg2; |
1243 | } |
1244 | |
1245 | |
1246 | //======================================================================= |
1247 | //function : Tangent2dOnS1 |
1248 | //purpose : |
1249 | //======================================================================= |
1250 | |
1251 | const gp_Vec2d& BlendFunc_EvolRad::Tangent2dOnS1 () const |
1252 | { |
9775fa61 |
1253 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1254 | return tg12d; |
1255 | } |
1256 | |
1257 | |
1258 | //======================================================================= |
1259 | //function : Tangent2dOnS2 |
1260 | //purpose : |
1261 | //======================================================================= |
1262 | |
1263 | const gp_Vec2d& BlendFunc_EvolRad::Tangent2dOnS2 () const |
1264 | { |
9775fa61 |
1265 | if (istangent) {throw Standard_DomainError();} |
7fd59977 |
1266 | return tg22d; |
1267 | } |
1268 | |
1269 | //======================================================================= |
1270 | //function : IsRational |
1271 | //purpose : |
1272 | //======================================================================= |
1273 | Standard_Boolean BlendFunc_EvolRad::IsRational () const |
1274 | { |
1275 | return (mySShape==BlendFunc_Rational || mySShape==BlendFunc_QuasiAngular); |
1276 | } |
1277 | |
1278 | //======================================================================= |
1279 | //function : GetSectionSize |
1280 | //purpose : |
1281 | //======================================================================= |
1282 | Standard_Real BlendFunc_EvolRad::GetSectionSize() const |
1283 | { |
1284 | return lengthmax; |
1285 | } |
1286 | |
1287 | //======================================================================= |
1288 | //function : GetMinimalWeight |
1289 | //purpose : |
1290 | //======================================================================= |
1291 | void BlendFunc_EvolRad::GetMinimalWeight(TColStd_Array1OfReal& Weights) const |
1292 | { |
1293 | BlendFunc::GetMinimalWeights(mySShape, myTConv, |
1294 | minang, maxang, Weights ); |
1295 | } |
1296 | |
1297 | //======================================================================= |
1298 | //function : NbIntervals |
1299 | //purpose : |
1300 | //======================================================================= |
1301 | |
1302 | Standard_Integer BlendFunc_EvolRad::NbIntervals (const GeomAbs_Shape S) const |
1303 | { |
1304 | Standard_Integer Nb_Int_Courbe, Nb_Int_Loi; |
1305 | Nb_Int_Courbe = curv->NbIntervals(BlendFunc::NextShape(S)); |
1306 | Nb_Int_Loi = fevol->NbIntervals(S); |
1307 | |
1308 | if (Nb_Int_Loi==1) { |
1309 | return Nb_Int_Courbe; |
1310 | } |
1311 | |
1312 | TColStd_Array1OfReal IntC(1, Nb_Int_Courbe+1); |
1313 | TColStd_Array1OfReal IntL(1, Nb_Int_Loi+1); |
1314 | TColStd_SequenceOfReal Inter; |
1315 | curv->Intervals(IntC, BlendFunc::NextShape(S)); |
1316 | fevol->Intervals(IntL, S); |
1317 | |
1318 | FusionneIntervalles( IntC, IntL, Inter); |
1319 | return Inter.Length()-1; |
1320 | } |
1321 | |
1322 | |
1323 | //======================================================================= |
1324 | //function : Intervals |
1325 | //purpose : |
1326 | //======================================================================= |
1327 | |
1328 | void BlendFunc_EvolRad::Intervals (TColStd_Array1OfReal& T, |
1329 | const GeomAbs_Shape S) const |
1330 | { |
1331 | Standard_Integer Nb_Int_Courbe, Nb_Int_Loi; |
1332 | Nb_Int_Courbe = curv->NbIntervals(BlendFunc::NextShape(S)); |
1333 | Nb_Int_Loi = fevol->NbIntervals(S); |
1334 | |
1335 | if (Nb_Int_Loi==1) { |
1336 | curv->Intervals(T, BlendFunc::NextShape(S)); |
1337 | } |
1338 | else { |
1339 | TColStd_Array1OfReal IntC(1, Nb_Int_Courbe+1); |
1340 | TColStd_Array1OfReal IntL(1, Nb_Int_Loi+1); |
1341 | TColStd_SequenceOfReal Inter; |
1342 | curv->Intervals(IntC, BlendFunc::NextShape(S)); |
1343 | fevol->Intervals(IntL, S); |
1344 | |
1345 | FusionneIntervalles( IntC, IntL, Inter); |
1346 | for (Standard_Integer ii=1; ii<=Inter.Length(); ii++) { |
1347 | T(ii) = Inter(ii); |
1348 | } |
1349 | } |
1350 | } |
1351 | |
1352 | //======================================================================= |
1353 | //function : GetShape |
1354 | //purpose : |
1355 | //======================================================================= |
1356 | |
1357 | void BlendFunc_EvolRad::GetShape (Standard_Integer& NbPoles, |
1358 | Standard_Integer& NbKnots, |
1359 | Standard_Integer& Degree, |
1360 | Standard_Integer& NbPoles2d) |
1361 | { |
1362 | NbPoles2d = 2; |
1363 | BlendFunc::GetShape(mySShape,maxang,NbPoles,NbKnots,Degree,myTConv); |
1364 | } |
1365 | |
1366 | //======================================================================= |
1367 | //function : GetTolerance |
81bba717 |
1368 | //purpose : Determine the Tolerance to be used in approximations. |
7fd59977 |
1369 | //======================================================================= |
1370 | void BlendFunc_EvolRad::GetTolerance(const Standard_Real BoundTol, |
1371 | const Standard_Real SurfTol, |
1372 | const Standard_Real AngleTol, |
1373 | math_Vector& Tol3d, |
1374 | math_Vector& Tol1d) const |
1375 | { |
1376 | Standard_Integer low = Tol3d.Lower() , up=Tol3d.Upper(); |
81bba717 |
1377 | Standard_Real rayon = lengthmin/maxang; // a radius is subtracted |
7fd59977 |
1378 | Standard_Real Tol; |
1379 | Tol= GeomFill::GetTolerance(myTConv, maxang, rayon, |
1380 | AngleTol, SurfTol); |
1381 | Tol1d.Init(SurfTol); |
1382 | Tol3d.Init(SurfTol); |
1383 | Tol3d(low+1) = Tol3d(up-1) = Min(Tol, SurfTol); |
1384 | Tol3d(low) = Tol3d(up) = Min(Tol, BoundTol); |
1385 | |
1386 | } |
1387 | |
1388 | //======================================================================= |
1389 | //function : Knots |
1390 | //purpose : |
1391 | //======================================================================= |
1392 | |
1393 | void BlendFunc_EvolRad::Knots(TColStd_Array1OfReal& TKnots) |
1394 | { |
1395 | GeomFill::Knots(myTConv, TKnots); |
1396 | } |
1397 | |
1398 | |
1399 | //======================================================================= |
1400 | //function : Mults |
1401 | //purpose : |
1402 | //======================================================================= |
1403 | |
1404 | void BlendFunc_EvolRad::Mults(TColStd_Array1OfInteger& TMults) |
1405 | { |
1406 | GeomFill::Mults(myTConv, TMults); |
1407 | } |
1408 | |
1409 | |
1410 | //======================================================================= |
1411 | //function : Section |
1412 | //purpose : |
1413 | //======================================================================= |
1414 | |
1415 | void BlendFunc_EvolRad::Section(const Blend_Point& P, |
1416 | TColgp_Array1OfPnt& Poles, |
1417 | TColgp_Array1OfPnt2d& Poles2d, |
1418 | TColStd_Array1OfReal& Weights) |
1419 | { |
1420 | gp_Pnt Center; |
1421 | gp_Vec ns1,ns2,np; |
1422 | |
1423 | math_Vector X(1,4); |
1424 | Standard_Real prm = P.Parameter(); |
7fd59977 |
1425 | |
1426 | Standard_Integer low = Poles.Lower(); |
1427 | Standard_Integer upp = Poles.Upper(); |
1428 | |
1429 | P.ParametersOnS1(X(1), X(2)); |
1430 | P.ParametersOnS2(X(3), X(4)); |
1431 | |
81bba717 |
1432 | // Calculation and storage of distmin |
96a95605 |
1433 | ComputeValues(X, 0, Standard_True, prm); |
7fd59977 |
1434 | distmin = Min (distmin, pts1.Distance(pts2)); |
1435 | |
81bba717 |
1436 | // ns1, ns2, np are copied locally to avoid crashing the fields ! |
7fd59977 |
1437 | ns1 = nsurf1; |
1438 | ns2 = nsurf2; |
1439 | np = nplan; |
1440 | |
1441 | |
1442 | Poles2d(Poles2d.Lower()).SetCoord(X(1), X(2)); |
1443 | Poles2d(Poles2d.Upper()).SetCoord(X(3), X(4)); |
1444 | |
1445 | if (mySShape == BlendFunc_Linear) { |
1446 | Poles(low) = pts1; |
1447 | Poles(upp) = pts2; |
1448 | Weights(low) = 1.0; |
1449 | Weights(upp) = 1.0; |
1450 | return; |
1451 | } |
1452 | |
1453 | Standard_Real norm1, norm2; |
1454 | norm1 = nplan.Crossed(ns1).Magnitude(); |
1455 | norm2 = nplan.Crossed(ns2).Magnitude(); |
1456 | if (norm1 < Eps) { |
81bba717 |
1457 | norm1 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1458 | #ifdef OCCT_DEBUG |
04232180 |
1459 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1460 | #endif |
1461 | } |
1462 | if (norm2 < Eps) { |
81bba717 |
1463 | norm2 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1464 | #ifdef OCCT_DEBUG |
04232180 |
1465 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1466 | #endif |
1467 | } |
1468 | |
1469 | ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1); |
1470 | ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2); |
1471 | |
1472 | Center.SetXYZ(pts1.XYZ()+sg1*ray*ns1.XYZ()); |
1473 | |
1474 | |
81bba717 |
1475 | // ns1 (resp. ns2) is oriented from center to pts1 (resp. pts2), |
1476 | // and the trihedron ns1,ns2,nplan is made direct. |
7fd59977 |
1477 | |
1478 | if (sg1 > 0.) { |
1479 | ns1.Reverse(); |
1480 | } |
1481 | if (sg2 >0.) { |
1482 | ns2.Reverse(); |
1483 | } |
1484 | if (choix%2 != 0) { |
1485 | np.Reverse(); |
1486 | } |
1487 | |
1488 | GeomFill::GetCircle(myTConv, |
1489 | ns1, ns2, |
1490 | np, pts1, pts2, |
1491 | Abs(ray), Center, |
1492 | Poles, Weights); |
1493 | } |
1494 | |
1495 | //======================================================================= |
1496 | //function : Section |
1497 | //purpose : |
1498 | //======================================================================= |
1499 | |
1500 | Standard_Boolean BlendFunc_EvolRad::Section |
1501 | (const Blend_Point& P, |
1502 | TColgp_Array1OfPnt& Poles, |
1503 | TColgp_Array1OfVec& DPoles, |
1504 | TColgp_Array1OfPnt2d& Poles2d, |
1505 | TColgp_Array1OfVec2d& DPoles2d, |
1506 | TColStd_Array1OfReal& Weights, |
1507 | TColStd_Array1OfReal& DWeights) |
1508 | { |
1509 | gp_Vec ns1, ns2, np, dnp, dnorm1w, dnorm2w, tgc; |
1510 | Standard_Real norm1, norm2, rayprim; |
1511 | |
1512 | gp_Pnt Center; |
1513 | math_Vector sol(1,4), secmember(1,4); |
1514 | |
1515 | Standard_Real prm = P.Parameter(); |
1516 | Standard_Integer low = Poles.Lower(); |
1517 | Standard_Integer upp = Poles.Upper(); |
1518 | Standard_Boolean istgt = Standard_True; |
1519 | |
1520 | P.ParametersOnS1(sol(1),sol(2)); |
1521 | P.ParametersOnS2(sol(3),sol(4)); |
1522 | |
81bba717 |
1523 | // Calculation of equations |
7fd59977 |
1524 | ComputeValues(sol, 1, Standard_True, prm); |
1525 | distmin = Min (distmin, pts1.Distance(pts2)); |
1526 | |
81bba717 |
1527 | // ns1, ns2, np are copied locally to avoid crashing fields ! |
7fd59977 |
1528 | ns1 = nsurf1; |
1529 | ns2 = nsurf2; |
1530 | np = nplan; |
1531 | dnp = dnplan; |
1532 | rayprim = dray; |
1533 | |
1534 | if ( ! pts1.IsEqual(pts2, 1.e-4)) { |
81bba717 |
1535 | // Calculation of derived Normal processing |
7fd59977 |
1536 | math_Gauss Resol(DEDX, 1.e-9); |
1537 | |
1538 | if (Resol.IsDone()) { |
1539 | Resol.Solve(-DEDT, secmember); |
1540 | istgt = Standard_False; |
1541 | } |
1542 | } |
1543 | |
1544 | if (istgt) { |
1545 | math_SVD SingRS (DEDX); |
1546 | if (SingRS.IsDone()) { |
1547 | SingRS.Solve(-DEDT, secmember, 1.e-6); |
1548 | istgt = Standard_False; |
1549 | } |
1550 | } |
1551 | |
1552 | if (!istgt) { |
1553 | tg1.SetLinearForm(secmember(1),d1u1, secmember(2),d1v1); |
1554 | tg2.SetLinearForm(secmember(3),d1u2, secmember(4),d1v2); |
1555 | |
1556 | dnorm1w.SetLinearForm(secmember(1),dndu1, secmember(2),dndv1, dn1w); |
1557 | dnorm2w.SetLinearForm(secmember(3),dndu2, secmember(4),dndv2, dn2w); |
1558 | |
1559 | istgt = Standard_False; |
1560 | } |
1561 | |
1562 | |
81bba717 |
1563 | // Tops 2D |
7fd59977 |
1564 | Poles2d(Poles2d.Lower()).SetCoord(sol(1),sol(2)); |
1565 | Poles2d(Poles2d.Upper()).SetCoord(sol(3),sol(4)); |
1566 | if (!istgt) { |
1567 | DPoles2d(Poles2d.Lower()).SetCoord(secmember(1),secmember(2)); |
1568 | DPoles2d(Poles2d.Upper()).SetCoord(secmember(3),secmember(4)); |
1569 | } |
1570 | |
81bba717 |
1571 | // the linear case is processed... |
7fd59977 |
1572 | if (mySShape == BlendFunc_Linear) { |
1573 | Poles(low) = pts1; |
1574 | Poles(upp) = pts2; |
1575 | Weights(low) = 1.0; |
1576 | Weights(upp) = 1.0; |
1577 | if (!istgt) { |
1578 | DPoles(low) = tg1; |
1579 | DPoles(upp) = tg2; |
1580 | DWeights(low) = 0.0; |
1581 | DWeights(upp) = 0.0; |
1582 | } |
1583 | return (!istgt); |
1584 | } |
1585 | |
81bba717 |
1586 | // Case of the circle |
7fd59977 |
1587 | norm1 = nplan.Crossed(ns1).Magnitude(); |
1588 | norm2 = nplan.Crossed(ns2).Magnitude(); |
1589 | if (norm1 < Eps) { |
81bba717 |
1590 | norm1 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1591 | #ifdef OCCT_DEBUG |
04232180 |
1592 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1593 | #endif |
1594 | } |
1595 | if (norm2 < Eps) { |
81bba717 |
1596 | norm2 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1597 | #ifdef OCCT_DEBUG |
04232180 |
1598 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1599 | #endif |
1600 | } |
1601 | |
1602 | ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1); |
1603 | ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2); |
1604 | |
1605 | Center.SetXYZ(pts1.XYZ()+sg1*ray*ns1.XYZ()); |
1606 | if (!istgt) { |
1607 | tgc.SetLinearForm(sg1*ray, dnorm1w, |
1608 | sg1*dray, ns1, |
1609 | tg1); |
1610 | } |
1611 | |
81bba717 |
1612 | // ns1 is oriented from center to pts1, and ns2 from center to pts2 |
1613 | // and the trihedron ns1,ns2,nplan is made direct |
7fd59977 |
1614 | |
1615 | if (sg1 > 0.) { |
1616 | ns1.Reverse(); |
1617 | if (!istgt) { |
1618 | dnorm1w.Reverse(); |
1619 | } |
1620 | } |
1621 | if (sg2 >0.) { |
1622 | ns2.Reverse(); |
1623 | if (!istgt) { |
1624 | dnorm2w.Reverse(); |
1625 | } |
1626 | } |
1627 | if (choix%2 != 0) { |
1628 | np.Reverse(); |
1629 | dnp.Reverse(); |
1630 | } |
1631 | |
81bba717 |
1632 | if (ray < 0.) { // to avoid Abs(dray) some lines below |
7fd59977 |
1633 | rayprim = -rayprim; |
1634 | } |
1635 | |
1636 | if (!istgt) { |
1637 | return GeomFill::GetCircle(myTConv, |
1638 | ns1, ns2, |
1639 | dnorm1w, dnorm2w, |
1640 | np, dnp, |
1641 | pts1, pts2, |
1642 | tg1, tg2, |
1643 | Abs(ray), rayprim, |
1644 | Center, tgc, |
1645 | Poles, |
1646 | DPoles, |
1647 | Weights, |
1648 | DWeights); |
1649 | } |
1650 | else { |
1651 | GeomFill::GetCircle(myTConv, |
1652 | ns1, ns2, |
1653 | np, pts1, pts2, |
1654 | Abs(ray), Center, |
1655 | Poles, Weights); |
1656 | return Standard_False; |
1657 | } |
1658 | } |
1659 | |
1660 | //======================================================================= |
1661 | //function : Section |
1662 | //purpose : |
1663 | //======================================================================= |
1664 | |
1665 | Standard_Boolean BlendFunc_EvolRad::Section |
1666 | (const Blend_Point& P, |
1667 | TColgp_Array1OfPnt& Poles, |
1668 | TColgp_Array1OfVec& DPoles, |
1669 | TColgp_Array1OfVec& D2Poles, |
1670 | TColgp_Array1OfPnt2d& Poles2d, |
1671 | TColgp_Array1OfVec2d& DPoles2d, |
1672 | TColgp_Array1OfVec2d& D2Poles2d, |
1673 | TColStd_Array1OfReal& Weights, |
1674 | TColStd_Array1OfReal& DWeights, |
1675 | TColStd_Array1OfReal& D2Weights) |
1676 | { |
1677 | gp_Vec ns1,ns2, np, dnp, d2np, dnorm1w, dnorm2w, d2norm1w, d2norm2w; |
1678 | gp_Vec tgc, dtgc, dtg1, dtg2, temp, tempbis; |
1679 | Standard_Real norm1, norm2, rayprim, raysecn; |
1680 | |
1681 | gp_Pnt Center; |
1682 | math_Vector X(1,4), sol(1,4), secmember(1,4); |
1683 | math_Matrix D2DXdSdt(1,4,1,4); |
1684 | |
1685 | Standard_Real prm = P.Parameter(); |
1686 | Standard_Integer low = Poles.Lower(); |
1687 | Standard_Integer upp = Poles.Upper(); |
1688 | Standard_Boolean istgt = Standard_True; |
1689 | |
1690 | P.ParametersOnS1(X(1), X(2)); |
1691 | P.ParametersOnS2(X(3), X(4)); |
1692 | |
1693 | /* |
0797d9d3 |
1694 | #ifdef OCCT_DEBUG |
7fd59977 |
1695 | Standard_Real deltat = 1.e-9; |
1696 | if (prm==tcurv->LastParameter()){deltat *= -1;} //Pour les discont |
1697 | Standard_Real deltaX = 1.e-9; |
1698 | Standard_Integer ii, jj; |
1699 | gp_Vec d_plan, d1, d2, pdiff; |
1700 | math_Matrix M(1,4,1,4), MDiff(1,4,1,4); |
1701 | math_Matrix Mu1(1,4,1,4), Mv1(1,4,1,4); |
1702 | math_Matrix Mu2(1,4,1,4), Mv2(1,4,1,4); |
1703 | math_Vector V(1,4), VDiff(1,4),dx(1,4); |
1704 | |
1705 | dx = X; |
1706 | dx(1)+=deltaX; |
1707 | ComputeValues(dx, 1, Standard_True, prm ); |
1708 | Mu1 = DEDX; |
1709 | |
1710 | dx = X; |
1711 | dx(2)+=deltaX; |
1712 | ComputeValues(dx, 1, Standard_True, prm); |
1713 | Mv1 = DEDX; |
1714 | |
1715 | dx = X; |
1716 | dx(3)+=deltaX; |
1717 | ComputeValues(dx, 1, Standard_True, prm ); |
1718 | Mu2 = DEDX; |
1719 | |
1720 | dx = X; |
1721 | dx(4)+=deltaX; |
1722 | ComputeValues(dx, 1, Standard_True, prm ); |
1723 | Mv2 = DEDX; |
1724 | |
1725 | ComputeValues(X, 1, Standard_True, prm+deltat); |
1726 | M = DEDX; |
1727 | V = DEDT; |
1728 | d_plan = dnplan; |
1729 | d1 = dn1w; |
1730 | d2 = dn2w; |
1731 | # endif |
1732 | */ |
1733 | // Calculs des equations |
1734 | ComputeValues(X, 2, Standard_True, prm); |
1735 | distmin = Min (distmin, pts1.Distance(pts2)); |
1736 | |
1737 | /* |
0797d9d3 |
1738 | #ifdef OCCT_DEBUG |
7fd59977 |
1739 | MDiff = (M - DEDX)*(1/deltat); |
1740 | VDiff = (V - DEDT)*(1/deltat); |
1741 | |
1742 | pdiff = (d_plan - dnplan)*(1/deltat); |
1743 | if ((pdiff-d2nplan).Magnitude() > 1.e-4*(pdiff.Magnitude()+1.e-1)) |
1744 | { |
04232180 |
1745 | std::cout << "d2nplan = (" << d2nplan.X() << ","<< d2nplan.Y() << ","<< d2nplan.Z() << ")"<<std::endl; |
1746 | std::cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<std::endl; |
7fd59977 |
1747 | } |
1748 | |
1749 | pdiff = (d1 - dn1w)*(1/deltat); |
1750 | if ((pdiff-d2n1w).Magnitude() > 1.e-4*(pdiff.Magnitude()+1.e-1)) |
1751 | { |
04232180 |
1752 | std::cout << "d2n1w = (" << d2n1w.X() << ","<< d2n1w.Y() << ","<< d2n1w.Z() << ")"<<std::endl; |
1753 | std::cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<std::endl; |
7fd59977 |
1754 | } |
1755 | pdiff = (d2 - dn2w)*(1/deltat); |
1756 | if ((pdiff-d2n2w).Magnitude() > 1.e-4*(pdiff.Magnitude()+1.e-1)) |
1757 | { |
04232180 |
1758 | std::cout << "d2n2w = (" << d2n2w.X() << ","<< d2n2w.Y() << ","<< d2n2w.Z() << ")"<<std::endl; |
1759 | std::cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<std::endl; |
7fd59977 |
1760 | } |
1761 | |
1762 | |
1763 | for ( ii=1; ii<=4; ii++) { |
1764 | if (Abs(VDiff(ii)-D2EDT2(ii)) > 1.e-4*(Abs(D2EDT2(ii))+1.e-1)) |
1765 | { |
04232180 |
1766 | std::cout << "erreur sur D2EDT2 : "<< ii << std::endl; |
1767 | std::cout << D2EDT2(ii) << " D.F = " << VDiff(ii) << std::endl; |
7fd59977 |
1768 | } |
1769 | for (jj=1; jj<=4; jj++) { |
1770 | if (Abs(MDiff(ii,jj)-D2EDXDT(ii, jj)) > |
1771 | 1.e-3*(Abs(D2EDXDT(ii, jj))+1.e-2)) |
1772 | { |
04232180 |
1773 | std::cout << "erreur sur D2EDXDT : "<< ii << " , " << jj << std::endl; |
1774 | std::cout << D2EDXDT(ii,jj) << " D.F = " << MDiff(ii,jj) << std::endl; |
7fd59977 |
1775 | } |
1776 | } |
1777 | } |
1778 | // Test de D2EDX2 en u1 |
1779 | MDiff = (Mu1 - DEDX)/deltaX; |
1780 | for (ii=1; ii<=4; ii++) { |
1781 | for (jj=1; jj<=4; jj++) { |
1782 | if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 1)) > |
1783 | 1.e-4*(Abs(D2EDX2(ii, jj, 1))+1.e-1)) |
1784 | { |
04232180 |
1785 | std::cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 1 << std::endl; |
1786 | std::cout << D2EDX2(ii,jj, 1) << " D.F = " << MDiff(ii,jj) << std::endl; |
7fd59977 |
1787 | } |
1788 | } |
1789 | } |
1790 | |
1791 | // Test de D2EDX2 en v1 |
1792 | MDiff = (Mv1 - DEDX)/deltaX; |
1793 | for (ii=1; ii<=4; ii++) { |
1794 | for (jj=1; jj<=4; jj++) { |
1795 | if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 2)) > |
1796 | 1.e-4*(Abs(D2EDX2(ii, jj, 2))+1.e-1)) |
1797 | { |
04232180 |
1798 | std::cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 2 << std::endl; |
1799 | std::cout << D2EDX2(ii,jj, 2) << " D.F = " << MDiff(ii,jj) << std::endl; |
7fd59977 |
1800 | } |
1801 | } |
1802 | } |
1803 | // Test de D2EDX2 en u2 |
1804 | MDiff = (Mu2 - DEDX)/deltaX; |
1805 | for (ii=1; ii<=4; ii++) { |
1806 | for (jj=1; jj<=4; jj++) { |
1807 | if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 3)) > |
1808 | 1.e-4*(Abs(D2EDX2(ii, jj, 3))+1.e-1)) |
1809 | { |
04232180 |
1810 | std::cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 3 << std::endl; |
1811 | std::cout << D2EDX2(ii,jj, 3) << " D.F = " << MDiff(ii,jj) << std::endl; |
7fd59977 |
1812 | } |
1813 | } |
1814 | } |
1815 | |
1816 | // Test de D2EDX2 en v2 |
1817 | MDiff = (Mv2 - DEDX)/deltaX; |
1818 | for (ii=1; ii<=4; ii++) { |
1819 | for (jj=1; jj<=4; jj++) { |
1820 | if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 4)) > |
1821 | 1.e-4*(Abs(D2EDX2(ii, jj, 4))+1.e-1)) |
1822 | { |
04232180 |
1823 | std::cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " |
1824 | << 4 << std::endl; |
1825 | std::cout << D2EDX2(ii,jj, 4) << " D.F = " << MDiff(ii,jj) << std::endl; |
7fd59977 |
1826 | } |
1827 | } |
1828 | } |
1829 | #endif |
1830 | */ |
1831 | |
81bba717 |
1832 | // ns1, ns2, np are copied locally to avoid crashing the fields |
7fd59977 |
1833 | ns1 = nsurf1; |
1834 | ns2 = nsurf2; |
1835 | np = nplan; |
1836 | dnp = dnplan; |
1837 | d2np = d2nplan; |
1838 | rayprim = dray; |
1839 | raysecn = d2ray; |
1840 | |
1841 | if ( ! pts1.IsEqual(pts2, 1.e-4)) { |
81bba717 |
1842 | math_Gauss Resol(DEDX, 1.e-9); // Tolerance to precise |
1843 | // Calculation of derived Normal Processing |
7fd59977 |
1844 | if (Resol.IsDone()) { |
1845 | Resol.Solve(-DEDT, sol); |
1846 | D2EDX2.Multiply(sol, D2DXdSdt); |
1847 | secmember = - (D2EDT2 + (2*D2EDXDT+D2DXdSdt)*sol); |
1848 | Resol.Solve(secmember); |
1849 | istgt = Standard_False; |
1850 | } |
1851 | } |
1852 | |
1853 | if (istgt) { |
1854 | math_SVD SingRS (DEDX); |
1855 | math_Vector Vbis(1,4); |
1856 | if (SingRS.IsDone()) { |
1857 | SingRS.Solve(-DEDT, sol, 1.e-6); |
1858 | D2EDX2.Multiply(sol, D2DXdSdt); |
1859 | Vbis = - (D2EDT2 + (2*D2EDXDT+D2DXdSdt)*sol); |
1860 | SingRS.Solve( Vbis, secmember, 1.e-6); |
1861 | istgt = Standard_False; |
1862 | } |
1863 | } |
1864 | |
1865 | if (!istgt) { |
1866 | tg1.SetLinearForm(sol(1),d1u1, sol(2),d1v1); |
1867 | tg2.SetLinearForm(sol(3),d1u2, sol(4),d1v2); |
1868 | |
1869 | dnorm1w.SetLinearForm(sol(1),dndu1, sol(2),dndv1, dn1w); |
1870 | dnorm2w.SetLinearForm(sol(3),dndu2, sol(4),dndv2, dn2w); |
1871 | temp.SetLinearForm(sol(1)*sol(1), d2u1, |
1872 | 2*sol(1)*sol(2), d2uv1, |
1873 | sol(2)*sol(2), d2v1); |
1874 | |
1875 | dtg1.SetLinearForm(secmember(1),d1u1, secmember(2),d1v1, temp); |
1876 | |
1877 | temp.SetLinearForm(sol(3)*sol(3), d2u2, |
1878 | 2*sol(3)*sol(4), d2uv2, |
1879 | sol(4)*sol(4), d2v2); |
1880 | dtg2.SetLinearForm(secmember(3),d1u2, secmember(4),d1v2, temp); |
1881 | |
1882 | temp.SetLinearForm(sol(1)*sol(1), d2ndu1, |
1883 | 2*sol(1)*sol(2), d2nduv1, |
1884 | sol(2)*sol(2), d2ndv1); |
1885 | |
1886 | tempbis.SetLinearForm(2*sol(1), d2ndtu1, |
1887 | 2*sol(2), d2ndtv1, |
1888 | d2n1w); |
1889 | temp+= tempbis; |
1890 | d2norm1w.SetLinearForm(secmember(1),dndu1, secmember(2),dndv1, temp); |
1891 | |
1892 | |
1893 | temp.SetLinearForm(sol(3)*sol(3), d2ndu2, |
1894 | 2*sol(3)*sol(4), d2nduv2, |
1895 | sol(4)*sol(4), d2ndv2); |
1896 | tempbis.SetLinearForm(2*sol(3), d2ndtu2, |
1897 | 2*sol(4), d2ndtv2, |
1898 | d2n2w); |
1899 | temp+= tempbis; |
1900 | d2norm2w.SetLinearForm(secmember(3),dndu2, secmember(4),dndv2, temp); |
1901 | } |
1902 | |
81bba717 |
1903 | // Tops 2d |
7fd59977 |
1904 | Poles2d(Poles2d.Lower()).SetCoord(X(1),X(2)); |
1905 | Poles2d(Poles2d.Upper()).SetCoord(X(3),X(4)); |
1906 | if (!istgt) { |
1907 | DPoles2d(Poles2d.Lower()) .SetCoord(sol(1),sol(2)); |
1908 | DPoles2d(Poles2d.Upper()) .SetCoord(sol(3),sol(4)); |
1909 | D2Poles2d(Poles2d.Lower()).SetCoord(secmember(1), secmember(2)); |
1910 | D2Poles2d(Poles2d.Upper()).SetCoord(secmember(3), secmember(4)); |
1911 | } |
1912 | |
81bba717 |
1913 | // the linear is processed... |
7fd59977 |
1914 | if (mySShape == BlendFunc_Linear) { |
1915 | Poles(low) = pts1; |
1916 | Poles(upp) = pts2; |
1917 | Weights(low) = 1.0; |
1918 | Weights(upp) = 1.0; |
1919 | if (!istgt) { |
1920 | DPoles(low) = tg1; |
1921 | DPoles(upp) = tg2; |
1922 | DPoles(low) = dtg1; |
1923 | DPoles(upp) = dtg2; |
1924 | DWeights(low) = 0.0; |
1925 | DWeights(upp) = 0.0; |
1926 | D2Weights(low) = 0.0; |
1927 | D2Weights(upp) = 0.0; |
1928 | } |
1929 | return (!istgt); |
1930 | } |
1931 | |
81bba717 |
1932 | // Case of the circle |
7fd59977 |
1933 | norm1 = nplan.Crossed(ns1).Magnitude(); |
1934 | norm2 = nplan.Crossed(ns2).Magnitude(); |
1935 | if (norm1 < Eps) { |
81bba717 |
1936 | norm1 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1937 | #ifdef OCCT_DEBUG |
04232180 |
1938 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1939 | #endif |
1940 | } |
1941 | if (norm2 < Eps) { |
81bba717 |
1942 | norm2 = 1; // Unsatisfactory, but it is not necessary to stop |
0797d9d3 |
1943 | #ifdef OCCT_DEBUG |
04232180 |
1944 | std::cout << " EvolRad : Surface singuliere " << std::endl; |
7fd59977 |
1945 | #endif |
1946 | } |
1947 | |
1948 | ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1, ns1); |
1949 | ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2, ns2); |
1950 | |
1951 | Center.SetXYZ(pts1.XYZ()+sg1*ray*ns1.XYZ()); |
1952 | if (!istgt) { |
1953 | tgc.SetLinearForm(sg1*ray, dnorm1w, |
1954 | sg1*dray, ns1, |
1955 | tg1); |
1956 | dtgc.SetLinearForm(sg1*ray, d2norm1w, |
1957 | 2*sg1*dray, dnorm1w, |
1958 | sg1*d2ray, ns1); |
1959 | dtgc += dtg1; |
1960 | } |
1961 | |
81bba717 |
1962 | // ns1 is oriented from the center to pts1, and ns2 from the center to pts2 |
1963 | // and the trihedron ns1,ns2,nplan is made direct |
7fd59977 |
1964 | |
1965 | if (sg1 > 0.) { |
1966 | ns1.Reverse(); |
1967 | if (!istgt) { |
1968 | dnorm1w.Reverse(); |
1969 | d2norm1w.Reverse(); |
1970 | } |
1971 | } |
1972 | if (sg2 >0.) { |
1973 | ns2.Reverse(); |
1974 | if (!istgt) { |
1975 | dnorm2w.Reverse(); |
1976 | d2norm2w.Reverse(); |
1977 | } |
1978 | } |
1979 | if (choix%2 != 0) { |
1980 | np.Reverse(); |
1981 | dnp.Reverse(); |
1982 | d2np.Reverse(); |
1983 | } |
1984 | |
81bba717 |
1985 | if (ray < 0.) { // to avoid Abs(dray) several lines below |
7fd59977 |
1986 | rayprim = -rayprim; |
1987 | raysecn = -raysecn; |
1988 | } |
1989 | |
1990 | if (!istgt) { |
1991 | return GeomFill::GetCircle(myTConv, |
1992 | ns1, ns2, |
1993 | dnorm1w, dnorm2w, |
1994 | d2norm1w, d2norm2w, |
1995 | np, dnp, d2np, |
1996 | pts1, pts2, |
1997 | tg1, tg2, |
1998 | dtg1, dtg2, |
1999 | Abs(ray), rayprim, raysecn, |
2000 | Center, tgc, dtgc, |
2001 | Poles, |
2002 | DPoles, |
2003 | D2Poles, |
2004 | Weights, |
2005 | DWeights, |
2006 | D2Weights); |
2007 | } |
2008 | else { |
2009 | GeomFill::GetCircle(myTConv, |
2010 | ns1, ns2, |
2011 | nplan, pts1, pts2, |
2012 | Abs(ray), Center, |
2013 | Poles, Weights); |
2014 | return Standard_False; |
2015 | } |
2016 | } |
2017 | |
2018 | void BlendFunc_EvolRad::Resolution(const Standard_Integer IC2d, |
2019 | const Standard_Real Tol, |
2020 | Standard_Real& TolU, |
2021 | Standard_Real& TolV) const |
2022 | { |
2023 | if(IC2d == 1){ |
2024 | TolU = surf1->UResolution(Tol); |
2025 | TolV = surf1->VResolution(Tol); |
2026 | } |
2027 | else { |
2028 | TolU = surf2->UResolution(Tol); |
2029 | TolV = surf2->VResolution(Tol); |
2030 | } |
2031 | } |