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