b311480e |
1 | // Created on: 1998-06-04 |
2 | // Created by: Philippe NOUAILLE |
3 | // Copyright (c) 1998-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_ChAsymInv.hxx> |
23 | #include <math_Matrix.hxx> |
7fd59977 |
24 | #include <Precision.hxx> |
25 | |
26 | //======================================================================= |
27 | //function : BlendFunc_ChAsymInv |
28 | //purpose : |
29 | //======================================================================= |
7fd59977 |
30 | BlendFunc_ChAsymInv::BlendFunc_ChAsymInv(const Handle(Adaptor3d_HSurface)& S1, |
31 | const Handle(Adaptor3d_HSurface)& S2, |
32 | const Handle(Adaptor3d_HCurve)& C) : |
d533dafb |
33 | surf1(S1),surf2(S2), |
34 | dist1(RealLast()), |
35 | angle(RealLast()), |
36 | tgang(RealLast()), |
37 | curv(C), choix(0), |
38 | first(Standard_False), |
7fd59977 |
39 | FX(1, 4), |
40 | DX(1, 4, 1, 4) |
41 | { |
42 | } |
43 | |
44 | |
45 | //======================================================================= |
46 | //function : Set |
47 | //purpose : |
48 | //======================================================================= |
49 | |
50 | void BlendFunc_ChAsymInv::Set(const Standard_Real Dist1, |
51 | const Standard_Real Angle, |
52 | const Standard_Integer Choix) |
53 | { |
54 | dist1 = Abs(Dist1); |
55 | angle = Angle; |
56 | tgang = Tan(Angle); |
57 | choix = Choix; |
58 | } |
59 | |
60 | //======================================================================= |
61 | //function : NbEquations |
62 | //purpose : |
63 | //======================================================================= |
64 | |
65 | Standard_Integer BlendFunc_ChAsymInv::NbEquations () const |
66 | { |
67 | return 4; |
68 | } |
69 | |
70 | //======================================================================= |
71 | //function : GetTolerance |
72 | //purpose : |
73 | //======================================================================= |
74 | |
75 | void BlendFunc_ChAsymInv::Set(const Standard_Boolean OnFirst, |
76 | const Handle(Adaptor2d_HCurve2d)& C) |
77 | { |
78 | first = OnFirst; |
79 | csurf = C; |
80 | } |
81 | |
82 | //======================================================================= |
83 | //function : GetTolerance |
84 | //purpose : |
85 | //======================================================================= |
86 | |
87 | void BlendFunc_ChAsymInv::GetTolerance(math_Vector& Tolerance, const Standard_Real Tol) const |
88 | { |
89 | Tolerance(1) = csurf->Resolution(Tol); |
90 | Tolerance(2) = curv->Resolution(Tol); |
91 | if (first) { |
92 | Tolerance(3) = surf2->UResolution(Tol); |
93 | Tolerance(4) = surf2->VResolution(Tol); |
94 | } |
95 | else { |
96 | Tolerance(3) = surf1->UResolution(Tol); |
97 | Tolerance(4) = surf1->VResolution(Tol); |
98 | } |
99 | } |
100 | |
101 | |
102 | //======================================================================= |
103 | //function : GetBounds |
104 | //purpose : |
105 | //======================================================================= |
106 | |
107 | void BlendFunc_ChAsymInv::GetBounds(math_Vector& InfBound, 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 | //======================================================================= |
153 | //function : IsSolution |
154 | //purpose : |
155 | //======================================================================= |
156 | |
157 | Standard_Boolean BlendFunc_ChAsymInv::IsSolution(const math_Vector& Sol, |
158 | const Standard_Real Tol) |
159 | { |
160 | math_Vector valsol(1, 4); |
161 | gp_Pnt pts1, pts2, ptgui; |
162 | gp_Vec nplan, d1gui, Nsurf1, tsurf1; |
163 | gp_Vec d1u1, d1v1; |
164 | |
165 | curv->D1(Sol(2), ptgui, d1gui); |
166 | nplan = d1gui.Normalized(); |
167 | |
168 | gp_Pnt2d pt2d(csurf->Value(Sol(1))); |
169 | |
170 | if (first) { |
171 | surf1->D1(pt2d.X(), pt2d.Y(), pts1, d1u1, d1v1); |
172 | pts2 = surf2->Value(Sol(3), Sol(4)); |
173 | } |
174 | else { |
175 | surf1->D1(Sol(3), Sol(4), pts1, d1u1, d1v1); |
176 | pts2 = surf2->Value(pt2d.X(), pt2d.Y()); |
177 | } |
178 | |
179 | Nsurf1 = d1u1.Crossed(d1v1); |
180 | tsurf1 = Nsurf1.Crossed(nplan); |
181 | |
182 | gp_Vec s1s2(pts1, pts2); |
183 | Standard_Real PScaInv = 1. / tsurf1.Dot(s1s2), temp;// ,F4; |
184 | Standard_Real Nordu1 = d1u1.Magnitude(), |
185 | Nordv1 = d1v1.Magnitude(); |
186 | |
187 | temp = 2. * (Nordu1 + Nordv1) * s1s2.Magnitude() + 2. * Nordu1 * Nordv1; |
188 | |
189 | Value(Sol, valsol); |
190 | |
191 | if (Abs(valsol(1)) < Tol && |
192 | Abs(valsol(2)) < Tol && |
193 | Abs(valsol(3)) < 2. * dist1 * Tol && |
194 | Abs(valsol(4)) < Tol * (1. + tgang) * Abs(PScaInv) * temp) { |
195 | |
196 | return Standard_True; |
197 | } |
198 | |
199 | return Standard_False; |
200 | |
201 | } |
202 | |
203 | |
204 | //======================================================================= |
205 | //function : ComputeValues |
206 | //purpose : |
207 | //======================================================================= |
208 | Standard_Boolean BlendFunc_ChAsymInv::ComputeValues(const math_Vector& X, |
209 | const Standard_Integer DegF, |
210 | const Standard_Integer DegL) |
211 | { |
212 | if (DegF > DegL) return Standard_False; |
213 | |
214 | gp_Vec nplan, dnplan, d1gui, d2gui, d1u1, d1v1, d2u1, d2v1, d2uv1, d1u2, d1v2; |
215 | gp_Vec Nsurf1, tsurf1; |
216 | gp_Pnt pts1, pts2, ptgui; |
217 | Standard_Real PScaInv, F4; |
218 | Standard_Real Normg = 0.; |
219 | gp_Pnt2d pt2d; |
220 | gp_Vec2d v2d; |
221 | |
222 | if ( (DegF == 0) && (DegL == 0) ) { |
223 | curv->D1(X(2), ptgui, d1gui); |
224 | nplan = d1gui.Normalized(); |
225 | |
226 | if (choix%2 != 0) nplan.Reverse(); |
227 | pt2d = csurf->Value(X(1)); |
228 | |
229 | if (first) { |
230 | surf1->D1(pt2d.X(), pt2d.Y(), pts1, d1u1, d1v1); |
231 | pts2 = surf2->Value(X(3), X(4)); |
232 | } |
233 | else { |
234 | surf1->D1(X(3), X(4), pts1, d1u1, d1v1); |
235 | pts2 = surf2->Value(pt2d.X(), pt2d.Y()); |
236 | } |
237 | } |
238 | else { |
239 | curv->D2(X(2), ptgui, d1gui, d2gui); |
240 | nplan = d1gui.Normalized(); |
241 | Normg = d1gui.Magnitude(); |
242 | dnplan = (d2gui - nplan.Dot(d2gui) * nplan) / Normg; |
243 | |
244 | if (choix%2 != 0) { |
245 | nplan.Reverse(); |
246 | dnplan.Reverse(); |
247 | Normg = - Normg; |
248 | } |
249 | |
250 | csurf->D1(X(1), pt2d, v2d); |
251 | |
252 | if (first) { |
253 | surf1->D2(pt2d.X(), pt2d.Y(), pts1, d1u1, d1v1, d2u1, d2v1, d2uv1); |
254 | surf2->D1(X(3), X(4), pts2, d1u2, d1v2); |
255 | } |
256 | else { |
257 | surf1->D2(X(3), X(4), pts1, d1u1, d1v1, d2u1, d2v1, d2uv1); |
258 | surf2->D1(pt2d.X(), pt2d.Y(), pts2, d1u2, d1v2); |
259 | } |
260 | } |
261 | |
262 | gp_Vec nps1(ptgui, pts1), s1s2(pts1, pts2); |
263 | Nsurf1 = d1u1.Crossed(d1v1); |
264 | tsurf1 = Nsurf1.Crossed(nplan); |
265 | PScaInv = 1. / s1s2.Dot(tsurf1); |
266 | F4 = nplan.Dot(tsurf1.Crossed(s1s2)) * PScaInv; |
267 | |
268 | if (DegF == 0) { |
269 | Standard_Real Dist; |
270 | Dist = ptgui.XYZ().Dot(nplan.XYZ()); |
271 | FX(1) = pts1.XYZ().Dot(nplan.XYZ()) - Dist; |
272 | FX(2) = pts2.XYZ().Dot(nplan.XYZ()) - Dist; |
273 | FX(3) = dist1 * dist1 - nps1.SquareMagnitude(); |
274 | FX(4) = tgang - F4; |
275 | |
276 | } |
277 | |
278 | if (DegL == 1) { |
279 | gp_Vec dwtsurf1, tempVec; |
280 | Standard_Real temp; |
281 | gp_Vec nps2(ptgui, pts2); |
282 | |
283 | if (first) { |
284 | gp_Vec dw1du1, dw1dv1, dw1csurf, dw1pts1; |
285 | dw1pts1 = v2d.X() * d1u1 + v2d.Y() * d1v1; |
286 | dw1du1 = v2d.X() * d2u1 + v2d.Y() * d2uv1; |
287 | dw1dv1 = v2d.X() * d2uv1 + v2d.Y() * d2v1; |
288 | dw1csurf = (dw1du1.Crossed(d1v1) + d1u1.Crossed(dw1dv1)).Crossed(nplan); |
289 | dwtsurf1 = Nsurf1.Crossed(dnplan); |
290 | |
291 | DX(1, 1) = nplan.Dot(dw1pts1); |
292 | DX(1, 2) = dnplan.Dot(nps1) - Normg; |
293 | DX(1, 3) = 0.; |
294 | DX(1, 4) = 0.; |
295 | |
296 | DX(2, 1) = 0.; |
297 | DX(2, 2) = dnplan.Dot(nps2) - Normg; |
298 | DX(2, 3) = nplan.Dot(d1u2); |
299 | DX(2, 4) = nplan.Dot(d1v2); |
300 | |
301 | tempVec = 2. * nps1; |
302 | DX(3, 1) = -dw1pts1.Dot(tempVec); |
303 | DX(3, 2) = d1gui.Dot(tempVec); |
304 | DX(3, 3) = 0.; |
305 | DX(3, 4) = 0.; |
306 | |
307 | temp = F4 * (dw1csurf.Dot(s1s2) - tsurf1.Dot(dw1pts1)); |
308 | temp += nplan.Dot(tsurf1.Crossed(dw1pts1) - dw1csurf.Crossed(s1s2)); |
309 | DX(4, 1) = PScaInv * temp; |
310 | |
311 | temp = F4 * dwtsurf1.Dot(s1s2); |
312 | temp -= dnplan.Dot(tempVec) + nplan.Dot(dwtsurf1.Crossed(s1s2)); |
313 | DX(4, 2) = PScaInv * temp; |
314 | temp = F4 * tsurf1.Dot(d1u2) - nplan.Dot(tsurf1.Crossed(d1u2)); |
315 | DX(4, 3) = PScaInv * temp; |
316 | |
317 | temp = F4 * tsurf1.Dot(d1v2) - nplan.Dot(tsurf1.Crossed(d1v2)); |
318 | DX(4, 4) = PScaInv * temp; |
319 | } |
320 | else { |
321 | gp_Vec d1utsurf1, d1vtsurf1, dw2pts2; |
322 | d1utsurf1 = (d2u1.Crossed(d1v1) + d1u1.Crossed(d2uv1)).Crossed(nplan); |
323 | d1vtsurf1 = (d2uv1.Crossed(d1v1) + d1u1.Crossed(d2v1)).Crossed(nplan); |
324 | dw2pts2 = v2d.X() * d1u2 + v2d.Y() * d1v2; |
325 | dwtsurf1 = Nsurf1.Crossed(dnplan); |
326 | |
327 | DX(1, 1) = 0.; |
328 | DX(1, 2) = dnplan.Dot(nps1) - Normg; |
329 | DX(1, 3) = nplan.Dot(d1u1); |
330 | DX(1, 4) = nplan.Dot(d1v1); |
331 | |
332 | DX(2, 1) = nplan.Dot(dw2pts2); |
333 | DX(2, 2) = dnplan.Dot(nps2) - Normg; |
334 | DX(2, 3) = 0.; |
335 | DX(2, 4) = 0.; |
336 | |
337 | tempVec = 2. * nps1; |
338 | DX(3, 1) = 0.; |
339 | DX(3, 2) = d1gui.Dot(tempVec); |
340 | |
341 | tempVec.Reverse(); |
342 | DX(3, 3) = d1u1.Dot(tempVec); |
343 | DX(3, 4) = d1v1.Dot(tempVec); |
344 | |
345 | temp = F4 * tsurf1.Dot(dw2pts2) - nplan.Dot(tsurf1.Crossed(dw2pts2)); |
346 | DX(4, 1) = PScaInv * temp; |
347 | |
348 | temp = F4 * dwtsurf1.Dot(s1s2); |
349 | temp -= dnplan.Dot(tempVec) + nplan.Dot(dwtsurf1.Crossed(s1s2)); |
350 | DX(4, 2) = PScaInv * temp; |
351 | |
352 | temp = F4 * (d1utsurf1.Dot(s1s2) - tsurf1.Dot(d1u1)); |
353 | temp += nplan.Dot(tsurf1.Crossed(d1u1) - d1utsurf1.Crossed(s1s2)); |
354 | DX(4, 3) = PScaInv * temp; |
355 | |
356 | temp = F4 * (d1vtsurf1.Dot(s1s2) - tsurf1.Dot(d1v1)); |
357 | temp += nplan.Dot(tsurf1.Crossed(d1v1) - d1vtsurf1.Crossed(s1s2)); |
358 | DX(4, 4) = PScaInv * temp; |
359 | } |
360 | } |
361 | |
362 | return Standard_True; |
363 | } |
364 | |
365 | |
366 | //======================================================================= |
367 | //function : Value |
368 | //purpose : |
369 | //======================================================================= |
370 | |
371 | Standard_Boolean BlendFunc_ChAsymInv::Value(const math_Vector& X, math_Vector& F) |
372 | { |
373 | const Standard_Boolean Error = ComputeValues(X, 0, 0); |
374 | F = FX; |
375 | return Error; |
376 | |
377 | } |
378 | |
379 | //======================================================================= |
380 | //function : Derivatives |
381 | //purpose : |
382 | //======================================================================= |
383 | |
384 | Standard_Boolean BlendFunc_ChAsymInv::Derivatives(const math_Vector& X, math_Matrix& D) |
385 | { |
386 | const Standard_Boolean Error = ComputeValues(X, 1, 1); |
387 | D = DX; |
388 | return Error; |
389 | } |
390 | |
391 | //======================================================================= |
392 | //function : Values |
393 | //purpose : |
394 | //======================================================================= |
395 | |
396 | Standard_Boolean BlendFunc_ChAsymInv::Values(const math_Vector& X, |
397 | math_Vector& F, |
398 | math_Matrix& D) |
399 | { |
400 | const Standard_Boolean Error = ComputeValues(X, 0, 1); |
401 | F = FX; |
402 | D = DX; |
403 | return Error; |
04232180 |
404 | /* std::cout<<std::endl; |
405 | std::cout<<" test ChAsymInv"<<std::endl; |
406 | std::cout<<"calcul exact <--> approche"<<std::endl; |
7fd59977 |
407 | |
408 | math_Vector X1(1,4); |
409 | math_Vector F1(1,4); |
410 | X1 = X; X1(1) += 1.e-10; |
411 | Value(X1,F1); |
04232180 |
412 | std::cout<<"D(1,1) : "<<D(1,1)<<" "<<(F1(1) - F(1)) * 1.e10<<std::endl; |
413 | std::cout<<"D(2,1) : "<<D(2,1)<<" "<<(F1(2) - F(2)) * 1.e10<<std::endl; |
414 | std::cout<<"D(3,1) : "<<D(3,1)<<" "<<(F1(3) - F(3)) * 1.e10<<std::endl; |
415 | std::cout<<"D(4,1) : "<<D(4,1)<<" "<<(F1(4) - F(4)) * 1.e10<<std::endl; |
7fd59977 |
416 | X1 = X; X1(2) += 1.e-10; |
417 | Value(X1,F1); |
04232180 |
418 | std::cout<<"D(1,2) : "<<D(1,2)<<" "<<(F1(1) - F(1)) * 1.e10<<std::endl; |
419 | std::cout<<"D(2,2) : "<<D(2,2)<<" "<<(F1(2) - F(2)) * 1.e10<<std::endl; |
420 | std::cout<<"D(3,2) : "<<D(3,2)<<" "<<(F1(3) - F(3)) * 1.e10<<std::endl; |
421 | std::cout<<"D(4,2) : "<<D(4,2)<<" "<<(F1(4) - F(4)) * 1.e10<<std::endl; |
7fd59977 |
422 | X1 = X; X1(3) += 1.e-10; |
423 | Value(X1,F1); |
04232180 |
424 | std::cout<<"D(1,3) : "<<D(1,3)<<" "<<(F1(1) - F(1)) * 1.e10<<std::endl; |
425 | std::cout<<"D(2,3) : "<<D(2,3)<<" "<<(F1(2) - F(2)) * 1.e10<<std::endl; |
426 | std::cout<<"D(3,3) : "<<D(3,3)<<" "<<(F1(3) - F(3)) * 1.e10<<std::endl; |
427 | std::cout<<"D(4,3) : "<<D(4,3)<<" "<<(F1(4) - F(4)) * 1.e10<<std::endl; |
7fd59977 |
428 | X1 = X; X1(4) += 1.e-10; |
429 | Value(X1,F1); |
04232180 |
430 | std::cout<<"D(1,4) : "<<D(1,4)<<" "<<(F1(1) - F(1)) * 1.e10<<std::endl; |
431 | std::cout<<"D(2,4) : "<<D(2,4)<<" "<<(F1(2) - F(2)) * 1.e10<<std::endl; |
432 | std::cout<<"D(3,4) : "<<D(3,4)<<" "<<(F1(3) - F(3)) * 1.e10<<std::endl; |
433 | std::cout<<"D(4,4) : "<<D(4,4)<<" "<<(F1(4) - F(4)) * 1.e10<<std::endl;*/ |
7fd59977 |
434 | } |