0024624: Lost word in license statement in source files
[occt.git] / src / AppParCurves / AppParCurves_LeastSquare.gxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
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
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.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 // lpa, le 27/07/91
16 // pmn, 30/10/98 : Protection dans les cas surcontraint -> "isready"
17
18 // Approximation d une MultiLine de points decrite par le tool MLineTool.
19 // Ce programme utilise les moindres carres pour le cas suivant:
20 // passage et tangences aux extremites.
21
22
23 #define No_Standard_RangeError
24 #define No_Standard_OutOfRange
25 #define No_Standard_DimensionError
26
27 #include <math_Householder.hxx>
28 #include <math_Crout.hxx>
29 #include <AppParCurves.hxx>
30 #include <gp_Pnt.hxx>
31 #include <gp_Pnt2d.hxx>
32 #include <gp_Vec.hxx>
33 #include <gp_Vec2d.hxx>
34 #include <TColgp_Array1OfPnt.hxx>
35 #include <TColgp_Array1OfPnt2d.hxx>
36 #include <TColgp_Array1OfVec.hxx>
37 #include <TColgp_Array1OfVec2d.hxx>
38 #include <TColStd_Array1OfInteger.hxx>
39 #include <TColStd_Array1OfReal.hxx>
40 #include <AppParCurves_Constraint.hxx>
41 #include <AppParCurves_MultiPoint.hxx>
42 #include <StdFail_NotDone.hxx>
43 #include <Standard_NoSuchObject.hxx>
44 #include <TColStd_Array1OfReal.hxx>
45 #include <math_Recipes.hxx>
46 #include <math_Crout.hxx>
47
48
49
50 static int FlatLength(const TColStd_Array1OfInteger& Mults) {
51   
52   Standard_Integer sum = 0;
53   for (Standard_Integer i = Mults.Lower(); i <= Mults.Upper(); i++) {
54     sum += Mults.Value(i);
55   }
56   return sum;
57 }
58
59
60 AppParCurves_LeastSquare::
61   AppParCurves_LeastSquare(const MultiLine&    SSP,
62                const Standard_Integer          FirstPoint,
63                const Standard_Integer          LastPoint,
64                const AppParCurves_Constraint   FirstCons,
65                const AppParCurves_Constraint   LastCons,
66                const math_Vector&              Parameters,
67                const Standard_Integer          NbPol):
68                SCU(NbPol),
69                mypoles(1, NbPol,
70                    1, NbBColumns(SSP)),
71                A(FirstPoint, LastPoint, 1, NbPol),
72                DA(FirstPoint, LastPoint, 1, NbPol),
73                B2(TheFirstPoint(FirstCons, FirstPoint), 
74                   Max(TheFirstPoint(FirstCons, FirstPoint),
75                       TheLastPoint(LastCons, LastPoint)),
76                   1, NbBColumns(SSP)),
77                mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
78                Vflatknots(1, 1),
79                Vec1t(1, NbBColumns(SSP)),
80                Vec1c(1, NbBColumns(SSP)),
81                Vec2t(1, NbBColumns(SSP)),
82                Vec2c(1, NbBColumns(SSP)),
83                theError(FirstPoint, LastPoint, 
84                         1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
85                myindex(FirstPoint, LastPoint, 0),
86                nbpoles(NbPol)
87 {
88   FirstConstraint = FirstCons;  
89   LastConstraint  = LastCons;
90   Init(SSP, FirstPoint, LastPoint);
91   Perform(Parameters);          
92 }
93
94
95
96 AppParCurves_LeastSquare::
97   AppParCurves_LeastSquare(const MultiLine&              SSP,
98                            const Standard_Integer        FirstPoint,
99                            const Standard_Integer        LastPoint,
100                            const AppParCurves_Constraint FirstCons,
101                            const AppParCurves_Constraint LastCons,
102                            const Standard_Integer        NbPol):
103                SCU(NbPol),
104                mypoles(1, NbPol,
105                    1, NbBColumns(SSP)),
106                A(FirstPoint, LastPoint, 1, NbPol),
107                DA(FirstPoint, LastPoint, 1, NbPol), 
108                B2(TheFirstPoint(FirstCons, FirstPoint), 
109                   Max(TheFirstPoint(FirstCons, FirstPoint),
110                       TheLastPoint(LastCons, LastPoint)),
111                   1, NbBColumns(SSP)),
112                mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
113                Vflatknots(1, 1),
114                Vec1t(1, NbBColumns(SSP)),
115                Vec1c(1, NbBColumns(SSP)),
116                Vec2t(1, NbBColumns(SSP)),
117                Vec2c(1, NbBColumns(SSP)),
118                theError(FirstPoint, LastPoint, 
119                         1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
120                myindex(FirstPoint, LastPoint, 0),
121                nbpoles(NbPol)
122 {
123   FirstConstraint = FirstCons;  
124   LastConstraint  = LastCons;
125   Init(SSP, FirstPoint, LastPoint);
126 }
127
128
129 AppParCurves_LeastSquare::
130   AppParCurves_LeastSquare(const MultiLine&       SSP,
131                const TColStd_Array1OfReal&        Knots,
132                const TColStd_Array1OfInteger&     Mults,
133                const Standard_Integer             FirstPoint,
134                const Standard_Integer             LastPoint,
135                const AppParCurves_Constraint      FirstCons,
136                const AppParCurves_Constraint      LastCons,
137                const math_Vector&                 Parameters,
138                const Standard_Integer             NbPol):
139                SCU(NbPol),
140                mypoles(1, NbPol,
141                    1, NbBColumns(SSP)),
142                A(FirstPoint, LastPoint, 1, NbPol),
143                DA(FirstPoint, LastPoint, 1, NbPol),
144                B2(TheFirstPoint(FirstCons, FirstPoint), 
145                   Max(TheFirstPoint(FirstCons, FirstPoint),
146                       TheLastPoint(LastCons, LastPoint)),
147                   1, NbBColumns(SSP)),
148                mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
149                Vflatknots(1, FlatLength(Mults)),
150                Vec1t(1, NbBColumns(SSP)),
151                Vec1c(1, NbBColumns(SSP)),
152                Vec2t(1, NbBColumns(SSP)),
153                Vec2c(1, NbBColumns(SSP)),
154                theError(FirstPoint, LastPoint, 
155                         1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
156                myindex(FirstPoint, LastPoint, 0),
157                nbpoles(NbPol)
158 {
159   FirstConstraint = FirstCons;  
160   LastConstraint  = LastCons;
161   myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper());
162   myknots->ChangeArray1() = Knots;
163   mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper());
164   mymults->ChangeArray1() = Mults;
165   SCU.SetKnots(Knots);
166   SCU.SetMultiplicities(Mults);
167   Init(SSP, FirstPoint, LastPoint);
168   Perform(Parameters);          
169 }
170
171
172
173 AppParCurves_LeastSquare::
174   AppParCurves_LeastSquare(const MultiLine&               SSP,
175                            const TColStd_Array1OfReal&    Knots,
176                            const TColStd_Array1OfInteger& Mults,
177                            const Standard_Integer         FirstPoint,
178                            const Standard_Integer         LastPoint,
179                            const AppParCurves_Constraint  FirstCons,
180                            const AppParCurves_Constraint  LastCons,
181                            const Standard_Integer         NbPol):
182                SCU(NbPol),
183                mypoles(1, NbPol,
184                    1, NbBColumns(SSP)),
185                A(FirstPoint, LastPoint, 1, NbPol),
186                DA(FirstPoint, LastPoint, 1, NbPol),
187                B2(TheFirstPoint(FirstCons, FirstPoint), 
188                   Max(TheFirstPoint(FirstCons, FirstPoint),
189                       TheLastPoint(LastCons, LastPoint)),
190                   1, NbBColumns(SSP)),
191                mypoints(FirstPoint, LastPoint, 1, NbBColumns(SSP)),
192                Vflatknots(1, FlatLength(Mults)),
193                Vec1t(1, NbBColumns(SSP)),
194                Vec1c(1, NbBColumns(SSP)),
195                Vec2t(1, NbBColumns(SSP)),
196                Vec2c(1, NbBColumns(SSP)),
197                theError(FirstPoint, LastPoint, 
198                         1, ToolLine::NbP3d(SSP)+ToolLine::NbP2d(SSP), 0.0),
199                myindex(FirstPoint, LastPoint, 0),
200                nbpoles(NbPol)
201 {
202   myknots = new TColStd_HArray1OfReal(Knots.Lower(), Knots.Upper());
203   myknots->ChangeArray1() = Knots;
204   mymults = new TColStd_HArray1OfInteger(Mults.Lower(), Mults.Upper());
205   mymults->ChangeArray1() = Mults;
206   SCU.SetKnots(Knots);
207   SCU.SetMultiplicities(Mults);
208   FirstConstraint = FirstCons;  
209   LastConstraint  = LastCons;
210   Init(SSP, FirstPoint, LastPoint);
211 }
212
213
214
215 void AppParCurves_LeastSquare::Init(const MultiLine& SSP, 
216                                     const Standard_Integer FirstPoint,
217                                     const Standard_Integer LastPoint)
218 {
219   // Variable de controle
220   iscalculated = Standard_False;
221   isready = Standard_True;
222
223   myfirstp = FirstPoint;
224   mylastp = LastPoint;
225   FirstP = TheFirstPoint(FirstConstraint, myfirstp);
226   LastP  = TheLastPoint(LastConstraint, mylastp);
227
228   // Reperage des contraintes aux extremites:
229   // ========================================
230   Standard_Integer i, j, k, i2;
231
232   nbP2d = ToolLine::NbP2d(SSP);
233   nbP   = ToolLine::NbP3d(SSP);
234   gp_Pnt Poi;
235   gp_Pnt2d Poi2d;
236 //  gp_Vec V3d;
237 //  gp_Vec2d V2d;
238   Standard_Integer mynbP2d = nbP2d, mynbP = nbP;
239   if (nbP2d == 0) mynbP2d = 1;
240   if (nbP == 0) mynbP = 1;
241   TColgp_Array1OfPnt TabP(1, mynbP);
242   TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
243   TColgp_Array1OfVec TabV(1, mynbP);
244   TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
245
246
247   deg = nbpoles-1;
248
249   if (!mymults.IsNull()) {
250     Standard_Integer sum = 0;
251     for (i = mymults->Lower(); i <= mymults->Upper(); i++) {
252       sum += mymults->Value(i);
253     }
254     deg = sum -nbpoles-1;
255     k = 1;
256     Standard_Real val;
257     for (i = myknots->Lower(); i <= myknots->Upper(); i++) {
258       for (j = 1; j <= mymults->Value(i); j++) {
259         val = myknots->Value(i);
260         Vflatknots(k) = val;
261         k++;
262       }
263     }
264   }
265
266
267   Affect(SSP, FirstPoint, FirstConstraint, Vec1t, Vec1c);
268
269   Affect(SSP, LastPoint, LastConstraint, Vec2t, Vec2c);
270
271   for (j = myfirstp; j <= mylastp; j++) {
272     i2 = 1;
273     if (nbP != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP,TabP2d);
274     else if (nbP2d != 0)        ToolLine::Value(SSP, j, TabP2d);
275     else                        ToolLine::Value(SSP, j, TabP);
276     for (i = 1; i <= nbP; i++) {
277       (TabP(i)).Coord(mypoints(j,i2),mypoints(j,i2+1),mypoints(j,i2+2));
278       i2 += 3;
279     }
280     for (i = 1;i <= nbP2d; i++) {
281       (TabP2d(i)).Coord(mypoints(j, i2), mypoints(j, i2+1));
282       i2 += 2;
283     }
284   }
285
286   AppParCurves_MultiPoint Pole1(nbP, nbP2d), PoleN(nbP, nbP2d);
287
288   if (FirstConstraint == AppParCurves_PassPoint ||
289       FirstConstraint == AppParCurves_TangencyPoint ||
290       FirstConstraint == AppParCurves_CurvaturePoint) {
291     i2 = 1;
292     for (i = 1; i <= nbP; i++) {
293       Poi.SetCoord(mypoints(myfirstp, i2), 
294                    mypoints(myfirstp, i2+1), 
295                    mypoints(myfirstp, i2+2));
296       Pole1.SetPoint(i, Poi);
297       i2 += 3;
298     }
299     for (i = 1; i <= nbP2d; i++) {
300       Poi2d.SetCoord(mypoints(myfirstp, i2), mypoints(myfirstp, i2+1));
301       Pole1.SetPoint2d(i+nbP, Poi2d);
302       i2 += 2;
303     }
304     for (i = 1; i <= mypoles.ColNumber(); i++)
305       mypoles(1, i) = mypoints(myfirstp, i);
306   }
307   
308
309
310   if (LastConstraint == AppParCurves_PassPoint ||
311       LastConstraint == AppParCurves_TangencyPoint ||
312       FirstConstraint == AppParCurves_CurvaturePoint) {
313     i2 = 1;
314     for (i = 1; i <= nbP; i++) {
315       Poi.SetCoord(mypoints(mylastp, i2), 
316                    mypoints(mylastp, i2+1), 
317                    mypoints(mylastp, i2+2));
318       PoleN.SetPoint(i, Poi);
319       i2 += 3;
320     }
321     for (i = 1; i <= nbP2d; i++) {
322       Poi2d.SetCoord(mypoints(mylastp, i2), 
323                      mypoints(mylastp, i2+1));
324       PoleN.SetPoint2d(i+nbP, Poi2d);
325       i2 += 2;
326     }
327     
328     for (i = 1; i <= mypoles.ColNumber(); i++)
329       mypoles(nbpoles, i) = mypoints(mylastp, i);
330   }
331
332
333   if (FirstConstraint == AppParCurves_NoConstraint) { 
334     resinit = 1;
335     SCU.SetValue(1, Pole1);
336     if (LastConstraint == AppParCurves_NoConstraint) {
337       resfin = nbpoles;
338     }
339     else if (LastConstraint == AppParCurves_PassPoint) {
340       resfin = nbpoles-1;
341       SCU.SetValue(nbpoles, PoleN);
342     }
343     else if (LastConstraint == AppParCurves_TangencyPoint) {
344       resfin = nbpoles-2;
345       SCU.SetValue(nbpoles, PoleN);
346     }
347     else if (LastConstraint == AppParCurves_CurvaturePoint) {
348       resfin = nbpoles-3;
349       SCU.SetValue(nbpoles, PoleN);
350     }
351   }
352   else if (FirstConstraint == AppParCurves_PassPoint) {
353     resinit = 2;
354     SCU.SetValue(1, Pole1);
355     if (LastConstraint == AppParCurves_NoConstraint) {
356       resfin = nbpoles;
357     }
358     else if (LastConstraint == AppParCurves_PassPoint) {
359       resfin = nbpoles-1;
360       SCU.SetValue(nbpoles, PoleN);
361     }
362     else if (LastConstraint == AppParCurves_TangencyPoint) {
363       resfin = nbpoles-2;
364       SCU.SetValue(nbpoles, PoleN);
365     }
366     else if (LastConstraint == AppParCurves_CurvaturePoint) {
367       resfin = nbpoles-3;
368       SCU.SetValue(nbpoles, PoleN);
369     }
370   }
371   else if (FirstConstraint == AppParCurves_TangencyPoint) {
372     resinit = 3;
373     SCU.SetValue(1, Pole1);
374     if (LastConstraint == AppParCurves_NoConstraint) {
375       resfin = nbpoles;
376     }
377     if (LastConstraint == AppParCurves_PassPoint) {
378       resfin = nbpoles-1;
379       SCU.SetValue(nbpoles, PoleN);
380     }
381     if (LastConstraint == AppParCurves_TangencyPoint) {
382       resfin = nbpoles-2;
383       SCU.SetValue(nbpoles, PoleN);
384     }
385     else if (LastConstraint == AppParCurves_CurvaturePoint) {
386       resfin = nbpoles-3;
387       SCU.SetValue(nbpoles, PoleN);
388     }
389   }
390   else if (FirstConstraint == AppParCurves_CurvaturePoint) {
391     resinit = 4;
392     SCU.SetValue(1, Pole1);
393     if (LastConstraint == AppParCurves_NoConstraint) {
394       resfin = nbpoles;
395     }
396     if (LastConstraint == AppParCurves_PassPoint) {
397       resfin = nbpoles-1;
398       SCU.SetValue(nbpoles, PoleN);
399     }
400     if (LastConstraint == AppParCurves_TangencyPoint) {
401       resfin = nbpoles-2;
402       SCU.SetValue(nbpoles, PoleN);
403     }
404     else if (LastConstraint == AppParCurves_CurvaturePoint) {
405       resfin = nbpoles-3;
406       SCU.SetValue(nbpoles, PoleN);
407     }
408   }
409
410   Standard_Integer Nincx = resfin-resinit+1;
411   if  (Nincx<1) { //Impossible d'aller plus loin
412     isready = Standard_False;
413     return;
414   } 
415   Standard_Integer Neq = LastP-FirstP+1;
416   
417   NA = 3*nbP+2*nbP2d;
418   Nlignes = NA*Neq;
419   Ninc = NA*Nincx;
420   if (FirstConstraint >= AppParCurves_TangencyPoint) Ninc++;
421   if (LastConstraint >= AppParCurves_TangencyPoint) Ninc++;
422 }
423
424
425
426
427 void AppParCurves_LeastSquare::Perform(const math_Vector& Parameters) {
428
429   done = Standard_False;
430   if (!isready) {
431     return;
432   }
433   Standard_Integer i, j, k, Ci, Nincx, i2, k1, k2;
434   Standard_Integer nbpol1 = nbpoles-1, Ninc1 = Ninc-1;
435   Standard_Real AD1, A0;
436 //  gp_Pnt Pt;
437 //  gp_Pnt2d Pt2d;
438   iscalculated = Standard_False;
439
440   // calcul de la matrice A et DA des fonctions d approximation:
441   ComputeFunction(Parameters);
442
443   if (FirstConstraint != AppParCurves_TangencyPoint && 
444       LastConstraint != AppParCurves_TangencyPoint) { 
445     if (FirstConstraint == AppParCurves_NoConstraint) { 
446       if (LastConstraint == AppParCurves_NoConstraint ) {
447         
448         math_Householder HouResol(A, mypoints); 
449         if (!(HouResol.IsDone())) {
450           done = Standard_False;
451           return;
452         }
453         done = Standard_True;
454         mypoles = HouResol.AllValues();
455         return;
456         
457       }
458       else {
459         for (j = FirstP; j <= LastP; j++) {
460           AD1 = A(j, nbpoles);
461           for (i = 1; i <= B2.ColNumber(); i++) {
462             B2(j, i) = mypoints(j,i) - AD1*mypoles(nbpoles, i);
463           }
464         }
465       }
466     }
467     else if (FirstConstraint == AppParCurves_PassPoint) {
468       if (LastConstraint == AppParCurves_NoConstraint) {
469         for (j = FirstP; j <= LastP; j++) {
470           A0 = A(j, 1);
471           for (i = 1; i <= B2.ColNumber(); i++) {
472             B2(j, i) =  mypoints(j, i)- A0*mypoles(1, i);
473           }
474         }
475       }
476       else if (LastConstraint == AppParCurves_PassPoint) {
477         for (j = FirstP; j <= LastP; j++) {
478           A0 = A(j, 1);
479           AD1 = A(j, nbpoles);
480           for (i = 1; i <= B2.ColNumber(); i++) {
481             B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i) 
482                                   - AD1* mypoles(nbpoles, i);
483           }
484         }
485       }
486     }
487
488     // resolution:
489
490     Standard_Integer Nincx = resfin-resinit+1;
491     if (Nincx < 1) { 
492       done = Standard_True;
493       return;
494     }
495     math_IntegerVector Index(1, Nincx);
496     SearchIndex(Index);
497     math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0);
498     math_Vector TheAA(1, Index(Nincx), 0.0);
499     math_Vector myTABB(1, Nincx, 0.0);
500     
501     MakeTAA(TheAA, mytab);
502     DACTCL_Decompose(TheAA, Index);
503     
504     Standard_Integer kk2;
505     for (j = 1; j <= B2.ColNumber(); j++) {
506       kk2 = 1;
507       for (i = resinit; i <= resfin; i++) {
508         myTABB(kk2) = mytab(i, j);
509         kk2++;
510       }
511       DACTCL_Solve(TheAA, myTABB, Index);
512       
513       i2 = 1;
514       for (k = resinit; k <= resfin; k++) {
515         mypoles(k, j)  = myTABB.Value(i2);
516         i2++;
517       }
518     }
519     done = Standard_True;
520   }
521
522   // ===========================================================
523   // cas de tangence:
524   // ===========================================================
525
526   Nincx = resfin-resinit+1;
527   Standard_Integer deport = 0, Nincx2 = 2*Nincx;
528   
529   math_IntegerVector InternalIndex(1, Nincx);
530   SearchIndex(InternalIndex);
531   math_IntegerVector Index(1, Ninc);
532   
533   Standard_Integer l = 1;
534   if (resinit <= resfin) {
535     for (j = 0; j <= NA-1; j++) {
536       deport = j*InternalIndex(Nincx);
537       for (i = 1; i <= Nincx; i++) {
538         Index(l) = InternalIndex(i) + deport;
539         l++;
540       }
541     }
542   }
543       
544   if (resinit > resfin) Index(1) = 1;
545   if (Ninc1 > 1) {
546     if (FirstConstraint >= AppParCurves_TangencyPoint &&
547         LastConstraint >= AppParCurves_TangencyPoint) 
548       Index(Ninc1) = Index(Ninc1 - 1) + Ninc1;
549   }
550   if (FirstConstraint >= AppParCurves_TangencyPoint ||
551       LastConstraint >= AppParCurves_TangencyPoint) 
552     Index(Ninc) = Index(Ninc-1) + Ninc;
553   
554
555   math_Vector TheA(1, Index(Ninc), 0.0);
556   math_Vector myTAB(1, Ninc, 0.0);
557   
558   MakeTAA(TheA, myTAB);
559   
560   Standard_Integer Error = DACTCL_Decompose(TheA, Index);
561   Error = DACTCL_Solve(TheA, myTAB, Index);
562   
563   if (!Error) done = Standard_True;
564   
565   if (FirstConstraint >= AppParCurves_TangencyPoint &&
566       LastConstraint >= AppParCurves_TangencyPoint) {
567     lambda1 = myTAB.Value(Ninc1);
568     lambda2 = myTAB.Value(Ninc);
569   }
570   else if (FirstConstraint >= AppParCurves_TangencyPoint)
571     lambda1 = myTAB.Value(Ninc);
572   else if (LastConstraint >= AppParCurves_TangencyPoint)
573     lambda2 = myTAB.Value(Ninc);
574
575
576   
577   // Les resultats sont stockes dans mypoles.
578   //=========================================
579   k = 1;
580   i2 = 1;
581   for (Ci = 1; Ci <= nbP; Ci++) {
582     k1 = k+1; k2 = k+2;
583     for (j = resinit; j <= resfin; j++) {
584       mypoles(j, k)  = myTAB.Value(i2);
585       mypoles(j, k1) = myTAB.Value(i2+Nincx);
586       mypoles(j, k2) = myTAB.Value(i2+Nincx2);
587       i2++;
588     }
589     
590     if (FirstConstraint >= AppParCurves_TangencyPoint) {
591       mypoles(2, k)        = mypoints(myfirstp, k)   + lambda1*Vec1t(k);
592       mypoles(2, k1)       = mypoints(myfirstp, k1)  + lambda1*Vec1t(k1);
593       mypoles(2, k2)       = mypoints(myfirstp, k2)  + lambda1*Vec1t(k2);
594     }
595     if (LastConstraint >= AppParCurves_TangencyPoint) {
596       mypoles(nbpol1, k)   = mypoints(mylastp,  k)   - lambda2*Vec2t(k);
597       mypoles(nbpol1, k1)  = mypoints(mylastp,  k1)  - lambda2*Vec2t(k1);
598       mypoles(nbpol1, k2)  = mypoints(mylastp,  k2)  - lambda2*Vec2t(k2);
599     }
600     k += 3; i2 += Nincx2;
601   }
602
603   for (Ci = 1; Ci <= nbP2d; Ci++) {
604     k1 = k+1; k2 = k+2;
605     for (j = resinit; j <= resfin; j++) {
606       mypoles(j, k)  = myTAB.Value(i2);
607       mypoles(j, k1) = myTAB.Value(i2+Nincx);
608       i2++;
609     }
610     if (FirstConstraint >= AppParCurves_TangencyPoint) {
611       mypoles(2, k)       = mypoints(myfirstp, k)  + lambda1*Vec1t(k);
612       mypoles(2, k1)      = mypoints(myfirstp, k1) + lambda1*Vec1t(k1);
613     }
614     if (LastConstraint >= AppParCurves_TangencyPoint) {
615       mypoles(nbpol1, k)  = mypoints(mylastp,  k)  - lambda2*Vec2t(k);
616       mypoles(nbpol1, k1) = mypoints(mylastp,  k1) - lambda2*Vec2t(k1);
617     }
618     k += 2; i2 += Nincx;
619   }
620   
621 }
622
623 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
624                                        const math_Vector&  V1t,
625                                        const math_Vector&  V2t,
626                                        const Standard_Real l1,
627                                        const Standard_Real l2) 
628 {
629   done = Standard_False;
630   if (!isready) {
631     return;
632   }
633   Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
634   resinit = 3; resfin = nbpoles-2;
635   Standard_Integer Nincx = resfin-resinit+1;
636   Ninc = NA*Nincx + 2;
637   FirstConstraint = AppParCurves_TangencyPoint;
638   LastConstraint = AppParCurves_TangencyPoint;
639   for (i = 1; i <= Vec1t.Upper(); i++) {
640     Vec1t(i) = V1t(i+lower1-1);
641     Vec2t(i) = V2t(i+lower2-1);
642   }
643   Perform (Parameters, l1, l2);
644 }
645
646
647 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
648                                        const math_Vector&  V1t,
649                                        const math_Vector&  V2t,
650                                        const math_Vector&  V1c,
651                                        const math_Vector&  V2c,
652                                        const Standard_Real l1,
653                                        const Standard_Real l2) {
654   done = Standard_False;
655   if (!isready) {
656     return;
657   }
658   Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
659   Standard_Integer lowc1 = V1c.Lower(), lowc2 = V2c.Lower();
660   resinit = 4; resfin = nbpoles-3;
661   Standard_Integer Nincx = resfin-resinit+1;
662   Ninc = NA*Nincx + 2;
663   FirstConstraint = AppParCurves_CurvaturePoint;
664   LastConstraint = AppParCurves_CurvaturePoint;
665
666   for (i = 1; i <= Vec1t.Upper(); i++) {
667     Vec1t(i) = V1t(i+lower1-1);
668     Vec2t(i) = V2t(i+lower2-1);
669     Vec1c(i) = V1c(i+lowc1-1);
670     Vec2c(i) = V2c(i+lowc2-1);
671   }
672   Perform (Parameters, l1, l2);
673 }
674
675
676
677 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
678                                        const Standard_Real l1,
679                                        const Standard_Real l2) {
680   done = Standard_False;
681   if (!isready) {
682     return;
683   }
684   if (FirstConstraint < AppParCurves_TangencyPoint &&
685       LastConstraint  < AppParCurves_TangencyPoint) {
686     Perform(Parameters);
687     return;
688   }
689   iscalculated = Standard_False;
690
691   lambda1 = l1;
692   lambda2 = l2;
693   Standard_Integer i, j, k, i2;
694   Standard_Real AD0, AD1, AD2, A0, A1, A2;
695 //  gp_Pnt Pt, P1, P2;
696 //  gp_Pnt2d Pt2d, P12d, P22d;
697   Standard_Real l11 = deg*l1, l22 = deg*l2;
698
699   ComputeFunction(Parameters);
700
701   if (FirstConstraint >= AppParCurves_TangencyPoint) {
702     for (i = 1; i <= mypoles.ColNumber(); i++)
703       mypoles(2, i) = mypoints(myfirstp, i) + l1*Vec1t(i);
704   }
705
706
707   if (FirstConstraint == AppParCurves_CurvaturePoint) {
708     for (i = 1; i <= mypoles.ColNumber(); i++)
709       mypoles(3, i) = 2*mypoles(2, i)-mypoles(1, i) 
710                   + l11*l11*Vec1c(i)/(deg*(deg-1));
711   }
712
713
714   if (LastConstraint >= AppParCurves_TangencyPoint) {
715     for (i = 1; i <= mypoles.ColNumber(); i++)
716       mypoles(nbpoles-1, i) = mypoints(mylastp, i) - l2*Vec2t(i);
717   }
718
719
720   if (LastConstraint == AppParCurves_CurvaturePoint) {
721     for (i = 1; i <= mypoles.ColNumber(); i++)
722       mypoles(nbpoles-2, i) = 2*mypoles(nbpoles-1, i) - mypoles(nbpoles, i)
723                           + l22*l22*Vec2c(i)/(deg*(deg-1));
724   }
725
726   if (resinit > resfin) {
727     done = Standard_True;
728     return;
729   }
730
731   if (FirstConstraint == AppParCurves_NoConstraint) { 
732     if (LastConstraint == AppParCurves_TangencyPoint) {
733       for (j = FirstP; j <= LastP; j++) {
734         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); 
735         for (i = 1; i <= B2.ColNumber(); i++) {
736           B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
737                                 - AD1 * mypoles(nbpoles-1, i);
738         }
739       }
740     }
741     if (LastConstraint == AppParCurves_CurvaturePoint) {
742       for (j = FirstP; j <= LastP; j++) {
743         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
744         for (i = 1; i <= B2.ColNumber(); i++) {
745           B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
746                                 - AD1 * mypoles(nbpoles-1, i)
747                                 - AD2 * mypoles(nbpoles-2, i);
748         }
749       }
750     }
751   }
752   else if (FirstConstraint == AppParCurves_PassPoint) {
753     if (LastConstraint == AppParCurves_TangencyPoint) {
754       for (j = FirstP; j <= LastP; j++) {
755         A0 = A(j, 1);
756         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1);
757         for (i = 1; i <= B2.ColNumber(); i++) {
758           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
759                                 - AD0 * mypoles(nbpoles, i)
760                                 - AD1 * mypoles(nbpoles-1, i);
761         }
762       }
763     }
764     if (LastConstraint == AppParCurves_CurvaturePoint) {
765       for (j = FirstP; j <= LastP; j++) {
766         A0 = A(j, 1);
767         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
768         for (i = 1; i <= B2.ColNumber(); i++) {
769           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
770                                 - AD0 * mypoles(nbpoles, i)
771                                 - AD1 * mypoles(nbpoles-1, i)
772                                 - AD2 * mypoles(nbpoles-2, i);
773         }
774       }
775     }
776   }
777   else if (FirstConstraint == AppParCurves_TangencyPoint) {
778     if (LastConstraint == AppParCurves_NoConstraint) {
779       for (j = FirstP; j <= LastP; j++) {
780         A0 = A(j, 1); A1 = A(j, 2);
781         for (i = 1; i <= B2.ColNumber(); i++) {
782           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
783                                     - A1  * mypoles(2, i);
784         }
785       }
786     }
787     else if (LastConstraint == AppParCurves_PassPoint) {
788       for (j = FirstP; j <= LastP; j++) {
789         A0 = A(j, 1);  AD0 = A(j, nbpoles); A1 = A(j, 2);
790         for (i = 1; i <= B2.ColNumber(); i++) {
791           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
792                                     - AD0 * mypoles(nbpoles, i)
793                                     - A1  * mypoles(2, i);
794         }
795       }
796     }
797     else if (LastConstraint == AppParCurves_TangencyPoint) {
798       for (j = FirstP; j <= LastP; j++) {
799         A0 = A(j, 1);  AD0 = A(j, nbpoles); A1 = A(j, 2); AD1 = A(j, nbpoles-1);
800         for (i = 1; i <= B2.ColNumber(); i++) {
801           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
802                                     - AD0 * mypoles(nbpoles, i)
803                                     - A1  * mypoles(2, i)
804                                     - AD1 * mypoles(nbpoles-1, i);
805         }
806       }
807     }
808   }
809   else if (FirstConstraint == AppParCurves_CurvaturePoint) {
810     if (LastConstraint == AppParCurves_NoConstraint) {
811       for (j = FirstP; j <= LastP; j++) {
812         A0 = A(j, 1);  A1 = A(j, 2);  A2 = A(j, 3);
813         for (i = 1; i <= B2.ColNumber(); i++) {
814           B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
815                                 - A1 * mypoles(2, i)
816                                 - A2 * mypoles(3, i);
817         }
818       }
819     }
820     else if (LastConstraint == AppParCurves_PassPoint) {
821       for (j = FirstP; j <= LastP; j++) {
822         A0 = A(j, 1);  A1 = A(j, 2);  A2 = A(j, 3);  AD0 = A(j, nbpoles);
823         for (i = 1; i <= B2.ColNumber(); i++) {
824           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
825                                 - A1  * mypoles(2, i)
826                                 - A2  * mypoles(3, i)
827                                 - AD0 * mypoles(nbpoles, i);
828         }
829       }
830     }
831     else if (LastConstraint == AppParCurves_TangencyPoint) {
832       for (j = FirstP; j <= LastP; j++) {
833         A0 = A(j, 1);     A1 = A(j, 2);   A2 = A(j, 3);
834         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1);
835         for (i = 1; i <= B2.ColNumber(); i++) {
836           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
837                                 - A1  * mypoles(2, i)
838                                 - A2  * mypoles(3, i)
839                                 - AD0 * mypoles(nbpoles, i)
840                                 - AD1 * mypoles(nbpoles-1, i);
841         }
842       }
843     }
844     else if (LastConstraint == AppParCurves_CurvaturePoint) {
845       for (j = FirstP; j <= LastP; j++) {
846         A0 = A(j, 1);     A1 = A(j, 2);   A2 = A(j, 3);
847         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
848         for (i = 1; i <= B2.ColNumber(); i++) {
849           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i) 
850                                 - A1  * mypoles(2, i)
851                                 - A2  * mypoles(3, i)
852                                 - AD0 * mypoles(nbpoles, i)
853                                 - AD1 * mypoles(nbpoles-1, i)
854                                 - AD2 * mypoles(nbpoles-2, i);
855         }
856       }
857     }
858   }
859   
860   Standard_Integer Nincx = resfin-resinit+1;
861
862   math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0);
863   math_IntegerVector Index(1, Nincx);
864   SearchIndex(Index);
865   math_Vector AA(1, Index(Nincx), 0.0);
866   MakeTAA(AA, mytab);
867
868   math_Vector myTABB(1, Nincx, 0.0);
869
870   DACTCL_Decompose(AA, Index);
871   
872   Standard_Integer kk2;
873   for (j = 1; j <= B2.ColNumber(); j++) {
874     kk2 = 1;
875     for (i = resinit; i <= resfin; i++) {
876       myTABB(kk2) = mytab(i, j);
877       kk2++;
878     }
879     
880     DACTCL_Solve(AA, myTABB, Index);
881     
882     i2 = 1;
883     for (k = resinit; k <= resfin; k++) {
884       mypoles(k, j)  = myTABB.Value(i2);
885       i2++;
886     }
887
888   }
889   
890   done = Standard_True;
891
892 }
893
894
895
896
897 void AppParCurves_LeastSquare::Affect(const MultiLine& SSP,
898                                       const Standard_Integer Index,
899                                       AppParCurves_Constraint& Cons,
900                                       math_Vector& Vt,
901                                       math_Vector& Vc)
902 {
903   // Vt: vecteur tangent, Vc: vecteur courbure.
904
905   if (Cons >= AppParCurves_TangencyPoint) {
906     Standard_Integer i, i2 = 1;
907     Standard_Boolean Ok;
908     Standard_Integer mynbP2d = nbP2d, mynbP = nbP;
909     if (nbP2d == 0) mynbP2d = 1;
910     if (nbP == 0) mynbP = 1;
911     TColgp_Array1OfPnt TabP(1, mynbP);
912     TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
913     TColgp_Array1OfVec TabV(1, mynbP);
914     TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
915     
916     if (Cons == AppParCurves_CurvaturePoint) {
917       if (nbP != 0 && nbP2d != 0) {
918         Ok = ToolLine::Curvature(SSP, Index,TabV,TabV2d);
919         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
920       }
921       else if (nbP2d != 0) {
922         Ok = ToolLine::Curvature(SSP, Index, TabV2d);
923         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
924       }
925       else {
926         Ok = ToolLine::Curvature(SSP, Index, TabV);
927         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
928       }
929       if (Ok) {
930         for (i = 1; i <= nbP; i++) {
931           (TabV(i)).Coord(Vc(i2), Vc(i2+1), Vc(i2+2));
932           i2 += 3;
933         }
934         for (i = 1; i <= nbP2d; i++) {
935           (TabV2d(i)).Coord(Vc(i2), Vc(i2+1));
936           i2 += 2;
937         }
938       }
939     }
940
941     i2 = 1;
942     if (Cons >= AppParCurves_TangencyPoint) {
943       if (nbP != 0 && nbP2d != 0) {
944         Ok = ToolLine::Tangency(SSP, Index, TabV, TabV2d);
945         if (!Ok) { Cons = AppParCurves_PassPoint;}
946       }
947       else if (nbP2d != 0) {
948         Ok = ToolLine::Tangency(SSP, Index, TabV2d);
949         if (!Ok) { Cons = AppParCurves_PassPoint;}
950       }
951       else {
952         Ok = ToolLine::Tangency(SSP, Index, TabV);
953         if (!Ok) { Cons = AppParCurves_PassPoint;}
954       }
955       if (Ok) {
956         for (i = 1; i <= nbP; i++) {
957           (TabV(i)).Coord(Vt(i2), Vt(i2+1), Vt(i2+2));
958           i2 += 3;
959         }
960         for (i = 1; i <= nbP2d; i++) {
961           (TabV2d(i)).Coord(Vt(i2), Vt(i2+1));
962           i2 += 2;
963         }
964       }
965     }
966   }
967 }
968
969
970
971 Standard_Integer AppParCurves_LeastSquare::NbBColumns(
972                                      const MultiLine& SSP) const
973 {
974   Standard_Integer BCol;
975   BCol = (ToolLine::NbP3d(SSP))*3 +
976          (ToolLine::NbP2d(SSP))*2;
977   return BCol;
978 }
979
980
981 void AppParCurves_LeastSquare::Error(Standard_Real& F, 
982                                      Standard_Real& MaxE3d,
983                                      Standard_Real& MaxE2d)
984 {
985
986   if (!done) {StdFail_NotDone::Raise();}
987   Standard_Integer i, j, k, i2, indexdeb, indexfin;
988   Standard_Integer i21, i22;
989   Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
990   MaxE3d = MaxE2d = 0.0;
991   F = 0.0;
992   i2 = 1;
993   math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
994
995   for (k = 1 ; k <= nbP+nbP2d; k++) {
996     i21 = i2+1; i22 = i2+2;
997     for (i = 1; i <= nbpoles; i++) {
998       Px(i) = mypoles(i, i2);
999       Py(i) = mypoles(i, i21);
1000       if (k <= nbP) Pz(i) = mypoles(i, i22);
1001     }
1002     for (i = FirstP; i <= LastP; i++) {
1003       AA = 0.0; BB = 0.0; CC = 0.0;
1004       indexdeb = myindex(i) + 1;
1005       indexfin = indexdeb + deg;
1006       for (j = indexdeb; j <= indexfin; j++) {
1007         AIJ = A(i, j);
1008         AA += AIJ*Px(j);
1009         BB += AIJ*Py(j);
1010         if (k <= nbP) CC += AIJ*Pz(j);
1011       }
1012       FX = AA-mypoints(i, i2);
1013       FY = BB-mypoints(i, i21);
1014       Fi= FX*FX + FY*FY;
1015       if (k <= nbP) {
1016         FZ = CC-mypoints(i, i22);
1017         Fi += FZ*FZ;
1018         if (Fi > MaxE3d) MaxE3d = Fi;
1019       }
1020       else {
1021         if (Fi > MaxE2d) MaxE2d = Fi;
1022       }
1023       theError(i, k) = Fi;
1024       F += Fi;
1025     }
1026     if (k <= nbP) i2 += 3;
1027     else i2 += 2;
1028   }
1029   MaxE3d = Sqrt(MaxE3d);
1030   MaxE2d = Sqrt(MaxE2d);
1031 }
1032
1033
1034 void AppParCurves_LeastSquare::ErrorGradient(math_Vector& Grad, 
1035                                              Standard_Real& F, 
1036                                              Standard_Real& MaxE3d,
1037                                              Standard_Real& MaxE2d)
1038 {
1039   if (!done) {StdFail_NotDone::Raise();}
1040   Standard_Integer i, j, k, i2, indexdeb, indexfin;
1041   Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
1042 //  Standard_Real DAIJ, DAA, DBB, DCC, Gr, gr1= 0.0, gr2= 0.0;
1043   Standard_Real DAIJ, DAA, DBB, DCC, Gr;
1044   MaxE3d = MaxE2d = 0.0;
1045 //  Standard_Integer i21, i22, diff = (deg-1);
1046   Standard_Integer i21, i22;
1047   F = 0.0;
1048   i2 = 1;
1049   math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
1050
1051   for (k = Grad.Lower(); k <= Grad.Upper(); k++) Grad(k) = 0.0;
1052
1053   for (k = 1 ; k <= nbP+nbP2d; k++) {
1054     i21 = i2+1; i22 = i2+2;
1055     for (i = 1; i <= nbpoles; i++) {
1056       Px(i) = mypoles(i, i2);
1057       Py(i) = mypoles(i, i21);
1058       if (k <= nbP) Pz(i) = mypoles(i, i22);
1059     }
1060     for (i = FirstP; i <= LastP; i++) {
1061       AA = 0.0; BB = 0.0; CC = 0.0; DAA = 0.0; DBB = 0.0; DCC = 0.0;
1062       indexdeb = myindex(i) + 1;
1063       indexfin = indexdeb + deg;
1064       for (j = indexdeb; j <= indexfin; j++) {
1065         AIJ = A(i, j); DAIJ = DA(i, j);
1066         AA += AIJ*Px(j); DAA += DAIJ*Px(j);
1067         BB += AIJ*Py(j); DBB += DAIJ*Py(j);
1068         if (k <= nbP) {
1069           CC += AIJ*Pz(j);
1070           DCC += DAIJ*Pz(j);
1071         }
1072       }
1073       FX = AA-mypoints(i, i2);
1074       FY = BB-mypoints(i, i2+1);
1075       Fi = FX*FX + FY*FY;
1076       Gr = 2.0*(DAA*FX + DBB*FY);
1077
1078       if (k <= nbP) {
1079         FZ = CC-mypoints(i, i2+2);
1080         Fi += FZ*FZ;
1081         Gr += 2.0*DCC*FZ;
1082         if (Fi > MaxE3d) MaxE3d = Fi;
1083       }
1084       else {
1085         if (Fi > MaxE2d) MaxE2d = Fi;
1086       }
1087       theError(i, k) = Fi;
1088       Grad(i) += Gr;
1089       F += Fi;
1090     }
1091     if (k <= nbP) i2 += 3;
1092     else i2 += 2;
1093   }
1094   MaxE3d = Sqrt(MaxE3d);
1095   MaxE2d = Sqrt(MaxE2d);
1096
1097 }
1098
1099
1100 const math_Matrix& AppParCurves_LeastSquare::Distance()
1101 {
1102   if (!iscalculated) {
1103     for (Standard_Integer i = myfirstp; i <= mylastp; i++) {
1104       for (Standard_Integer  j = 1; j <= nbP+nbP2d; j++) {
1105         theError(i, j) = Sqrt(theError(i, j));
1106       }
1107     }
1108     iscalculated = Standard_True;
1109   }
1110   return theError;
1111 }
1112
1113
1114 Standard_Real AppParCurves_LeastSquare::FirstLambda() const
1115 {
1116   return lambda1;
1117 }
1118
1119 Standard_Real AppParCurves_LeastSquare::LastLambda() const
1120 {
1121   return lambda2;
1122 }
1123
1124
1125
1126 Standard_Boolean AppParCurves_LeastSquare::IsDone() const
1127 {
1128   return done;
1129 }
1130
1131
1132 AppParCurves_MultiCurve AppParCurves_LeastSquare::BezierValue() 
1133 {
1134   if (!myknots.IsNull()) Standard_NoSuchObject::Raise();
1135   return (AppParCurves_MultiCurve)(BSplineValue());
1136 }
1137
1138
1139 const AppParCurves_MultiBSpCurve& AppParCurves_LeastSquare::BSplineValue() 
1140 {
1141   if (!done) {StdFail_NotDone::Raise();}
1142
1143   Standard_Integer i, j, j2, npoints = nbP+nbP2d;;
1144   gp_Pnt Pt;
1145   gp_Pnt2d Pt2d;
1146   Standard_Integer ideb = resinit, ifin = resfin;
1147   if (ideb >= 2) ideb = 2;
1148   if (ifin <= nbpoles-1) ifin = nbpoles-1;
1149
1150   // On met le resultat dans les curves correspondantes
1151   for (i = ideb; i <= ifin; i++) {
1152     j2 = 1;
1153     AppParCurves_MultiPoint MPole(nbP, nbP2d);
1154     for (j = 1; j <= nbP; j++) {
1155       Pt.SetCoord(mypoles(i, j2), mypoles(i, j2+1), mypoles(i,j2+2));
1156       MPole.SetPoint(j, Pt);
1157       j2 += 3;
1158     }
1159     for (j = nbP+1;j <= npoints; j++) {
1160       Pt2d.SetCoord(mypoles(i, j2), mypoles(i, j2+1));
1161       MPole.SetPoint2d(j, Pt2d);
1162       j2 += 2;
1163     }
1164     SCU.SetValue(i, MPole);
1165   }
1166   return SCU;
1167 }
1168
1169
1170
1171 const math_Matrix& AppParCurves_LeastSquare::FunctionMatrix() const
1172 {
1173   if (!done) {StdFail_NotDone::Raise();}
1174   return A;
1175 }
1176
1177
1178 const math_Matrix& AppParCurves_LeastSquare::DerivativeFunctionMatrix() const
1179 {
1180   if (!done) {StdFail_NotDone::Raise();}
1181   return DA;
1182 }
1183
1184
1185
1186
1187 Standard_Integer AppParCurves_LeastSquare::
1188                  TheFirstPoint(const AppParCurves_Constraint FirstCons, 
1189                                const Standard_Integer        FirstPoint) const
1190 {
1191   if (FirstCons == AppParCurves_NoConstraint) 
1192     return FirstPoint;
1193   else 
1194     return FirstPoint + 1;
1195 }
1196
1197
1198
1199 Standard_Integer AppParCurves_LeastSquare::
1200                  TheLastPoint(const AppParCurves_Constraint LastCons,
1201                               const Standard_Integer LastPoint) const
1202 {
1203   if (LastCons == AppParCurves_NoConstraint)
1204     return LastPoint;
1205   else 
1206     return LastPoint - 1;
1207 }
1208
1209
1210 void AppParCurves_LeastSquare::ComputeFunction(const math_Vector& Parameters) 
1211 {
1212   if (myknots.IsNull()) {
1213     AppParCurves::Bernstein(nbpoles, Parameters, A, DA);  
1214   }
1215   else {
1216     AppParCurves::SplineFunction(nbpoles, deg, Parameters,
1217                                  Vflatknots, A, DA, myindex);
1218   }
1219 }
1220
1221
1222
1223 const math_Matrix& AppParCurves_LeastSquare::Points() const
1224 {
1225   return mypoints;
1226 }
1227
1228 const math_Matrix& AppParCurves_LeastSquare::Poles() const
1229 {
1230   return mypoles;
1231 }
1232
1233
1234
1235 const math_IntegerVector& AppParCurves_LeastSquare::KIndex() const
1236 {
1237   return myindex;
1238 }
1239
1240
1241
1242
1243 void AppParCurves_LeastSquare::MakeTAA(math_Vector& TheA,
1244                                        math_Vector& myTAB)
1245 {
1246   Standard_Integer i, j, k, Ci, i2, i21, i22;
1247   Standard_Real xx = 0.0, yy = 0.0;
1248
1249   Standard_Integer Nincx = resfin-resinit+1;
1250   Standard_Integer Nrow, Neq = LastP-FirstP+1;
1251
1252   Standard_Integer Ninc1;
1253
1254   if (FirstConstraint >= AppParCurves_TangencyPoint &&
1255       LastConstraint >= AppParCurves_TangencyPoint)  {
1256     Ninc1 = Ninc-1;
1257   }
1258   else Ninc1 = Ninc;
1259
1260   Standard_Integer myfirst = A.LowerRow();
1261   Standard_Integer ix, iy, iz;
1262   Standard_Integer mylast = myfirst+Nlignes-1;
1263   Standard_Integer k1;
1264   Standard_Real taf1 = 0.0, taf2 = 0.0, taf3 = 0.0, 
1265   tab1 = 0.0, tab2 = 0.0;
1266   Standard_Integer nb, inc, jinit, jfin, u;
1267   Standard_Integer indexdeb, indexfin;
1268   Standard_Integer NA1 = NA-1;
1269   Standard_Real v1=0, v2=0, b;
1270   Standard_Real Ai2, Aid, Akj;
1271   math_Vector  myB(myfirst, mylast, 0.0),
1272                myV1(myfirst, mylast, 0.0), 
1273                myV2(myfirst, mylast, 0.0);
1274   math_Vector TheV1(1, Ninc, 0.0), TheV2(1, Ninc, 0.0);
1275
1276
1277   for (i = FirstP; i <= LastP; i++) {
1278     Ai2 = A(i, 2);
1279     Aid = A(i, nbpoles-1);
1280     if (FirstConstraint >= AppParCurves_PassPoint) xx = A(i, 1);
1281     if (FirstConstraint >= AppParCurves_TangencyPoint) xx += Ai2;
1282     if (LastConstraint >= AppParCurves_PassPoint) yy = A(i, nbpoles);
1283     if (LastConstraint >= AppParCurves_TangencyPoint) yy += Aid;
1284     i2 = 1;
1285     Nrow = myfirst-FirstP;
1286     for (Ci = 1; Ci <= nbP; Ci++) {
1287       i21 = i2+1; i22 = i2+2;
1288       ix = i+Nrow; iy = ix+Neq; iz = iy+Neq;
1289       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1290         myV1(ix) =  Ai2*Vec1t(i2);
1291         myV1(iy) =  Ai2*Vec1t(i21);
1292         myV1(iz) =  Ai2*Vec1t(i22);
1293       }
1294       if (LastConstraint >= AppParCurves_TangencyPoint) {
1295         myV2(ix) = -Aid*Vec2t(i2);
1296         myV2(iy) = -Aid*Vec2t(i21);
1297         myV2(iz) = -Aid*Vec2t(i22);
1298       }
1299       myB(ix)  = mypoints(i, i2)  - xx*mypoints(myfirstp, i2) 
1300                                   - yy*mypoints(mylastp,  i2);
1301       myB(iy)  = mypoints(i, i21) - xx*mypoints(myfirstp, i21) 
1302                                   - yy*mypoints(mylastp,  i21);
1303       myB(iz)  = mypoints(i, i22) - xx*mypoints(myfirstp, i22) 
1304                                   - yy*mypoints(mylastp,  i22);
1305       i2 += 3;
1306       Nrow = Nrow + 3*Neq;
1307     }   
1308     
1309     for (Ci = 1; Ci <= nbP2d; Ci++) {
1310       i21 = i2+1; i22 = i2+2;
1311       ix = i+Nrow; iy = ix+Neq;
1312       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1313         myV1(ix) =  Ai2*Vec1t(i2);
1314         myV1(iy) =  Ai2*Vec1t(i21);
1315       }
1316       if (LastConstraint >= AppParCurves_TangencyPoint) {
1317         myV2(ix) = -Aid*Vec2t(i2);
1318         myV2(iy) = -Aid*Vec2t(i21);
1319       }
1320       myB(ix)  = mypoints(i, i2)  - xx*mypoints(myfirstp, i2) 
1321                                   - yy*mypoints(mylastp,  i2);
1322       myB(iy)  = mypoints(i, i21) - xx*mypoints(myfirstp, i21) 
1323                                   - yy*mypoints(mylastp,  i21);
1324       Nrow = Nrow + 2*Neq;
1325       i2 += 2;
1326     }
1327   } 
1328   
1329   
1330   
1331   // Construction de TA*A et TA*B:
1332   
1333   for (k = FirstP; k <= LastP; k++) {
1334     indexdeb = myindex(k)+1;
1335     indexfin = indexdeb + deg; 
1336     jinit = Max(resinit, indexdeb);
1337     jfin = Min(resfin, indexfin);
1338     k1 = k + myfirst - FirstP;
1339     for (i = 0; i <= NA1; i++) {
1340       nb = i*Neq + k1;
1341       if (FirstConstraint >= AppParCurves_TangencyPoint)
1342         v1 = myV1(nb); 
1343       if (LastConstraint >= AppParCurves_TangencyPoint)
1344         v2 = myV2(nb); 
1345       b = myB(nb);
1346       inc = i*Nincx-resinit+1;
1347       for (j = jinit; j <= jfin; j++) {
1348         Akj = A(k, j);
1349         u = j+inc;
1350         if (FirstConstraint >= AppParCurves_TangencyPoint)
1351           TheV1(u) += Akj*v1;
1352         if (LastConstraint >= AppParCurves_TangencyPoint)
1353           TheV2(u) += Akj*v2;
1354         myTAB(u) += Akj*b;
1355       }
1356       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1357         taf1 += v1*v1; 
1358         tab1 += v1*b;
1359       }
1360       if (LastConstraint >= AppParCurves_TangencyPoint) {
1361         taf2 += v2*v2;
1362         tab2 += v2*b;
1363       }
1364       if (FirstConstraint >= AppParCurves_TangencyPoint && 
1365           LastConstraint >= AppParCurves_TangencyPoint) {
1366         taf3 += v1*v2;
1367       }
1368     }
1369   }
1370   
1371
1372   if (FirstConstraint >= AppParCurves_TangencyPoint) {
1373     TheV1(Ninc1) = taf1;
1374     myTAB(Ninc1) = tab1;
1375   }
1376   if (LastConstraint >= AppParCurves_TangencyPoint) {
1377     TheV2(Ninc)  = taf2;
1378     myTAB(Ninc)  = tab2;
1379   }
1380   if (FirstConstraint >= AppParCurves_TangencyPoint && 
1381       LastConstraint >= AppParCurves_TangencyPoint) {
1382     TheV2(Ninc1) = taf3;
1383   }
1384
1385   
1386   if (resinit <= resfin) {
1387     math_IntegerVector Index(1, Nincx);
1388     SearchIndex(Index);
1389     math_Vector AA(1, Index(Nincx));
1390     MakeTAA(AA);
1391     
1392     Standard_Integer kk = 1;
1393     for (k = 1; k <= NA; k++)   {
1394       for(i = 1; i <= AA.Length(); i++) {
1395         TheA(kk) = AA(i);
1396         kk++;
1397       }
1398     }
1399   }
1400   
1401   Standard_Integer length = TheA.Length();
1402   
1403   if (FirstConstraint >= AppParCurves_TangencyPoint && 
1404       LastConstraint >= AppParCurves_TangencyPoint) {
1405     for (j = 1; j <= Ninc1; j++) 
1406       TheA(length- 2*Ninc+j+1) = TheV1(j);
1407     for (j = 1; j <= Ninc; j++) 
1408       TheA(length- Ninc+j) = TheV2(j);
1409   }
1410
1411
1412   else if (FirstConstraint >= AppParCurves_TangencyPoint) {
1413     for (j = 1; j <= Ninc; j++) 
1414       TheA(length- Ninc+j) = TheV1(j);
1415   }
1416
1417   else if (LastConstraint >= AppParCurves_TangencyPoint) {
1418     for (j = 1; j <= Ninc; j++) 
1419       TheA(length- Ninc+j) = TheV2(j);
1420   }
1421 }
1422
1423
1424
1425
1426 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA,
1427                                        math_Matrix& myTAB)
1428 {
1429   Standard_Integer i, j, k;
1430   Standard_Integer indexdeb, indexfin, jinit, jfin;
1431   Standard_Integer iinit, ifin;
1432   Standard_Real Akj;
1433   math_Matrix TheA(resinit, resfin, resinit, resfin);
1434   TheA.Init(0.0);
1435
1436   for (k = FirstP ; k <= LastP; k++) {
1437     indexdeb = myindex(k)+1;
1438     indexfin = indexdeb + deg;
1439     jinit = Max(resinit, indexdeb);
1440     jfin = Min(resfin, indexfin);
1441     for (i = jinit; i <= jfin; i++) {
1442       Akj = A(k, i);
1443       for (j = jinit; j <= i; j++) {
1444         TheA(i, j) += A(k, j) * Akj;
1445       }
1446       for (j = 1; j <= B2.ColNumber(); j++) {
1447         myTAB(i, j) += Akj*B2(k, j);
1448       }
1449     }
1450   }
1451
1452   Standard_Integer len;
1453   if (!myknots.IsNull()) len = myknots->Length();
1454   else len = 2;
1455   Standard_Integer d, i2 = 1;
1456   iinit = resinit;
1457   jinit = resinit;
1458   ifin = Min(resfin, deg+1);
1459   for (k = 2; k <= len; k++) {
1460     for (i = iinit; i <= ifin; i++) {
1461       for (j = jinit; j <= i; j++) {
1462         AA(i2) = TheA(i, j);
1463         i2++;
1464       }
1465     }
1466     if (!mymults.IsNull()) {
1467       iinit = ifin+1;
1468       d = ifin + mymults->Value(k);
1469       ifin = Min(d, resfin);
1470       jinit = Max(resinit, d - deg);
1471     }
1472   }
1473 }
1474
1475
1476 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA)
1477 {
1478   Standard_Integer i, j, k;
1479   Standard_Integer indexdeb, indexfin, jinit, jfin;
1480   Standard_Integer iinit, ifin;
1481   Standard_Real Akj;
1482   math_Matrix TheA(resinit, resfin, resinit, resfin, 0.0);
1483
1484
1485   for (k = FirstP ; k <= LastP; k++) {
1486     indexdeb = myindex(k)+1;
1487     indexfin = indexdeb + deg;
1488     jinit = Max(resinit, indexdeb);
1489     jfin = Min(resfin, indexfin);
1490     for (i = jinit; i <= jfin; i++) {
1491       Akj = A(k, i);
1492       for (j = jinit; j <= i; j++) {
1493         TheA(i, j) += A(k, j) * Akj;
1494       }
1495     }
1496   }
1497
1498   
1499   Standard_Integer i2 = 1;
1500   iinit = resinit;
1501   jinit = resinit;
1502   ifin = Min(resfin, deg+1);
1503   Standard_Integer len, d;
1504   if (!myknots.IsNull()) len = myknots->Length();
1505   else len = 2;
1506   for (k = 2; k <= len; k++) {
1507     for (i = iinit; i <= ifin; i++) {
1508       for (j = jinit; j <= i; j++) {
1509         AA(i2) = TheA(i, j);
1510         i2++;
1511       }
1512     }
1513     if (!mymults.IsNull()) {
1514       iinit = ifin+1;
1515       d = ifin + mymults->Value(k);
1516       ifin = Min(d, resfin);
1517       jinit = Max(resinit, d - deg);
1518     }
1519   }
1520   
1521 }
1522
1523
1524
1525 void AppParCurves_LeastSquare::SearchIndex(math_IntegerVector& Index)
1526 {
1527   Standard_Integer iinit, jinit, ifin;
1528   Standard_Integer i, j, k, d, i2 = 1;
1529   Standard_Integer len;
1530   Standard_Integer Nincx = resfin-resinit+1;
1531   Index(1) = 1;
1532
1533   if (myknots.IsNull()) {
1534     if (resinit <= resfin) {
1535       Standard_Integer l = 1;
1536       for (i = 2; i <= Nincx; i++) {
1537         l++;
1538         Index(l) = Index(l-1)+i;
1539       }
1540     }
1541     
1542   }
1543   else {
1544     iinit = resinit;
1545     jinit = resinit;
1546     ifin = Min(resfin, deg+1);
1547     len = myknots->Length();
1548     
1549     for (k = 2; k <= len; k++) {
1550       for (i = iinit; i <= ifin; i++) {
1551         for (j = jinit; j <= i; j++) {
1552           if (i2 != 1) 
1553             Index(i2) = Index(i2-1) + i-jinit+1;
1554         }
1555         i2++;
1556       }
1557       iinit = ifin+1;
1558       d = ifin + mymults->Value(k);
1559       ifin = Min(d, resfin);
1560       jinit = Max(resinit, d - deg);
1561     }
1562   }
1563 }