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