b311480e |
1 | // Created on: 1993-12-21 |
2 | // Created by: Jacques GOUSSARD |
3 | // Copyright (c) 1993-1999 Matra Datavision |
973c2be1 |
4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
5 | // |
973c2be1 |
6 | // This file is part of Open CASCADE Technology software library. |
b311480e |
7 | // |
d5f74e42 |
8 | // This library is free software; you can redistribute it and/or modify it under |
9 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
10 | // by the Free Software Foundation, with special exception defined in the file |
11 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
12 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
13 | // |
973c2be1 |
14 | // Alternatively, this file may be used under the terms of Open CASCADE |
15 | // commercial license or contractual agreement. |
7fd59977 |
16 | |
7fd59977 |
17 | |
42cf5bc1 |
18 | #include <Adaptor2d_HCurve2d.hxx> |
19 | #include <Adaptor3d_HCurve.hxx> |
20 | #include <Adaptor3d_HSurface.hxx> |
7fd59977 |
21 | #include <BlendFunc.hxx> |
42cf5bc1 |
22 | #include <BlendFunc_EvolRadInv.hxx> |
23 | #include <Law_Function.hxx> |
24 | #include <math_Matrix.hxx> |
25 | #include <Precision.hxx> |
26 | |
7fd59977 |
27 | #define Eps 1.e-15 |
28 | |
29 | |
30 | BlendFunc_EvolRadInv::BlendFunc_EvolRadInv(const Handle(Adaptor3d_HSurface)& S1, |
31 | const Handle(Adaptor3d_HSurface)& S2, |
32 | const Handle(Adaptor3d_HCurve)& C, |
33 | const Handle(Law_Function)& Law) : |
34 | surf1(S1),surf2(S2),curv(C) |
35 | { |
36 | fevol = Law; |
37 | } |
38 | |
39 | void BlendFunc_EvolRadInv::Set(const Standard_Integer Choix) |
40 | |
41 | { |
42 | choix = Choix; |
43 | switch (choix) { |
44 | case 1: |
45 | case 2: |
46 | { |
47 | sg1 = -1.; |
48 | sg2 = -1.; |
49 | } |
50 | break; |
51 | case 3: |
52 | case 4: |
53 | { |
54 | sg1 = 1.; |
55 | sg2 = -1.; |
56 | } |
57 | break; |
58 | case 5: |
59 | case 6: |
60 | { |
61 | sg1 = 1.; |
62 | sg2 = 1.; |
63 | } |
64 | break; |
65 | case 7: |
66 | case 8: |
67 | { |
68 | sg1 = -1.; |
69 | sg2 = 1.; |
70 | } |
71 | break; |
72 | default: |
73 | sg1 = sg2 = -1.; |
74 | } |
75 | } |
76 | |
77 | void BlendFunc_EvolRadInv::Set(const Standard_Boolean OnFirst, |
78 | const Handle(Adaptor2d_HCurve2d)& C) |
79 | { |
80 | first = OnFirst; |
81 | csurf = C; |
82 | } |
83 | |
84 | Standard_Integer BlendFunc_EvolRadInv::NbEquations () const |
85 | { |
86 | return 4; |
87 | } |
88 | |
89 | |
90 | void BlendFunc_EvolRadInv::GetTolerance(math_Vector& Tolerance, |
91 | const Standard_Real Tol) const |
92 | { |
93 | Tolerance(1) = csurf->Resolution(Tol); |
94 | Tolerance(2) = curv->Resolution(Tol); |
95 | if (first) { |
96 | Tolerance(3) = surf2->UResolution(Tol); |
97 | Tolerance(4) = surf2->VResolution(Tol); |
98 | } |
99 | else { |
100 | Tolerance(3) = surf1->UResolution(Tol); |
101 | Tolerance(4) = surf1->VResolution(Tol); |
102 | } |
103 | } |
104 | |
105 | |
106 | void BlendFunc_EvolRadInv::GetBounds(math_Vector& InfBound, |
107 | math_Vector& SupBound) const |
108 | { |
109 | InfBound(1) = csurf->FirstParameter(); |
110 | InfBound(2) = curv->FirstParameter(); |
111 | SupBound(1) = csurf->LastParameter(); |
112 | SupBound(2) = curv->LastParameter(); |
113 | |
114 | if (first) { |
115 | InfBound(3) = surf2->FirstUParameter(); |
116 | InfBound(4) = surf2->FirstVParameter(); |
117 | SupBound(3) = surf2->LastUParameter(); |
118 | SupBound(4) = surf2->LastVParameter(); |
119 | if(!Precision::IsInfinite(InfBound(3)) && |
120 | !Precision::IsInfinite(SupBound(3))) { |
121 | const Standard_Real range = (SupBound(3) - InfBound(3)); |
122 | InfBound(3) -= range; |
123 | SupBound(3) += range; |
124 | } |
125 | if(!Precision::IsInfinite(InfBound(4)) && |
126 | !Precision::IsInfinite(SupBound(4))) { |
127 | const Standard_Real range = (SupBound(4) - InfBound(4)); |
128 | InfBound(4) -= range; |
129 | SupBound(4) += range; |
130 | } |
131 | } |
132 | else { |
133 | InfBound(3) = surf1->FirstUParameter(); |
134 | InfBound(4) = surf1->FirstVParameter(); |
135 | SupBound(3) = surf1->LastUParameter(); |
136 | SupBound(4) = surf1->LastVParameter(); |
137 | if(!Precision::IsInfinite(InfBound(3)) && |
138 | !Precision::IsInfinite(SupBound(3))) { |
139 | const Standard_Real range = (SupBound(3) - InfBound(3)); |
140 | InfBound(3) -= range; |
141 | SupBound(3) += range; |
142 | } |
143 | if(!Precision::IsInfinite(InfBound(4)) && |
144 | !Precision::IsInfinite(SupBound(4))) { |
145 | const Standard_Real range = (SupBound(4) - InfBound(4)); |
146 | InfBound(4) -= range; |
147 | SupBound(4) += range; |
148 | } |
149 | } |
150 | } |
151 | |
152 | Standard_Boolean BlendFunc_EvolRadInv::IsSolution(const math_Vector& Sol, |
153 | const Standard_Real Tol) |
154 | { |
155 | math_Vector valsol(1,4); |
156 | Value(Sol,valsol); |
157 | if (Abs(valsol(1)) <= Tol && |
158 | (valsol(2)*valsol(2) + valsol(3)*valsol(3) + valsol(4)*valsol(4)) <= Tol*Tol) |
159 | return Standard_True; |
160 | |
161 | return Standard_False; |
162 | } |
163 | |
164 | |
165 | Standard_Boolean BlendFunc_EvolRadInv::Value(const math_Vector& X, |
166 | math_Vector& F) |
167 | { |
168 | const Standard_Real ray = fevol->Value(X(2)); |
169 | |
170 | gp_Pnt ptcur; |
171 | gp_Vec d1cur; |
172 | curv->D1(X(2),ptcur,d1cur); |
173 | |
174 | const gp_Vec nplan = d1cur.Normalized(); |
175 | const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ())); |
176 | |
177 | const gp_Pnt2d pt2d(csurf->Value(X(1))); |
178 | |
179 | gp_Pnt pts1,pts2; |
180 | gp_Vec d1u1,d1v1,d1u2,d1v2; |
181 | if (first) |
182 | { |
183 | surf1->D1(pt2d.X(),pt2d.Y(),pts1,d1u1,d1v1); |
184 | surf2->D1(X(3),X(4),pts2,d1u2,d1v2); |
185 | } |
186 | else |
187 | { |
188 | surf1->D1(X(3),X(4),pts1,d1u1,d1v1); |
189 | surf2->D1(pt2d.X(),pt2d.Y(),pts2,d1u2,d1v2); |
190 | } |
191 | |
192 | F(1) = (nplan.X() * (pts1.X() + pts2.X()) + |
193 | nplan.Y() * (pts1.Y() + pts2.Y()) + |
194 | nplan.Z() * (pts1.Z() + pts2.Z())) /2. + theD; |
195 | |
196 | gp_Vec ns1 = d1u1.Crossed(d1v1); |
197 | if (ns1.Magnitude() < Eps) { |
198 | if (first) BlendFunc::ComputeNormal(surf1, pt2d, ns1); |
199 | else { |
200 | gp_Pnt2d P(X(3), X(4)); |
201 | BlendFunc::ComputeNormal(surf1, P, ns1); |
202 | } |
203 | } |
204 | |
205 | gp_Vec ns2 = d1u2.Crossed(d1v2).XYZ(); |
206 | if (ns2.Magnitude() < Eps) { |
207 | if (!first) BlendFunc::ComputeNormal(surf2, pt2d, ns2); |
208 | else { |
209 | gp_Pnt2d P(X(3), X(4)); |
210 | BlendFunc::ComputeNormal(surf2, P, ns2); |
211 | } |
212 | } |
213 | |
214 | const gp_Vec ncrossns1 = nplan.Crossed(ns1); |
215 | const gp_Vec ncrossns2 = nplan.Crossed(ns2); |
216 | Standard_Real norm1 = ncrossns1.Magnitude(); |
217 | Standard_Real norm2 = ncrossns2.Magnitude(); |
218 | |
219 | if (norm1 < Eps) { |
220 | norm1 = 1.; |
7fd59977 |
221 | } |
222 | if (norm2 < Eps) { |
223 | norm2 = 1.; |
7fd59977 |
224 | } |
225 | |
226 | gp_Vec resul; |
227 | const Standard_Real ndotns1 = nplan.Dot(ns1); |
228 | const Standard_Real ndotns2 = nplan.Dot(ns2); |
229 | ns1.SetLinearForm(ndotns1/norm1,nplan, -1./norm1,ns1); |
230 | ns2.SetLinearForm(ndotns2/norm2,nplan, -1./norm2,ns2); |
231 | resul.SetLinearForm(sg1*ray,ns1,-1.,pts2.XYZ(),-sg2*ray,ns2,pts1.XYZ()); |
232 | F(2) = resul.X(); |
233 | F(3) = resul.Y(); |
234 | F(4) = resul.Z(); |
235 | |
236 | return Standard_True; |
237 | } |
238 | |
239 | Standard_Boolean BlendFunc_EvolRadInv::Derivatives(const math_Vector& X, |
240 | math_Matrix& D) |
241 | { |
242 | math_Vector F(1, 4); |
243 | return Values (X, F, D); |
244 | } |
245 | |
246 | Standard_Boolean BlendFunc_EvolRadInv::Values(const math_Vector& X, |
247 | math_Vector& F, |
248 | math_Matrix& D) |
249 | { |
250 | Standard_Real ray,dray; |
251 | fevol->D1(X(2),ray,dray); |
252 | |
253 | gp_Pnt ptcur; |
254 | gp_Vec d1cur,d2cur; |
255 | curv->D2(X(2),ptcur,d1cur,d2cur); |
256 | |
257 | const Standard_Real normtgcur = d1cur.Magnitude(); |
258 | const gp_Vec nplan = d1cur.Normalized(); |
259 | const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ())); |
260 | |
261 | gp_Vec dnplan; |
262 | dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur); |
263 | dnplan /= normtgcur; |
264 | |
265 | gp_Pnt2d p2d; |
266 | gp_Vec2d v2d; |
267 | csurf->D1(X(1),p2d,v2d); |
268 | |
269 | gp_Pnt pts1,pts2; |
270 | gp_Vec d1u1,d1v1,d1u2,d1v2; |
271 | gp_Vec d2u1,d2v1,d2u2,d2v2,d2uv1,d2uv2; |
272 | gp_Vec temp; |
273 | if (first) |
274 | { |
275 | surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1); |
276 | surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2); |
277 | |
278 | temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1); |
279 | D(1,1) = nplan.Dot(temp)/2.; |
280 | temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ()); |
281 | D(1,2) = dnplan.Dot(temp) - normtgcur; |
282 | D(1,3) = nplan.Dot(d1u2)/2.; |
283 | D(1,4) = nplan.Dot(d1v2)/2.; |
284 | } |
285 | else |
286 | { |
287 | surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1); |
288 | surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2); |
289 | |
290 | temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2); |
291 | D(1,1) = nplan.Dot(temp)/2.; |
292 | temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ()); |
293 | D(1,2) = dnplan.Dot(temp) - normtgcur; |
294 | D(1,3) = nplan.Dot(d1u1)/2.; |
295 | D(1,4) = nplan.Dot(d1v1)/2.; |
296 | } |
297 | |
298 | F(1) = (nplan.X()* (pts1.X() + pts2.X()) + |
299 | nplan.Y()* (pts1.Y() + pts2.Y()) + |
300 | nplan.Z()* (pts1.Z() + pts2.Z())) /2. + theD; |
301 | |
302 | gp_Vec ns1 = d1u1.Crossed(d1v1); |
303 | if (ns1.Magnitude() < Eps) { |
304 | if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1); |
305 | else { |
306 | gp_Pnt2d P(X(3), X(4)); |
307 | BlendFunc::ComputeNormal(surf1, P, ns1); |
308 | } |
309 | } |
310 | |
311 | gp_Vec ns2 = d1u2.Crossed(d1v2); |
312 | if (ns2.Magnitude() < Eps) { |
313 | if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2); |
314 | else { |
315 | gp_Pnt2d P(X(3), X(4)); |
316 | BlendFunc::ComputeNormal(surf2, P, ns2); |
317 | } |
318 | } |
319 | |
320 | const gp_Vec ncrossns1 = nplan.Crossed(ns1); |
321 | const gp_Vec ncrossns2 = nplan.Crossed(ns2); |
322 | Standard_Real norm1 = ncrossns1.Magnitude(); |
323 | Standard_Real norm2 = ncrossns2.Magnitude(); |
324 | if (norm1 < Eps) { |
325 | norm1 = 1.; |
7fd59977 |
326 | } |
327 | if (norm2 < Eps) { |
328 | norm2 = 1.; |
7fd59977 |
329 | } |
330 | |
331 | gp_Vec resul1,resul2; |
332 | Standard_Real grosterme; |
333 | |
334 | const Standard_Real ndotns1 = nplan.Dot(ns1); |
335 | const Standard_Real ndotns2 = nplan.Dot(ns2); |
336 | temp.SetLinearForm(ndotns1/norm1,nplan, -1./norm1,ns1); |
337 | resul1.SetLinearForm(sg1*ray,temp,gp_Vec(pts2,pts1)); |
338 | temp.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2); |
339 | resul1.Subtract(sg2*ray*temp); |
340 | |
341 | F(2) = resul1.X(); |
342 | F(3) = resul1.Y(); |
343 | F(4) = resul1.Z(); |
344 | |
345 | // Derivee par rapport a u1 |
346 | |
347 | temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1)); |
348 | grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1; |
349 | resul1.SetLinearForm |
350 | (-sg1*ray/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan, |
351 | sg1*ray*grosterme/norm1,ns1, |
352 | -sg1*ray/norm1,temp, |
353 | d1u1); |
354 | |
355 | |
356 | // Derivee par rapport a v1 |
357 | |
358 | temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1)); |
359 | grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1; |
360 | resul2.SetLinearForm |
361 | (-sg1*ray/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan, |
362 | sg1*ray*grosterme/norm1,ns1, |
363 | -sg1*ray/norm1,temp, |
364 | d1v1); |
365 | |
366 | if (first) { |
367 | D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X(); |
368 | D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y(); |
369 | D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z(); |
370 | } |
371 | else { |
372 | D(2,3) = resul1.X(); |
373 | D(3,3) = resul1.Y(); |
374 | D(4,3) = resul1.Z(); |
375 | |
376 | D(2,4) = resul2.X(); |
377 | D(3,4) = resul2.Y(); |
378 | D(4,4) = resul2.Z(); |
379 | } |
380 | |
381 | // derivee par rapport a w (parametre sur ligne guide) |
382 | |
383 | grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1; |
384 | resul1.SetLinearForm(-sg1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan, |
385 | sg1*ndotns1/norm1,dnplan, |
386 | sg1*grosterme/norm1,ns1); |
387 | |
388 | |
389 | grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2; |
390 | resul2.SetLinearForm(sg2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan, |
391 | -sg2*ndotns2/norm2,dnplan, |
392 | -sg2*grosterme/norm2,ns2); |
393 | |
394 | |
395 | D(2,2) = ray*(resul1.X() + resul2.X()); |
396 | D(3,2) = ray*(resul1.Y() + resul2.Y()); |
397 | D(4,2) = ray*(resul1.Z() + resul2.Z()); |
398 | |
399 | temp.SetLinearForm(sg1*ndotns1/norm1 - sg2*ndotns2/norm2,nplan, |
400 | -sg1/norm1,ns1, |
401 | sg2/norm2,ns2); |
402 | |
403 | D(2,2) += dray*temp.X(); |
404 | D(3,2) += dray*temp.Y(); |
405 | D(4,2) += dray*temp.Z(); |
406 | |
407 | |
408 | |
409 | // Derivee par rapport a u2 |
410 | temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2)); |
411 | grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2; |
412 | resul1.SetLinearForm(sg2*ray/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan, |
413 | -sg2*ray*grosterme/norm2,ns2, |
414 | sg2*ray/norm2,temp); |
415 | resul1.Subtract(d1u2); |
416 | |
417 | // Derivee par rapport a v2 |
418 | temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2)); |
419 | grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2; |
420 | resul2.SetLinearForm(sg2*ray/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan, |
421 | -sg2*ray*grosterme/norm2,ns2, |
422 | sg2*ray/norm2,temp); |
423 | resul2.Subtract(d1v2); |
424 | |
425 | if (!first) { |
426 | D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X(); |
427 | D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y(); |
428 | D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z(); |
429 | } |
430 | else { |
431 | D(2,3) = resul1.X(); |
432 | D(3,3) = resul1.Y(); |
433 | D(4,3) = resul1.Z(); |
434 | |
435 | D(2,4) = resul2.X(); |
436 | D(3,4) = resul2.Y(); |
437 | D(4,4) = resul2.Z(); |
438 | } |
439 | |
440 | return Standard_True; |
441 | } |