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