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