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