b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
6 | // This library is free software; you can redistribute it and/or modify it under |
7 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
8 | // by the Free Software Foundation, with special exception defined in the file |
9 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
10 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | // lpa, le 11/09/91 |
16 | |
17 | |
18 | // Approximation d un ensemble de points contraints (MultiLine) avec une |
19 | // solution approchee (MultiCurve). L algorithme utilise est l algorithme |
20 | // d Uzawa du package mathematique. |
21 | |
22 | #define No_Standard_RangeError |
23 | #define No_Standard_OutOfRange |
24 | |
25 | #include <math_Vector.hxx> |
26 | #include <math_Matrix.hxx> |
27 | #include <AppParCurves_Constraint.hxx> |
28 | #include <AppParCurves_ConstraintCouple.hxx> |
29 | #include <AppParCurves_MultiPoint.hxx> |
30 | #include <AppParCurves.hxx> |
31 | #include <Standard_DimensionError.hxx> |
32 | #include <math_Uzawa.hxx> |
33 | #include <TColStd_Array1OfInteger.hxx> |
34 | #include <TColStd_Array2OfInteger.hxx> |
35 | #include <gp_Pnt.hxx> |
36 | #include <gp_Pnt2d.hxx> |
37 | #include <gp_Vec.hxx> |
38 | #include <gp_Vec2d.hxx> |
39 | #include <TColgp_Array1OfPnt.hxx> |
40 | #include <TColgp_Array1OfPnt2d.hxx> |
41 | #include <TColgp_Array1OfVec.hxx> |
42 | #include <TColgp_Array1OfVec2d.hxx> |
43 | |
44 | |
45 | AppParCurves_ResolConstraint:: |
46 | AppParCurves_ResolConstraint(const MultiLine& SSP, |
47 | AppParCurves_MultiCurve& SCurv, |
48 | const Standard_Integer FirstPoint, |
49 | const Standard_Integer LastPoint, |
50 | const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints, |
51 | const math_Matrix& Bern, |
52 | const math_Matrix& DerivativeBern, |
53 | const Standard_Real Tolerance): |
54 | Cont(1, NbConstraints(SSP, FirstPoint, |
55 | LastPoint, TheConstraints), |
56 | 1, NbColumns(SSP, SCurv.NbPoles()-1), 0.0), |
57 | DeCont(1, NbConstraints(SSP, FirstPoint, |
58 | LastPoint, TheConstraints), |
59 | 1, NbColumns(SSP, SCurv.NbPoles()-1), 0.0), |
60 | Secont(1, NbConstraints(SSP, FirstPoint, |
61 | LastPoint, TheConstraints), 0.0), |
62 | CTCinv(1, NbConstraints(SSP, FirstPoint, |
63 | LastPoint, TheConstraints), |
64 | 1, NbConstraints(SSP, FirstPoint, |
65 | LastPoint, TheConstraints)), |
66 | Vardua(1, NbConstraints(SSP, FirstPoint, |
67 | LastPoint, TheConstraints)), |
68 | IPas(1, LastPoint-FirstPoint+1), |
69 | ITan(1, LastPoint-FirstPoint+1), |
70 | ICurv(1, LastPoint-FirstPoint+1) |
71 | { |
72 | Standard_Integer i, j, k, NbCu= SCurv.NbCurves(); |
7fd59977 |
73 | Standard_Integer Npt; |
1d47d8d0 |
74 | Standard_Integer Inc3, IncSec, IncCol, IP = 0, CCol; |
7fd59977 |
75 | Standard_Integer myindex, Def = SCurv.NbPoles()-1; |
76 | Standard_Integer nb3d, nb2d, Npol= Def+1, Npol2 = 2*Npol; |
1d47d8d0 |
77 | Standard_Real T1 = 0., T2 = 0., T3, Tmax, Daij; |
7fd59977 |
78 | Standard_Boolean Ok; |
79 | gp_Vec V; |
80 | gp_Vec2d V2d; |
81 | gp_Pnt Poi; |
82 | gp_Pnt2d Poi2d; |
83 | AppParCurves_ConstraintCouple mycouple; |
84 | AppParCurves_Constraint FC = AppParCurves_NoConstraint, |
85 | LC = AppParCurves_NoConstraint, Cons; |
86 | |
87 | |
88 | |
89 | // Boucle de calcul du nombre de points de passage afin de dimensionner |
90 | // les matrices. |
91 | IncPass = 0; |
92 | IncTan = 0; |
93 | IncCurv = 0; |
94 | for (i = TheConstraints->Lower(); i <= TheConstraints->Upper(); i++) { |
95 | mycouple = TheConstraints->Value(i); |
96 | myindex = mycouple.Index(); |
97 | Cons = mycouple.Constraint(); |
98 | if (myindex == FirstPoint) FC = Cons; |
99 | if (myindex == LastPoint) LC = Cons; |
100 | if (Cons >= 1) { |
101 | IncPass++; // IncPass = nbre de points de passage. |
102 | IPas(IncPass) = myindex; |
103 | } |
104 | if (Cons >= 2) { |
105 | IncTan++; // IncTan= nbre de points de tangence. |
106 | ITan(IncTan) = myindex; |
107 | } |
108 | if (Cons == 3) { |
109 | IncCurv++; // IncCurv = nbre de pts de courbure. |
110 | ICurv(IncCurv) = myindex; |
111 | } |
112 | } |
113 | |
114 | |
115 | if (IncPass == 0) { |
116 | Done = Standard_True; |
117 | return; |
118 | } |
119 | |
120 | nb3d = ToolLine::NbP3d(SSP); |
121 | nb2d = ToolLine::NbP2d(SSP); |
122 | Standard_Integer mynb3d=nb3d, mynb2d=nb2d; |
123 | if (nb3d == 0) mynb3d = 1; |
124 | if (nb2d == 0) mynb2d = 1; |
125 | CCol = nb3d* 3 + nb2d* 2; |
126 | |
127 | |
128 | // Declaration et initialisation des matrices et vecteurs de contraintes: |
129 | math_Matrix ContInit(1, IncPass, 1, Npol); |
130 | math_Vector Start(1, CCol*Npol); |
131 | TColStd_Array2OfInteger Ibont(1, NbCu, 1, IncTan); |
132 | |
133 | |
134 | // Remplissage de Cont pour les points de passage: |
135 | // ================================================= |
136 | for (i =1; i <= IncPass; i++) { // Cette partie ne depend que de Bernstein |
137 | Npt = IPas(i); |
138 | for (j = 1; j <= Npol; j++) { |
139 | ContInit(i, j) = Bern(Npt, j); |
140 | } |
141 | } |
142 | for (i = 1; i <= CCol; i++) { |
143 | Cont.Set(IncPass*(i-1)+1, IncPass*i, Npol*(i-1)+1, Npol*i, ContInit); |
144 | } |
145 | |
146 | |
147 | // recuperation des vecteurs de depart pour Uzawa. Ce vecteur represente les |
148 | // poles de SCurv. |
149 | // Remplissage de secont et resolution. |
150 | |
151 | TColgp_Array1OfVec tabV(1, mynb3d); |
152 | TColgp_Array1OfVec2d tabV2d(1, mynb2d); |
153 | TColgp_Array1OfPnt tabP(1, mynb3d); |
154 | TColgp_Array1OfPnt2d tabP2d(1, mynb2d); |
155 | |
156 | Inc3 = CCol*IncPass +1; |
157 | IncCol = 0; |
158 | IncSec = 0; |
159 | for (k = 1; k <= NbCu; k++) { |
160 | if (k <= nb3d) { |
161 | for (i = 1; i <= IncTan; i++) { |
162 | Npt = ITan(i); |
163 | // choix du maximum de tangence pour exprimer la colinearite: |
96a95605 |
164 | ToolLine::Tangency(SSP, Npt, tabV); |
7fd59977 |
165 | V = tabV(k); |
166 | V.Coord(T1, T2, T3); |
167 | Tmax = Abs(T1); |
168 | Ibont(k, i) = 1; |
169 | if (Abs(T2) > Tmax) { |
170 | Tmax = Abs(T2); |
171 | Ibont(k, i) = 2; |
172 | } |
173 | if (Abs(T3) > Tmax) { |
174 | Tmax = Abs(T3); |
175 | Ibont(k, i) = 3; |
176 | } |
177 | if (Ibont(k, i) == 3) { |
178 | for (j = 1; j <= Npol; j++) { |
179 | Daij = DerivativeBern(Npt, j); |
180 | Cont(Inc3, j+ Npol + IncCol) = Daij*T3/Tmax; |
181 | Cont(Inc3, j + Npol2 + IncCol) = -Daij *T2/Tmax; |
182 | Cont(Inc3+1, j+ IncCol) = Daij* T3/Tmax; |
183 | Cont(Inc3+1, j+ Npol2 + IncCol) = -Daij*T1/Tmax; |
184 | } |
185 | } |
186 | else if (Ibont(k, i) == 1) { |
187 | for (j = 1; j <= Npol; j++) { |
188 | Daij = DerivativeBern(Npt, j); |
189 | Cont(Inc3, j + IncCol) = Daij*T3/Tmax; |
190 | Cont(Inc3, j + Npol2 + IncCol) = -Daij *T1/Tmax; |
191 | Cont(Inc3+1, j+ IncCol) = Daij* T2/Tmax; |
192 | Cont(Inc3+1, j+ Npol +IncCol) = -Daij*T1/Tmax; |
193 | } |
194 | } |
195 | else if (Ibont(k, i) == 2) { |
196 | for (j = 1; j <= Def+1; j++) { |
197 | Daij = DerivativeBern(Npt, j); |
198 | Cont(Inc3, j+ Npol + IncCol) = Daij*T3/Tmax; |
199 | Cont(Inc3, j + Npol2 + IncCol) = -Daij *T2/Tmax; |
200 | Cont(Inc3+1, j+ IncCol) = Daij* T2/Tmax; |
201 | Cont(Inc3+1, j+ Npol + IncCol) = -Daij*T1/Tmax; |
202 | } |
203 | } |
204 | Inc3 = Inc3 + 2; |
205 | } |
206 | |
207 | // Remplissage du second membre: |
208 | for (i = 1; i <= IncPass; i++) { |
209 | ToolLine::Value(SSP, IPas(i), tabP); |
210 | Poi = tabP(k); |
211 | Secont(i + IncSec) = Poi.X(); |
212 | Secont(i + IncPass + IncSec) = Poi.Y(); |
213 | Secont(i + 2*IncPass + IncSec) = Poi.Z(); |
214 | } |
215 | IncSec = IncSec + 3*IncPass; |
216 | |
217 | // Vecteur de depart: |
218 | for (j =1; j <= Npol; j++) { |
219 | Poi = SCurv.Value(j).Point(k); |
220 | Start(j + IncCol) = Poi.X(); |
221 | Start(j+ Npol + IncCol) = Poi.Y(); |
222 | Start(j + Npol2 + IncCol) = Poi.Z(); |
223 | } |
224 | IncCol = IncCol + 3*Npol; |
225 | } |
226 | |
227 | |
228 | else { |
229 | for (i = 1; i <= IncTan; i++) { |
230 | Npt = ITan(i); |
96a95605 |
231 | ToolLine::Tangency(SSP, Npt, tabV2d); |
7fd59977 |
232 | V2d = tabV2d(k-nb3d); |
233 | T1 = V2d.X(); |
234 | T2 = V2d.Y(); |
235 | Tmax = Abs(T1); |
236 | Ibont(k, i) = 1; |
237 | if (Abs(T2) > Tmax) { |
238 | Tmax = Abs(T2); |
239 | Ibont(k, i) = 2; |
240 | } |
241 | for (j = 1; j <= Npol; j++) { |
242 | Daij = DerivativeBern(Npt, j); |
243 | Cont(Inc3, j + IncCol) = Daij*T2; |
244 | Cont(Inc3, j + Npol + IncCol) = -Daij*T1; |
245 | } |
246 | Inc3 = Inc3 +1; |
247 | } |
248 | |
249 | // Remplissage du second membre: |
250 | for (i = 1; i <= IncPass; i++) { |
251 | ToolLine::Value(SSP, IPas(i), tabP2d); |
252 | Poi2d = tabP2d(i-nb3d); |
253 | Secont(i + IncSec) = Poi2d.X(); |
254 | Secont(i + IncPass + IncSec) = Poi2d.Y(); |
255 | } |
256 | IncSec = IncSec + 2*IncPass; |
257 | |
258 | // Remplissage du vecteur de depart: |
259 | for (j = 1; j <= Npol; j++) { |
260 | Poi2d = SCurv.Value(j).Point2d(k); |
261 | Start(j + IncCol) = Poi2d.X(); |
262 | Start(j + Npol + IncCol) = Poi2d.Y(); |
263 | } |
264 | IncCol = IncCol + Npol2; |
265 | } |
266 | } |
267 | |
268 | // Equations exprimant le meme rapport de tangence sur chaque courbe: |
269 | // On prend les coordonnees les plus significatives. |
270 | |
1d47d8d0 |
271 | --Inc3; |
272 | for (i = 1; i <= IncTan; ++i) { |
7fd59977 |
273 | IncCol = 0; |
274 | Npt = ITan(i); |
1d47d8d0 |
275 | for (k = 1; k <= NbCu-1; ++k) { |
276 | ++Inc3; |
277 | // Initialize first relation variable (T1) |
278 | Standard_Integer addIndex_1 = 0, aVal = Ibont(k, i); |
279 | switch (aVal) |
280 | { |
281 | case 1: //T1 ~ T1x |
282 | { |
283 | if (k <= nb3d) { |
284 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
285 | V = tabV(k); |
286 | T1 = V.X(); |
287 | IP = 3 * Npol; |
288 | } |
289 | else { |
290 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
291 | V2d = tabV2d(k-nb3d); |
292 | T1 = V2d.X(); |
293 | IP = 2 * Npol; |
294 | } |
295 | addIndex_1 = 0; |
296 | break; |
297 | } |
298 | case 2: //T1 ~ T1y |
299 | { |
300 | if (k <= nb3d) { |
301 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
302 | V = tabV(k); |
303 | IP = 3 * Npol; |
304 | } |
305 | else { |
306 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
307 | V2d = tabV2d(k-nb3d); |
308 | T1 = V2d.Y(); |
309 | IP = 2 * Npol; |
310 | } |
311 | addIndex_1 = Npol; |
312 | break; |
313 | } |
314 | case 3: //T1 ~ T1z |
315 | { |
316 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
317 | V = tabV(k); |
318 | T1 = V.Z(); |
319 | IP = 3 * Npol; |
320 | addIndex_1 = 2 * Npol; |
321 | break; |
322 | } |
7fd59977 |
323 | } |
1d47d8d0 |
324 | // Initialize second relation variable (T2) |
325 | Standard_Integer addIndex_2 = 0, aNextVal = Ibont(k + 1, i); |
326 | switch (aNextVal) |
327 | { |
328 | case 1: //T2 ~ T2x |
329 | { |
330 | if ((k+1) <= nb3d) { |
331 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
332 | V = tabV(k+1); |
333 | T2 = V.X(); |
334 | } |
335 | else { |
336 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
337 | V2d = tabV2d(k+1-nb3d); |
338 | T2 = V2d.X(); |
339 | } |
340 | addIndex_2 = 0; |
341 | break; |
342 | } |
343 | case 2: //T2 ~ T2y |
344 | { |
345 | if ((k+1) <= nb3d) { |
346 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
347 | V = tabV(k+1); |
348 | T2 = V.Y(); |
349 | } |
350 | else { |
351 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
352 | V2d = tabV2d(k+1-nb3d); |
353 | T2 = V2d.Y(); |
354 | } |
355 | addIndex_2 = Npol; |
356 | break; |
357 | } |
358 | case 3: //T2 ~ T2z |
359 | { |
360 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
361 | V = tabV(k+1); |
362 | T2 = V.Z(); |
363 | addIndex_2 = 2 * Npol; |
364 | break; |
365 | } |
7fd59977 |
366 | } |
1d47d8d0 |
367 | |
368 | // Relations between T1 and T2: |
369 | for (j = 1; j <= Npol; j++) { |
370 | Daij = DerivativeBern(Npt, j); |
371 | Cont(Inc3, j + IncCol + addIndex_1) = Daij*T2; |
372 | Cont(Inc3, j + IP + IncCol + addIndex_2) = -Daij*T1; |
7fd59977 |
373 | } |
1d47d8d0 |
374 | IncCol += IP; |
7fd59977 |
375 | } |
376 | } |
377 | |
378 | |
379 | |
380 | // Equations concernant la courbure: |
381 | |
382 | |
383 | /* Inc3 = Inc3 +1; |
384 | IncCol = 0; |
385 | for (k = 1; k <= NbCu; k++) { |
386 | for (i = 1; i <= IncCurv; i++) { |
387 | Npt = ICurv(i); |
388 | AppDef_MultiPointConstraint MP = SSP.Value(Npt); |
389 | DDA = SecondDerivativeBern(Parameters(Npt)); |
390 | if (SSP.Value(1).Dimension(k) == 3) { |
391 | C1 = MP.Curv(k).X(); |
392 | C2 = MP.Curv(k).Y(); |
393 | C3 = MP.Curv(k).Z(); |
394 | for (j = 1; j <= Npol; j++) { |
395 | Daij = DerivativeBern(Npt, j); |
396 | D2Aij = DDA(j); |
397 | Cont(Inc3, j + IncCol) = D2Aij; |
398 | Cont(Inc3, j + Npol2 + IncCol) = -C2*Daij; |
399 | Cont(Inc3, j + Npol + IncCol) = C3*Daij; |
400 | |
401 | Cont(Inc3+1, j + Npol + IncCol) = D2Aij; |
402 | Cont(Inc3+1, j +IncCol) = -C3*Daij; |
403 | Cont(Inc3+1, j + Npol2 + IncCol) = C1*Daij; |
404 | |
405 | Cont(Inc3+2, j + Npol2+IncCol) = D2Aij; |
406 | Cont(Inc3+2, j + Npol+IncCol) = -C1*Daij; |
407 | Cont(Inc3+2, j + IncCol) = C2*Daij; |
408 | } |
409 | Inc3 = Inc3 + 3; |
410 | } |
411 | else { // Dimension 2: |
412 | C1 = MP.Curv2d(k).X(); |
413 | C2 = MP.Curv2d(k).Y(); |
414 | for (j = 1; j <= Npol; j++) { |
415 | Daij = DerivativeBern(Npt, j); |
416 | D2Aij = DDA(j); |
417 | Cont(Inc3, j + IncCol) = D2Aij*C1; |
418 | Cont(Inc3+1, j + Npol + IncCol) = D2Aij*C2; |
419 | } |
420 | Inc3 = Inc3 + 2; |
421 | } |
422 | } |
423 | } |
424 | |
425 | */ |
426 | // Resolution par Uzawa: |
427 | |
428 | math_Uzawa UzaResol(Cont, Secont, Start, Tolerance); |
429 | if (!(UzaResol.IsDone())) { |
430 | Done = Standard_False; |
431 | return; |
432 | } |
433 | CTCinv = UzaResol.InverseCont(); |
434 | UzaResol.Duale(Vardua); |
435 | for (i = 1; i <= CTCinv.RowNumber(); i++) { |
436 | for (j = i; j <= CTCinv.RowNumber(); j++) { |
437 | CTCinv(i, j) = CTCinv(j, i); |
438 | } |
439 | } |
440 | Done = Standard_True; |
441 | math_Vector VecPoles (1, CCol*Npol); |
442 | VecPoles = UzaResol.Value(); |
443 | |
444 | Standard_Integer polinit = 1, polfin=Npol; |
445 | if (FC >= 1) polinit = 2; |
446 | if (LC >= 1) polfin = Npol-1; |
447 | |
448 | for (i = polinit; i <= polfin; i++) { |
449 | IncCol = 0; |
450 | AppParCurves_MultiPoint MPol(nb3d, nb2d); |
451 | for (k = 1; k <= NbCu; k++) { |
452 | if (k <= nb3d) { |
453 | gp_Pnt Pol(VecPoles(IncCol + i), |
454 | VecPoles(IncCol + Npol +i), |
455 | VecPoles(IncCol + 2*Npol + i)); |
456 | MPol.SetPoint(k, Pol); |
457 | IncCol = IncCol + 3*Npol; |
458 | } |
459 | else { |
460 | gp_Pnt2d Pol2d(VecPoles(IncCol + i), |
461 | VecPoles(IncCol + Npol + i)); |
462 | MPol.SetPoint2d(k, Pol2d); |
463 | IncCol = IncCol + 2*Npol; |
464 | } |
465 | } |
466 | SCurv.SetValue(i, MPol); |
467 | } |
468 | |
469 | } |
470 | |
471 | |
472 | |
473 | Standard_Boolean AppParCurves_ResolConstraint::IsDone () const { |
474 | return Done; |
475 | } |
476 | |
477 | |
478 | Standard_Integer AppParCurves_ResolConstraint:: |
479 | NbConstraints(const MultiLine& SSP, |
480 | const Standard_Integer, |
481 | const Standard_Integer, |
482 | const Handle(AppParCurves_HArray1OfConstraintCouple)& |
483 | TheConstraints) |
484 | const |
485 | { |
486 | // Boucle de calcul du nombre de points de passage afin de dimensionner |
487 | // les matrices. |
75259fc5 |
488 | Standard_Integer aIncPass, aIncTan, aIncCurv, aCCol; |
7fd59977 |
489 | Standard_Integer i; |
490 | AppParCurves_Constraint Cons; |
491 | |
75259fc5 |
492 | aIncPass = 0; |
493 | aIncTan = 0; |
494 | aIncCurv = 0; |
7fd59977 |
495 | |
496 | for (i = TheConstraints->Lower(); i <= TheConstraints->Upper(); i++) { |
497 | Cons = (TheConstraints->Value(i)).Constraint(); |
498 | if (Cons >= 1) { |
75259fc5 |
499 | aIncPass++; // IncPass = nbre de points de passage. |
7fd59977 |
500 | } |
501 | if (Cons >= 2) { |
75259fc5 |
502 | aIncTan++; // IncTan= nbre de points de tangence. |
7fd59977 |
503 | } |
504 | if (Cons == 3) { |
75259fc5 |
505 | aIncCurv++; // IncCurv = nbre de pts de courbure. |
7fd59977 |
506 | } |
507 | } |
508 | Standard_Integer nb3d = ToolLine::NbP3d(SSP); |
509 | Standard_Integer nb2d = ToolLine::NbP2d(SSP); |
75259fc5 |
510 | aCCol = nb3d* 3 + nb2d* 2; |
7fd59977 |
511 | |
75259fc5 |
512 | return aCCol*aIncPass + aIncTan*(aCCol-1) + 3*aIncCurv; |
7fd59977 |
513 | |
514 | } |
515 | |
516 | |
517 | Standard_Integer AppParCurves_ResolConstraint::NbColumns(const MultiLine& SSP, |
518 | const Standard_Integer Deg) |
519 | const |
520 | { |
521 | Standard_Integer nb3d = ToolLine::NbP3d(SSP); |
522 | Standard_Integer nb2d = ToolLine::NbP2d(SSP); |
523 | Standard_Integer CCol = nb3d* 3 + nb2d* 2; |
524 | |
525 | return CCol*(Deg+1); |
526 | } |
527 | |
528 | const math_Matrix& AppParCurves_ResolConstraint::ConstraintMatrix() const |
529 | { |
530 | return Cont; |
531 | } |
532 | |
533 | const math_Matrix& AppParCurves_ResolConstraint::InverseMatrix() const |
534 | { |
535 | return CTCinv; |
536 | } |
537 | |
538 | |
539 | const math_Vector& AppParCurves_ResolConstraint::Duale() const |
540 | { |
541 | return Vardua; |
542 | } |
543 | |
544 | |
545 | |
546 | const math_Matrix& AppParCurves_ResolConstraint:: |
547 | ConstraintDerivative(const MultiLine& SSP, |
548 | const math_Vector& Parameters, |
549 | const Standard_Integer Deg, |
550 | const math_Matrix& DA) |
551 | { |
552 | Standard_Integer i, j, k, nb2d, nb3d, CCol, Inc3, IncCol, Npt; |
553 | Standard_Integer Npol, Npol2, NbCu =ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP); |
554 | Standard_Integer IP; |
555 | Standard_Real Daij; |
556 | Npol = Deg+1; Npol2 = 2*Npol; |
557 | Standard_Boolean Ok; |
558 | TColStd_Array2OfInteger Ibont(1, NbCu, 1, IncTan); |
559 | math_Matrix DeCInit(1, IncPass, 1, Npol); |
560 | math_Vector DDA(1, Npol); |
561 | gp_Vec V; |
562 | gp_Vec2d V2d; |
563 | Standard_Real T1, T2, T3, Tmax, DDaij; |
564 | // Standard_Integer FirstP = IPas(1); |
565 | nb3d = ToolLine::NbP3d(SSP); |
566 | nb2d = ToolLine::NbP2d(SSP); |
567 | Standard_Integer mynb3d = nb3d, mynb2d = nb2d; |
568 | if (nb3d == 0) mynb3d = 1; |
569 | if (nb2d == 0) mynb2d = 1; |
570 | CCol = nb3d* 3 + nb2d* 2; |
571 | |
572 | TColgp_Array1OfVec tabV(1, mynb3d); |
573 | TColgp_Array1OfVec2d tabV2d(1, mynb2d); |
574 | TColgp_Array1OfPnt tabP(1, mynb3d); |
575 | TColgp_Array1OfPnt2d tabP2d(1, mynb2d); |
576 | |
577 | for (i = 1; i <= DeCont.RowNumber(); i++) |
578 | for (j = 1; j <= DeCont.ColNumber(); j++) |
579 | DeCont(i, j) = 0.0; |
580 | |
581 | |
582 | // Remplissage de DK pour les points de passages: |
583 | |
584 | for (i =1; i <= IncPass; i++) { |
585 | Npt = IPas(i); |
586 | for (j = 1; j <= Npol; j++) DeCInit(i, j) = DA(Npt, j); |
587 | } |
588 | for (i = 1; i <= CCol; i++) { |
589 | DeCont.Set(IncPass*(i-1)+1, IncPass*i, Npol*(i-1)+1, Npol*i, DeCInit); |
590 | } |
591 | |
592 | |
593 | // Pour les points de tangence: |
594 | |
595 | Inc3 = CCol*IncPass +1; |
596 | IncCol = 0; |
597 | |
598 | for (k = 1; k <= NbCu; k++) { |
599 | if (k <= nb3d) { |
600 | for (i = 1; i <= IncTan; i++) { |
601 | Npt = ITan(i); |
602 | // MultiPoint MPoint = ToolLine::Value(SSP, Npt); |
603 | // choix du maximum de tangence pour exprimer la colinearite: |
604 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
605 | V = tabV(k); |
606 | V.Coord(T1, T2, T3); |
607 | Tmax = Abs(T1); |
608 | Ibont(k, i) = 1; |
609 | if (Abs(T2) > Tmax) { |
610 | Tmax = Abs(T2); |
611 | Ibont(k, i) = 2; |
612 | } |
613 | if (Abs(T3) > Tmax) { |
614 | Tmax = Abs(T3); |
615 | Ibont(k, i) = 3; |
616 | } |
617 | AppParCurves::SecondDerivativeBernstein(Parameters(Npt), DDA); |
618 | if (Ibont(k, i) == 3) { |
619 | for (j = 1; j <= Npol; j++) { |
620 | DDaij = DDA(j); |
621 | DeCont(Inc3, j+ Npol + IncCol) = DDaij*T3/Tmax; |
622 | DeCont(Inc3, j + Npol2 + IncCol) = -DDaij *T2/Tmax; |
623 | DeCont(Inc3+1, j+ IncCol) = DDaij* T3/Tmax; |
624 | DeCont(Inc3+1, j+ Npol2 + IncCol) = -DDaij*T1/Tmax; |
625 | } |
626 | } |
627 | else if (Ibont(k, i) == 1) { |
628 | for (j = 1; j <= Npol; j++) { |
629 | DDaij = DDA(j); |
630 | DeCont(Inc3, j + IncCol) = DDaij*T3/Tmax; |
631 | DeCont(Inc3, j + Npol2 + IncCol) = -DDaij *T1/Tmax; |
632 | DeCont(Inc3+1, j+ IncCol) = DDaij* T2/Tmax; |
633 | DeCont(Inc3+1, j+ Npol +IncCol) = -DDaij*T1/Tmax; |
634 | } |
635 | } |
636 | else if (Ibont(k, i) == 2) { |
637 | for (j = 1; j <= Npol; j++) { |
638 | DDaij = DDA(j); |
639 | DeCont(Inc3, j+ Npol + IncCol) = DDaij*T3/Tmax; |
640 | DeCont(Inc3, j + Npol2 + IncCol) = -DDaij *T2/Tmax; |
641 | DeCont(Inc3+1, j+ IncCol) = DDaij* T2/Tmax; |
642 | DeCont(Inc3+1, j+ Npol + IncCol) = -DDaij*T1/Tmax; |
643 | } |
644 | } |
645 | Inc3 = Inc3 + 2; |
646 | } |
647 | IncCol = IncCol + 3*Npol; |
648 | } |
649 | else { |
650 | for (i = 1; i <= IncTan; i++) { |
651 | Npt = ITan(i); |
652 | AppParCurves::SecondDerivativeBernstein(Parameters(Npt), DDA); |
653 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
654 | V2d = tabV2d(k); |
655 | V2d.Coord(T1, T2); |
656 | Tmax = Abs(T1); |
657 | Ibont(k, i) = 1; |
658 | if (Abs(T2) > Tmax) { |
659 | Tmax = Abs(T2); |
660 | Ibont(k, i) = 2; |
661 | } |
662 | for (j = 1; j <= Npol; j++) { |
663 | DDaij = DDA(j); |
664 | DeCont(Inc3, j + IncCol) = DDaij*T2; |
665 | DeCont(Inc3, j + Npol + IncCol) = -DDaij*T1; |
666 | } |
667 | Inc3 = Inc3 +1; |
668 | } |
669 | } |
670 | } |
671 | |
672 | // Equations exprimant le meme rapport de tangence sur chaque courbe: |
673 | // On prend les coordonnees les plus significatives. |
674 | |
675 | Inc3 = Inc3 -1; |
676 | for (i =1; i <= IncTan; i++) { |
677 | IncCol = 0; |
678 | Npt = ITan(i); |
679 | AppParCurves::SecondDerivativeBernstein(Parameters(Npt), DDA); |
680 | // MultiPoint MP = ToolLine::Value(SSP, Npt); |
681 | for (k = 1; k <= NbCu-1; k++) { |
682 | Inc3 = Inc3 + 1; |
683 | if (Ibont(k, i) == 1) { |
684 | if (k <= nb3d) { |
685 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
686 | V = tabV(k); |
687 | T1 = V.X(); |
688 | IP = 3*Npol; |
689 | } |
690 | else { |
691 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
692 | V2d = tabV2d(k); |
693 | T1 = V2d.X(); |
694 | IP = 2*Npol; |
695 | } |
696 | if (Ibont(k+1, i) == 1) { // Relations entre T1x et T2x: |
697 | if ((k+1) <= nb3d) { |
698 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
699 | V = tabV(k+1); |
700 | T2 = V.X(); |
701 | } |
702 | else { |
703 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
704 | V2d = tabV2d(k+1); |
705 | T2 = V2d.X(); |
706 | } |
707 | for (j = 1; j <= Npol; j++) { |
708 | Daij = DDA(j); |
709 | Cont(Inc3, j + IncCol) = Daij*T2; |
710 | Cont(Inc3, j + IP + IncCol) = -Daij*T1; |
711 | } |
712 | IncCol = IncCol + IP; |
713 | } |
714 | else if (Ibont(k+1, i) == 2) { // Relations entre T1x et T2y: |
715 | if ((k+1) <= nb3d) { |
716 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
717 | V = tabV(k+1); |
718 | T2 = V.Y(); |
719 | } |
720 | else { |
721 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
722 | V2d = tabV2d(k+1); |
723 | T2 = V2d.Y(); |
724 | } |
725 | for (j = 1; j <= Npol; j++) { |
726 | Daij = DDA(j); |
727 | Cont(Inc3, j + IncCol) = Daij*T2; |
728 | Cont(Inc3, j + IP + Npol + IncCol) = -Daij*T1; |
729 | } |
730 | IncCol = IncCol + IP; |
731 | } |
732 | else if (Ibont(k+1,i) == 3) { // Relations entre T1x et T2z: |
96a95605 |
733 | ToolLine::Tangency(SSP, Npt, tabV); |
7fd59977 |
734 | V = tabV(k+1); |
735 | T2 = V.Z(); |
736 | for (j = 1; j <= Npol; j++) { |
737 | Daij = DDA(j); |
738 | Cont(Inc3, j + IncCol) = Daij*T2; |
739 | Cont(Inc3, j + IP + 2*Npol + IncCol) = -Daij*T1; |
740 | } |
741 | IncCol = IncCol + IP; |
742 | } |
743 | } |
744 | else if (Ibont(k,i) == 2) { |
745 | if (k <= nb3d) { |
746 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
747 | V = tabV(k); |
748 | T1 = V.Y(); |
749 | IP = 3*Npol; |
750 | } |
751 | else { |
752 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
753 | V2d = tabV2d(k); |
754 | T1 = V2d.Y(); |
755 | IP = 2*Npol; |
756 | } |
757 | if (Ibont(k+1, i) == 1) { // Relations entre T1y et T2x: |
758 | if ((k+1) <= nb3d) { |
759 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
760 | V = tabV(k+1); |
761 | T2 = V.X(); |
762 | } |
763 | else { |
764 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
765 | V2d = tabV2d(k+1); |
766 | T2 = V2d.X(); |
767 | } |
768 | for (j = 1; j <= Npol; j++) { |
769 | Daij = DDA( j); |
770 | Cont(Inc3, j + Npol + IncCol) = Daij*T2; |
771 | Cont(Inc3, j + IP + IncCol) = -Daij*T1; |
772 | } |
773 | IncCol = IncCol + IP; |
774 | |
775 | } |
776 | else if (Ibont(k+1, i) == 2) { // Relations entre T1y et T2y: |
777 | if ((k+1) <= nb3d) { |
778 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
779 | V = tabV(k+1); |
780 | T2 = V.Y(); |
781 | } |
782 | else { |
783 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
784 | V2d = tabV2d(k+1); |
785 | T2 = V2d.Y(); |
786 | } |
787 | for (j = 1; j <= Npol; j++) { |
788 | Daij = DDA(j); |
789 | Cont(Inc3, j + Npol + IncCol) = Daij*T2; |
790 | Cont(Inc3, j + IP + Npol + IncCol) = -Daij*T1; |
791 | } |
792 | IncCol = IncCol + IP; |
793 | |
794 | } |
795 | else if (Ibont(k+1,i)== 3) { // Relations entre T1y et T2z: |
96a95605 |
796 | ToolLine::Tangency(SSP, Npt, tabV); |
7fd59977 |
797 | V = tabV(k+1); |
798 | T2 = V.Z(); |
799 | for (j = 1; j <= Npol; j++) { |
800 | Daij = DDA(j); |
801 | Cont(Inc3, j + Npol +IncCol) = Daij*T2; |
802 | Cont(Inc3, j + IP + 2*Npol + IncCol) = -Daij*T1; |
803 | } |
804 | IncCol = IncCol + IP; |
805 | } |
806 | } |
807 | |
808 | else { |
809 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
810 | V = tabV(k); |
811 | T1 = V.Z(); |
812 | IP = 3*Npol; |
813 | if (Ibont(k+1, i) == 1) { // Relations entre T1z et T2x: |
814 | if ((k+1) <= nb3d) { |
815 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
816 | V = tabV(k+1); |
817 | T2 = V.X(); |
818 | } |
819 | else { |
820 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
821 | V2d = tabV2d(k+1); |
822 | T2 = V2d.X(); |
823 | } |
824 | for (j = 1; j <= Npol; j++) { |
825 | Daij = DDA(j); |
826 | Cont(Inc3, j + 2*Npol +IncCol) = Daij*T2; |
827 | Cont(Inc3, j + IP + IncCol) = -Daij*T1; |
828 | } |
829 | IncCol = IncCol + IP; |
830 | } |
831 | |
832 | else if (Ibont(k+1, i) == 2) { // Relations entre T1z et T2y: |
833 | if ((k+1) <= nb3d) { |
834 | Ok = ToolLine::Tangency(SSP, Npt, tabV); |
835 | V = tabV(k+1); |
836 | T2 = V.Y(); |
837 | } |
838 | else { |
839 | Ok = ToolLine::Tangency(SSP, Npt, tabV2d); |
840 | V2d = tabV2d(k+1); |
841 | T2 = V2d.Y(); |
842 | } |
843 | for (j = 1; j <= Npol; j++) { |
844 | Daij = DDA(j); |
845 | Cont(Inc3, j + 2*Npol +IncCol) = Daij*T2; |
846 | Cont(Inc3, j + IP + Npol + IncCol) = -Daij*T1; |
847 | } |
848 | IncCol = IncCol + IP; |
849 | } |
850 | |
851 | else if (Ibont(k+1,i)==3) { // Relations entre T1z et T2z: |
96a95605 |
852 | ToolLine::Tangency(SSP, Npt, tabV); |
7fd59977 |
853 | V = tabV(k+1); |
854 | T2 = V.Z(); |
855 | for (j = 1; j <= Npol; j++) { |
856 | Daij = DDA(j); |
857 | Cont(Inc3, j + 2*Npol + IncCol) = Daij*T2; |
858 | Cont(Inc3, j + IP + 2*Npol + IncCol) = -Daij*T1; |
859 | } |
860 | IncCol = IncCol + IP; |
861 | } |
862 | } |
863 | } |
864 | } |
865 | |
866 | return DeCont; |
867 | } |