b311480e |
1 | // Created on: 1998-03-27 |
2 | // Created by: # Andre LIEUTIER |
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 <gp_Ax1.hxx> |
7fd59977 |
19 | #include <gp_Dir.hxx> |
42cf5bc1 |
20 | #include <gp_Pnt.hxx> |
7fd59977 |
21 | #include <gp_Trsf.hxx> |
42cf5bc1 |
22 | #include <gp_XY.hxx> |
23 | #include <math_Gauss.hxx> |
24 | #include <math_Matrix.hxx> |
25 | #include <math_Vector.hxx> |
26 | #include <Plate_D1.hxx> |
27 | #include <Plate_D2.hxx> |
28 | #include <Plate_D3.hxx> |
29 | #include <Plate_FreeGtoCConstraint.hxx> |
30 | #include <Plate_LinearScalarConstraint.hxx> |
31 | #include <Plate_PinpointConstraint.hxx> |
7fd59977 |
32 | |
33 | static const Standard_Real NORMIN = 1.e-10; |
34 | static const Standard_Real COSMIN = 1.e-2; |
35 | |
36 | |
37 | // G1 Constraints |
38 | |
39 | Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T, |
40 | const Standard_Real IncrementalLoad, const Standard_Integer orientation) |
41 | { |
42 | pnt2d = point2d; |
43 | nb_PPConstraints = 0; |
44 | nb_LSConstraints = 0; |
45 | |
46 | gp_XYZ normale = D1T.Du^D1T.Dv; |
47 | if(normale.Modulus() < NORMIN) return; |
48 | normale.Normalize(); |
49 | |
50 | if(IncrementalLoad!=1.) |
51 | { |
52 | gp_XYZ N0 = D1S.Du^D1S.Dv; |
53 | if(N0.Modulus()< NORMIN) return; |
54 | N0.Normalize(); |
55 | gp_XYZ N1 = normale; |
56 | if(orientation!=0) N1 *= orientation; |
57 | Standard_Real c = N0*N1; |
58 | if(orientation==0) |
59 | { |
60 | if (c <0.) |
61 | { |
62 | c *= -1.; |
63 | N1 *= -1.; |
64 | } |
65 | } |
66 | |
67 | Standard_Real s = N0.CrossMagnitude(N1); |
68 | if((s < 1.e-2)&&(c<0.)) return; |
69 | Standard_Real angle = atan2(c,s); |
c6541a0c |
70 | //if (angle < 0.) angle += M_PI; |
7fd59977 |
71 | |
72 | gp_XYZ d = N0^N1; |
73 | d.Normalize(); |
74 | gp_Dir dir = gp_Dir(d); |
75 | gp_Trsf rota; |
76 | gp_Ax1 Axe(gp_Pnt(0,0,0), dir); |
77 | rota.SetRotation(Axe,angle*(IncrementalLoad-1.)); |
78 | // gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.)); |
79 | rota.Transforms(normale); |
80 | } |
81 | |
82 | gp_XYZ du = D1S.Du*(-1.); |
83 | gp_XYZ dv = D1S.Dv*(-1.); |
84 | |
85 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale); |
86 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale); |
87 | nb_LSConstraints = 2; |
88 | |
89 | } |
90 | |
91 | // G1 + G2 Constraints |
92 | |
93 | Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T0, |
94 | const Plate_D2& D2S,const Plate_D2& D2T0, |
95 | const Standard_Real IncrementalLoad, const Standard_Integer orientation) |
96 | { |
97 | pnt2d = point2d; |
98 | nb_PPConstraints = 0; |
99 | nb_LSConstraints = 0; |
100 | Plate_D1 D1T = D1T0; |
101 | Plate_D2 D2T = D2T0; |
102 | |
103 | gp_XYZ normale = D1T.Du^D1T.Dv; |
104 | if(normale.Modulus() < NORMIN) return; |
105 | normale.Normalize(); |
106 | |
107 | // G1 Constraints |
108 | gp_XYZ normaleS = D1S.Du^D1S.Dv; |
109 | if(normaleS.Modulus() < NORMIN) |
110 | { |
111 | if(IncrementalLoad!=1.) return; |
112 | gp_XYZ du = D1S.Du*(-1.); |
113 | gp_XYZ dv = D1S.Dv*(-1.); |
114 | |
115 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale); |
116 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale); |
117 | nb_LSConstraints = 2; |
118 | return; |
119 | } |
120 | normaleS.Normalize(); |
121 | |
122 | |
123 | if(IncrementalLoad!=1.) |
124 | { |
125 | gp_XYZ N0 = normaleS; |
126 | gp_XYZ N1 = normale; |
127 | if(orientation!=0) N1 *= orientation; |
128 | Standard_Real c = N0*N1; |
129 | if(orientation==0) |
130 | { |
131 | if (c <0.) |
132 | { |
133 | c *= -1.; |
134 | N1 *= -1.; |
135 | } |
136 | } |
137 | |
138 | Standard_Real s = N0.CrossMagnitude(N1); |
139 | if((s < 1.e-2)&&(c<0.)) return; |
140 | Standard_Real angle = atan2(c,s); |
141 | |
142 | gp_XYZ d = N0^N1; |
143 | d.Normalize(); |
144 | gp_Dir dir = gp_Dir(d); |
145 | gp_Trsf rota; |
146 | gp_Ax1 Axe(gp_Pnt(0,0,0), dir); |
147 | rota.SetRotation(Axe,angle*(IncrementalLoad-1.)); |
148 | // gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.)); |
149 | rota.Transforms(normale); |
150 | rota.Transforms(D1T.Du); |
151 | rota.Transforms(D1T.Dv); |
152 | rota.Transforms(D2T.Duu); |
153 | rota.Transforms(D2T.Duv); |
154 | rota.Transforms(D2T.Dvv); |
155 | } |
156 | |
157 | |
158 | Standard_Real cos_normales = normale*normaleS; |
159 | if( fabs(cos_normales)<COSMIN) |
160 | { |
161 | gp_XYZ du = D1S.Du*(-1.); |
162 | gp_XYZ dv = D1S.Dv*(-1.); |
163 | |
164 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale); |
165 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale); |
166 | nb_LSConstraints = 2; |
167 | return; |
168 | } |
169 | |
170 | Standard_Real invcos = 1./cos_normales; |
171 | |
172 | gp_XYZ du = normaleS* -(normale*D1S.Du)*invcos; |
173 | gp_XYZ dv = normaleS* -(normale*D1S.Dv)*invcos; |
174 | |
175 | myPPC[0] = Plate_PinpointConstraint(pnt2d, du,1,0); |
176 | myPPC[1] = Plate_PinpointConstraint(pnt2d, dv,0,1); |
177 | nb_PPConstraints = 2; |
178 | |
179 | // G2 Constraints |
180 | gp_XYZ Su = D1S.Du+du; |
181 | gp_XYZ Sv = D1S.Dv+dv; |
182 | |
183 | math_Matrix mat(0,1,0,1); |
184 | mat(0,0) = Su*D1T.Du; |
185 | mat(0,1) = Su*D1T.Dv; |
186 | mat(1,0) = Sv*D1T.Du; |
187 | mat(1,1) = Sv*D1T.Dv; |
188 | math_Gauss gauss(mat); |
189 | if(!gauss.IsDone()) return; |
190 | |
191 | math_Vector vec(0,1); |
192 | vec(0) = Su*Su; |
193 | vec(1) = Su*Sv; |
194 | math_Vector sol(0,1); |
195 | |
196 | gauss.Solve(vec,sol); |
197 | Standard_Real a = sol(0); |
198 | Standard_Real b = sol(1); |
199 | |
200 | vec(0) = Sv*Su; |
201 | vec(1) = Sv*Sv; |
202 | |
203 | gauss.Solve(vec,sol); |
204 | Standard_Real c = sol(0); |
205 | Standard_Real d = sol(1); |
206 | |
207 | gp_XYZ Suu = D2T.Duu*(a*a) + D2T.Duv*(2*a*b) + D2T.Dvv*(b*b); |
208 | gp_XYZ Suv = D2T.Duu*(a*c) + D2T.Duv*(a*d+b*c) + D2T.Dvv*(b*d); |
209 | gp_XYZ Svv = D2T.Duu*(c*c) + D2T.Duv*(2*c*d) + D2T.Dvv*(d*d); |
210 | |
211 | gp_XYZ duu = Suu-D2S.Duu; |
212 | gp_XYZ duv = Suv-D2S.Duv; |
213 | gp_XYZ dvv = Svv-D2S.Dvv; |
214 | duu *= IncrementalLoad; |
215 | duv *= IncrementalLoad; |
216 | dvv *= IncrementalLoad; |
217 | |
218 | |
219 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duu,2,0), normale); |
220 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duv,1,1), normale); |
221 | myLSC[2] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dvv,0,2), normale); |
222 | nb_LSConstraints = 3; |
223 | } |
224 | |
225 | // G1 + G2 + G3 Constraints |
226 | |
227 | Plate_FreeGtoCConstraint::Plate_FreeGtoCConstraint(const gp_XY& point2d,const Plate_D1& D1S,const Plate_D1& D1T0, |
228 | const Plate_D2& D2S,const Plate_D2& D2T0, |
229 | const Plate_D3& D3S,const Plate_D3& D3T0, |
230 | const Standard_Real IncrementalLoad, const Standard_Integer orientation) |
231 | { |
232 | pnt2d = point2d; |
233 | nb_PPConstraints = 0; |
234 | nb_LSConstraints = 0; |
235 | Plate_D1 D1T = D1T0; |
236 | Plate_D2 D2T = D2T0; |
237 | Plate_D3 D3T = D3T0; |
238 | |
239 | gp_XYZ normale = D1T.Du^D1T.Dv; |
240 | if(normale.Modulus() < NORMIN) return; |
241 | normale.Normalize(); |
242 | |
243 | // G1 Constraints |
244 | gp_XYZ normaleS = D1S.Du^D1S.Dv; |
245 | if(normaleS.Modulus() < NORMIN) |
246 | { |
247 | if(IncrementalLoad!=1.) return; |
248 | gp_XYZ du = D1S.Du*(-1.); |
249 | gp_XYZ dv = D1S.Dv*(-1.); |
250 | |
251 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale); |
252 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale); |
253 | nb_LSConstraints = 2; |
254 | return; |
255 | } |
256 | normaleS.Normalize(); |
257 | |
258 | if(IncrementalLoad!=1.) |
259 | { |
260 | gp_XYZ N0 = normaleS; |
261 | gp_XYZ N1 = normale; |
262 | if(orientation!=0) N1 *= orientation; |
263 | Standard_Real c = N0*N1; |
264 | if(orientation==0) |
265 | { |
266 | if (c <0.) |
267 | { |
268 | c *= -1.; |
269 | N1 *= -1.; |
270 | } |
271 | } |
272 | Standard_Real s = N0.CrossMagnitude(N1); |
273 | if((s < 1.e-2)&&(c<0.)) return; |
274 | Standard_Real angle = atan2(c,s); |
275 | |
276 | gp_XYZ d = N0^N1; |
277 | d.Normalize(); |
278 | gp_Dir dir = gp_Dir(d); |
279 | gp_Trsf rota; |
280 | gp_Ax1 Axe(gp_Pnt(0,0,0), dir); |
281 | rota.SetRotation(Axe,angle*(IncrementalLoad-1.)); |
282 | // gp_Trsf rota = gce_MakeRotation(gp_Pnt(0,0,0), dir, angle*(IncrementalLoad-1.)); |
283 | rota.Transforms(normale); |
284 | rota.Transforms(D1T.Du); |
285 | rota.Transforms(D1T.Dv); |
286 | rota.Transforms(D2T.Duu); |
287 | rota.Transforms(D2T.Duv); |
288 | rota.Transforms(D2T.Dvv); |
289 | rota.Transforms(D3T.Duuu); |
290 | rota.Transforms(D3T.Duuv); |
291 | rota.Transforms(D3T.Duvv); |
292 | rota.Transforms(D3T.Dvvv); |
293 | } |
294 | |
295 | Standard_Real cos_normales = normale*normaleS; |
296 | if( fabs(cos_normales)<COSMIN) |
297 | { |
298 | gp_XYZ du = D1S.Du*(-1.); |
299 | gp_XYZ dv = D1S.Dv*(-1.); |
300 | |
301 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, du,1,0), normale); |
302 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dv,0,1), normale); |
303 | nb_LSConstraints = 2; |
304 | return; |
305 | } |
306 | |
307 | Standard_Real invcos = 1./cos_normales; |
308 | |
309 | gp_XYZ du = normaleS* -(normale*D1S.Du)*invcos; |
310 | gp_XYZ dv = normaleS* -(normale*D1S.Dv)*invcos; |
311 | |
312 | myPPC[0] = Plate_PinpointConstraint(pnt2d, du,1,0); |
313 | myPPC[1] = Plate_PinpointConstraint(pnt2d, dv,0,1); |
314 | nb_PPConstraints = 2; |
315 | |
316 | // G2 Constraints |
317 | gp_XYZ Su = D1S.Du+du; |
318 | gp_XYZ Sv = D1S.Dv+dv; |
319 | |
320 | math_Matrix mat(0,1,0,1); |
321 | mat(0,0) = Su*D1T.Du; |
322 | mat(0,1) = Su*D1T.Dv; |
323 | mat(1,0) = Sv*D1T.Du; |
324 | mat(1,1) = Sv*D1T.Dv; |
325 | math_Gauss gauss(mat); |
326 | if(!gauss.IsDone()) return; |
327 | |
328 | math_Vector vec(0,1); |
329 | vec(0) = Su*Su; |
330 | vec(1) = Su*Sv; |
331 | math_Vector sol(0,1); |
332 | |
333 | gauss.Solve(vec,sol); |
334 | Standard_Real a = sol(0); |
335 | Standard_Real b = sol(1); |
336 | |
337 | vec(0) = Sv*Su; |
338 | vec(1) = Sv*Sv; |
339 | |
340 | gauss.Solve(vec,sol); |
341 | Standard_Real c = sol(0); |
342 | Standard_Real d = sol(1); |
343 | |
344 | gp_XYZ Suu = D2T.Duu*(a*a) + D2T.Duv*(2*a*b) + D2T.Dvv*(b*b); |
345 | gp_XYZ Suv = D2T.Duu*(a*c) + D2T.Duv*(a*d+b*c) + D2T.Dvv*(b*d); |
346 | gp_XYZ Svv = D2T.Duu*(c*c) + D2T.Duv*(2*c*d) + D2T.Dvv*(d*d); |
347 | |
348 | gp_XYZ duu = normaleS * (normale*(Suu-D2S.Duu))*invcos; |
349 | gp_XYZ duv = normaleS * (normale*(Suv-D2S.Duv))*invcos; |
350 | gp_XYZ dvv = normaleS * (normale*(Svv-D2S.Dvv))*invcos; |
351 | |
352 | myPPC[2] = Plate_PinpointConstraint(pnt2d, duu,2,0); |
353 | myPPC[3] = Plate_PinpointConstraint(pnt2d, duv,1,1); |
354 | myPPC[4] = Plate_PinpointConstraint(pnt2d, dvv,0,2); |
355 | nb_PPConstraints = 5; |
356 | |
357 | // G3 Constraints |
358 | |
359 | vec(0) = (D2S.Duu + duu - Suu)*Su; |
360 | vec(1) = (D2S.Duu + duu - Suu)*Sv; |
361 | gauss.Solve(vec,sol); |
362 | Standard_Real B0uu = sol(0); |
363 | Standard_Real B1uu = sol(1); |
364 | |
365 | vec(0) = (D2S.Duv + duv - Suv)*Su; |
366 | vec(1) = (D2S.Duv + duv - Suv)*Sv; |
367 | gauss.Solve(vec,sol); |
368 | Standard_Real B0uv = sol(0); |
369 | Standard_Real B1uv = sol(1); |
370 | |
371 | vec(0) = (D2S.Dvv + dvv - Svv)*Su; |
372 | vec(1) = (D2S.Dvv + dvv - Svv)*Sv; |
373 | gauss.Solve(vec,sol); |
374 | Standard_Real B0vv = sol(0); |
375 | Standard_Real B1vv = sol(1); |
376 | |
377 | gp_XYZ Suuu = D3T.Duuu*(a*a*a) + D3T.Duuv*(3*a*a*b) + D3T.Duvv*(3*a*b*b) + D3T.Dvvv*(b*b*b); |
378 | gp_XYZ Suuv = D3T.Duuu*(a*a*c) + D3T.Duuv*(a*a*d+2*a*b*c) + D3T.Duvv*(b*b*c+2*a*b*d) + D3T.Dvvv*(b*b*d); |
379 | gp_XYZ Suvv = D3T.Duuu*(a*c*c) + D3T.Duuv*(b*c*c+2*a*c*d) + D3T.Duvv*(a*d*d+2*b*c*d) + D3T.Dvvv*(b*d*d); |
380 | gp_XYZ Svvv = D3T.Duuu*(c*c*c) + D3T.Duuv*(3*c*c*d) + D3T.Duvv*(3*c*d*d) + D3T.Dvvv*(d*d*d); |
381 | |
382 | Standard_Real &A0u = a; |
383 | Standard_Real &A1u = b; |
384 | Standard_Real &A0v = c; |
385 | Standard_Real &A1v = d; |
386 | Suuu += D2T.Duu*(3*A0u*B0uu) + D2T.Duv*(3*(A0u*B1uu+A1u*B0uu)) + D2T.Dvv*(3*A1u*B1uu); |
387 | Suuv += D2T.Duu*(2*A0u*B0uv+A0v*B0uu) + D2T.Duv*(2*(A0u*B1uv+A1u*B0uv)+A0v*B1uu+A1v*B0uu) + D2T.Dvv*(2*A1u*B1uv+A1v*B1uu); |
388 | Suvv += D2T.Duu*(A0u*B0vv+2*A0v*B0uv) + D2T.Duv*(2*(A0v*B1uv+A1v*B0uv)+A0u*B1vv+A1u*B0vv) + D2T.Dvv*(2*A1v*B1uv+A1u*B1vv); |
389 | Svvv += D2T.Duu*(3*A0v*B0vv) + D2T.Duv*(3*(A0v*B1vv+A1v*B0vv)) + D2T.Dvv*(3*A1v*B1vv); |
390 | |
391 | |
392 | |
393 | gp_XYZ duuu = Suuu-D3S.Duuu; |
394 | gp_XYZ duuv = Suuv-D3S.Duuv; |
395 | gp_XYZ duvv = Suvv-D3S.Duvv; |
396 | gp_XYZ dvvv = Svvv-D3S.Dvvv; |
397 | duuu *= IncrementalLoad; |
398 | duuv *= IncrementalLoad; |
399 | duvv *= IncrementalLoad; |
400 | dvvv *= IncrementalLoad; |
401 | |
402 | |
403 | myLSC[0] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duuu,3,0), normale); |
404 | myLSC[1] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duuv,2,1), normale); |
405 | myLSC[2] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, duvv,1,2), normale); |
406 | myLSC[3] = Plate_LinearScalarConstraint(Plate_PinpointConstraint(pnt2d, dvvv,0,3), normale); |
407 | nb_LSConstraints = 4; |
408 | } |