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
   7 // under the terms of the GNU Lesser General Public 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, Neq, 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     Standard_Integer Error = 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       Error = 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   Neq = LastP-FirstP+1;
 528   Standard_Integer deport = 0, Nincx2 = 2*Nincx;
 529
 530   math_IntegerVector InternalIndex(1, Nincx);
 531   SearchIndex(InternalIndex);
 532   math_IntegerVector Index(1, Ninc);
 533
 534   Standard_Integer l = 1;
 535   if (resinit <= resfin) {
 536     for (j = 0; j <= NA-1; j++) {
 537       deport = j*InternalIndex(Nincx);
 538       for (i = 1; i <= Nincx; i++) {
 539         Index(l) = InternalIndex(i) + deport;
 540         l++;
 541       }
 542     }
 543   }
 544
 545   if (resinit > resfin) Index(1) = 1;
 546   if (Ninc1 > 1) {
 547     if (FirstConstraint >= AppParCurves_TangencyPoint &&
 548         LastConstraint >= AppParCurves_TangencyPoint)
 549       Index(Ninc1) = Index(Ninc1 - 1) + Ninc1;
 550   }
 551   if (FirstConstraint >= AppParCurves_TangencyPoint ||
 552       LastConstraint >= AppParCurves_TangencyPoint)
 553     Index(Ninc) = Index(Ninc-1) + Ninc;
 554
 555
 556   math_Vector TheA(1, Index(Ninc), 0.0);
 557   math_Vector myTAB(1, Ninc, 0.0);
 558
 559   MakeTAA(TheA, myTAB);
 560
 561   Standard_Integer Error = DACTCL_Decompose(TheA, Index);
 562   Error = DACTCL_Solve(TheA, myTAB, Index);
 563
 564   if (!Error) done = Standard_True;
 565
 566   if (FirstConstraint >= AppParCurves_TangencyPoint &&
 567       LastConstraint >= AppParCurves_TangencyPoint) {
 568     lambda1 = myTAB.Value(Ninc1);
 569     lambda2 = myTAB.Value(Ninc);
 570   }
 571   else if (FirstConstraint >= AppParCurves_TangencyPoint)
 572     lambda1 = myTAB.Value(Ninc);
 573   else if (LastConstraint >= AppParCurves_TangencyPoint)
 574     lambda2 = myTAB.Value(Ninc);
 575
 576
 577
 578   // Les resultats sont stockes dans mypoles.
 579   //=========================================
 580   k = 1;
 581   i2 = 1;
 582   for (Ci = 1; Ci <= nbP; Ci++) {
 583     k1 = k+1; k2 = k+2;
 584     for (j = resinit; j <= resfin; j++) {
 585       mypoles(j, k)  = myTAB.Value(i2);
 586       mypoles(j, k1) = myTAB.Value(i2+Nincx);
 587       mypoles(j, k2) = myTAB.Value(i2+Nincx2);
 588       i2++;
 589     }
 590
 591     if (FirstConstraint >= AppParCurves_TangencyPoint) {
 592       mypoles(2, k)        = mypoints(myfirstp, k)   + lambda1*Vec1t(k);
 593       mypoles(2, k1)       = mypoints(myfirstp, k1)  + lambda1*Vec1t(k1);
 594       mypoles(2, k2)       = mypoints(myfirstp, k2)  + lambda1*Vec1t(k2);
 595     }
 596     if (LastConstraint >= AppParCurves_TangencyPoint) {
 597       mypoles(nbpol1, k)   = mypoints(mylastp,  k)   - lambda2*Vec2t(k);
 598       mypoles(nbpol1, k1)  = mypoints(mylastp,  k1)  - lambda2*Vec2t(k1);
 599       mypoles(nbpol1, k2)  = mypoints(mylastp,  k2)  - lambda2*Vec2t(k2);
 600     }
 601     k += 3; i2 += Nincx2;
 602   }
 603
 604   for (Ci = 1; Ci <= nbP2d; Ci++) {
 605     k1 = k+1; k2 = k+2;
 606     for (j = resinit; j <= resfin; j++) {
 607       mypoles(j, k)  = myTAB.Value(i2);
 608       mypoles(j, k1) = myTAB.Value(i2+Nincx);
 609       i2++;
 610     }
 611     if (FirstConstraint >= AppParCurves_TangencyPoint) {
 612       mypoles(2, k)       = mypoints(myfirstp, k)  + lambda1*Vec1t(k);
 613       mypoles(2, k1)      = mypoints(myfirstp, k1) + lambda1*Vec1t(k1);
 614     }
 615     if (LastConstraint >= AppParCurves_TangencyPoint) {
 616       mypoles(nbpol1, k)  = mypoints(mylastp,  k)  - lambda2*Vec2t(k);
 617       mypoles(nbpol1, k1) = mypoints(mylastp,  k1) - lambda2*Vec2t(k1);
 618     }
 619     k += 2; i2 += Nincx;
 620   }
 621
 622 }
 623
 624 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
 625                                        const math_Vector&  V1t,
 626                                        const math_Vector&  V2t,
 627                                        const Standard_Real l1,
 628                                        const Standard_Real l2)
 629 {
 630   done = Standard_False;
 631   if (!isready) {
 632     return;
 633   }
 634   Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
 635   resinit = 3; resfin = nbpoles-2;
 636   Standard_Integer Nincx = resfin-resinit+1;
 637   Ninc = NA*Nincx + 2;
 638   FirstConstraint = AppParCurves_TangencyPoint;
 639   LastConstraint = AppParCurves_TangencyPoint;
 640   for (i = 1; i <= Vec1t.Upper(); i++) {
 641     Vec1t(i) = V1t(i+lower1-1);
 642     Vec2t(i) = V2t(i+lower2-1);
 643   }
 644   Perform (Parameters, l1, l2);
 645 }
 646
 647
 648 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
 649                                        const math_Vector&  V1t,
 650                                        const math_Vector&  V2t,
 651                                        const math_Vector&  V1c,
 652                                        const math_Vector&  V2c,
 653                                        const Standard_Real l1,
 654                                        const Standard_Real l2) {
 655   done = Standard_False;
 656   if (!isready) {
 657     return;
 658   }
 659   Standard_Integer i, lower1 = V1t.Lower(), lower2 = V2t.Lower();
 660   Standard_Integer lowc1 = V1c.Lower(), lowc2 = V2c.Lower();
 661   resinit = 4; resfin = nbpoles-3;
 662   Standard_Integer Nincx = resfin-resinit+1;
 663   Ninc = NA*Nincx + 2;
 664   FirstConstraint = AppParCurves_CurvaturePoint;
 665   LastConstraint = AppParCurves_CurvaturePoint;
 666
 667   for (i = 1; i <= Vec1t.Upper(); i++) {
 668     Vec1t(i) = V1t(i+lower1-1);
 669     Vec2t(i) = V2t(i+lower2-1);
 670     Vec1c(i) = V1c(i+lowc1-1);
 671     Vec2c(i) = V2c(i+lowc2-1);
 672   }
 673   Perform (Parameters, l1, l2);
 674 }
 675
 676
 677
 678 void AppParCurves_LeastSquare::Perform(const math_Vector&  Parameters,
 679                                        const Standard_Real l1,
 680                                        const Standard_Real l2) {
 681   done = Standard_False;
 682   if (!isready) {
 683     return;
 684   }
 685   if (FirstConstraint < AppParCurves_TangencyPoint &&
 686       LastConstraint  < AppParCurves_TangencyPoint) {
 687     Perform(Parameters);
 688     return;
 689   }
 690   iscalculated = Standard_False;
 691
 692   lambda1 = l1;
 693   lambda2 = l2;
 694   Standard_Integer i, j, k, i2;
 695   Standard_Real AD0, AD1, AD2, A0, A1, A2;
 696 //  gp_Pnt Pt, P1, P2;
 697 //  gp_Pnt2d Pt2d, P12d, P22d;
 698   Standard_Real l11 = deg*l1, l22 = deg*l2;
 699
 700   ComputeFunction(Parameters);
 701
 702   if (FirstConstraint >= AppParCurves_TangencyPoint) {
 703     for (i = 1; i <= mypoles.ColNumber(); i++)
 704       mypoles(2, i) = mypoints(myfirstp, i) + l1*Vec1t(i);
 705   }
 706
 707
 708   if (FirstConstraint == AppParCurves_CurvaturePoint) {
 709     for (i = 1; i <= mypoles.ColNumber(); i++)
 710       mypoles(3, i) = 2*mypoles(2, i)-mypoles(1, i)
 711                   + l11*l11*Vec1c(i)/(deg*(deg-1));
 712   }
 713
 714
 715   if (LastConstraint >= AppParCurves_TangencyPoint) {
 716     for (i = 1; i <= mypoles.ColNumber(); i++)
 717       mypoles(nbpoles-1, i) = mypoints(mylastp, i) - l2*Vec2t(i);
 718   }
 719
 720
 721   if (LastConstraint == AppParCurves_CurvaturePoint) {
 722     for (i = 1; i <= mypoles.ColNumber(); i++)
 723       mypoles(nbpoles-2, i) = 2*mypoles(nbpoles-1, i) - mypoles(nbpoles, i)
 724                           + l22*l22*Vec2c(i)/(deg*(deg-1));
 725   }
 726
 727   if (resinit > resfin) {
 728     done = Standard_True;
 729     return;
 730   }
 731
 732   if (FirstConstraint == AppParCurves_NoConstraint) {
 733     if (LastConstraint == AppParCurves_TangencyPoint) {
 734       for (j = FirstP; j <= LastP; j++) {
 735         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1);
 736         for (i = 1; i <= B2.ColNumber(); i++) {
 737           B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
 738                                 - AD1 * mypoles(nbpoles-1, i);
 739         }
 740       }
 741     }
 742     if (LastConstraint == AppParCurves_CurvaturePoint) {
 743       for (j = FirstP; j <= LastP; j++) {
 744         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
 745         for (i = 1; i <= B2.ColNumber(); i++) {
 746           B2(j, i) = mypoints(j, i) - AD0 * mypoles(nbpoles, i)
 747                                 - AD1 * mypoles(nbpoles-1, i)
 748                                 - AD2 * mypoles(nbpoles-2, i);
 749         }
 750       }
 751     }
 752   }
 753   else if (FirstConstraint == AppParCurves_PassPoint) {
 754     if (LastConstraint == AppParCurves_TangencyPoint) {
 755       for (j = FirstP; j <= LastP; j++) {
 756         A0 = A(j, 1);
 757         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1);
 758         for (i = 1; i <= B2.ColNumber(); i++) {
 759           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 760                                 - AD0 * mypoles(nbpoles, i)
 761                                 - AD1 * mypoles(nbpoles-1, i);
 762         }
 763       }
 764     }
 765     if (LastConstraint == AppParCurves_CurvaturePoint) {
 766       for (j = FirstP; j <= LastP; j++) {
 767         A0 = A(j, 1);
 768         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
 769         for (i = 1; i <= B2.ColNumber(); i++) {
 770           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 771                                 - AD0 * mypoles(nbpoles, i)
 772                                 - AD1 * mypoles(nbpoles-1, i)
 773                                 - AD2 * mypoles(nbpoles-2, i);
 774         }
 775       }
 776     }
 777   }
 778   else if (FirstConstraint == AppParCurves_TangencyPoint) {
 779     if (LastConstraint == AppParCurves_NoConstraint) {
 780       for (j = FirstP; j <= LastP; j++) {
 781         A0 = A(j, 1); A1 = A(j, 2);
 782         for (i = 1; i <= B2.ColNumber(); i++) {
 783           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 784                                     - A1  * mypoles(2, i);
 785         }
 786       }
 787     }
 788     else if (LastConstraint == AppParCurves_PassPoint) {
 789       for (j = FirstP; j <= LastP; j++) {
 790         A0 = A(j, 1);  AD0 = A(j, nbpoles); A1 = A(j, 2);
 791         for (i = 1; i <= B2.ColNumber(); i++) {
 792           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 793                                     - AD0 * mypoles(nbpoles, i)
 794                                     - A1  * mypoles(2, i);
 795         }
 796       }
 797     }
 798     else if (LastConstraint == AppParCurves_TangencyPoint) {
 799       for (j = FirstP; j <= LastP; j++) {
 800         A0 = A(j, 1);  AD0 = A(j, nbpoles); A1 = A(j, 2); AD1 = A(j, nbpoles-1);
 801         for (i = 1; i <= B2.ColNumber(); i++) {
 802           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 803                                     - AD0 * mypoles(nbpoles, i)
 804                                     - A1  * mypoles(2, i)
 805                                     - AD1 * mypoles(nbpoles-1, i);
 806         }
 807       }
 808     }
 809   }
 810   else if (FirstConstraint == AppParCurves_CurvaturePoint) {
 811     if (LastConstraint == AppParCurves_NoConstraint) {
 812       for (j = FirstP; j <= LastP; j++) {
 813         A0 = A(j, 1);  A1 = A(j, 2);  A2 = A(j, 3);
 814         for (i = 1; i <= B2.ColNumber(); i++) {
 815           B2(j, i) = mypoints(j, i) - A0 * mypoles(1, i)
 816                                 - A1 * mypoles(2, i)
 817                                 - A2 * mypoles(3, i);
 818         }
 819       }
 820     }
 821     else if (LastConstraint == AppParCurves_PassPoint) {
 822       for (j = FirstP; j <= LastP; j++) {
 823         A0 = A(j, 1);  A1 = A(j, 2);  A2 = A(j, 3);  AD0 = A(j, nbpoles);
 824         for (i = 1; i <= B2.ColNumber(); i++) {
 825           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 826                                 - A1  * mypoles(2, i)
 827                                 - A2  * mypoles(3, i)
 828                                 - AD0 * mypoles(nbpoles, i);
 829         }
 830       }
 831     }
 832     else if (LastConstraint == AppParCurves_TangencyPoint) {
 833       for (j = FirstP; j <= LastP; j++) {
 834         A0 = A(j, 1);     A1 = A(j, 2);   A2 = A(j, 3);
 835         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1);
 836         for (i = 1; i <= B2.ColNumber(); i++) {
 837           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 838                                 - A1  * mypoles(2, i)
 839                                 - A2  * mypoles(3, i)
 840                                 - AD0 * mypoles(nbpoles, i)
 841                                 - AD1 * mypoles(nbpoles-1, i);
 842         }
 843       }
 844     }
 845     else if (LastConstraint == AppParCurves_CurvaturePoint) {
 846       for (j = FirstP; j <= LastP; j++) {
 847         A0 = A(j, 1);     A1 = A(j, 2);   A2 = A(j, 3);
 848         AD0 = A(j, nbpoles);  AD1 = A(j, nbpoles-1); AD2 = A(j, nbpoles-2);
 849         for (i = 1; i <= B2.ColNumber(); i++) {
 850           B2(j, i) = mypoints(j, i) - A0  * mypoles(1, i)
 851                                 - A1  * mypoles(2, i)
 852                                 - A2  * mypoles(3, i)
 853                                 - AD0 * mypoles(nbpoles, i)
 854                                 - AD1 * mypoles(nbpoles-1, i)
 855                                 - AD2 * mypoles(nbpoles-2, i);
 856         }
 857       }
 858     }
 859   }
 860
 861   Standard_Integer Nincx = resfin-resinit+1;
 862
 863   math_Matrix mytab(resinit, resfin, 1, B2.ColNumber(),0.0);
 864   math_IntegerVector Index(1, Nincx);
 865   SearchIndex(Index);
 866   math_Vector AA(1, Index(Nincx), 0.0);
 867   MakeTAA(AA, mytab);
 868
 869   math_Vector myTABB(1, Nincx, 0.0);
 870
 871   Standard_Integer Error = DACTCL_Decompose(AA, Index);
 872
 873   Standard_Integer kk2;
 874   for (j = 1; j <= B2.ColNumber(); j++) {
 875     kk2 = 1;
 876     for (i = resinit; i <= resfin; i++) {
 877       myTABB(kk2) = mytab(i, j);
 878       kk2++;
 879     }
 880
 881     Error = DACTCL_Solve(AA, myTABB, Index);
 882
 883     i2 = 1;
 884     for (k = resinit; k <= resfin; k++) {
 885       mypoles(k, j)  = myTABB.Value(i2);
 886       i2++;
 887     }
 888
 889   }
 890
 891   done = Standard_True;
 892
 893 }
 894
 895
 896
 897
 898 void AppParCurves_LeastSquare::Affect(const MultiLine& SSP,
 899                                       const Standard_Integer Index,
 900                                       AppParCurves_Constraint& Cons,
 901                                       math_Vector& Vt,
 902                                       math_Vector& Vc)
 903 {
 904   // Vt: vecteur tangent, Vc: vecteur courbure.
 905
 906   if (Cons >= AppParCurves_TangencyPoint) {
 907     Standard_Integer i, i2 = 1;
 908     Standard_Boolean Ok;
 909     Standard_Integer mynbP2d = nbP2d, mynbP = nbP;
 910     if (nbP2d == 0) mynbP2d = 1;
 911     if (nbP == 0) mynbP = 1;
 912     TColgp_Array1OfPnt TabP(1, mynbP);
 913     TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
 914     TColgp_Array1OfVec TabV(1, mynbP);
 915     TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
 916
 917     if (Cons == AppParCurves_CurvaturePoint) {
 918       if (nbP != 0 && nbP2d != 0) {
 919         Ok = ToolLine::Curvature(SSP, Index,TabV,TabV2d);
 920         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
 921       }
 922       else if (nbP2d != 0) {
 923         Ok = ToolLine::Curvature(SSP, Index, TabV2d);
 924         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
 925       }
 926       else {
 927         Ok = ToolLine::Curvature(SSP, Index, TabV);
 928         if (!Ok) { Cons = AppParCurves_TangencyPoint;}
 929       }
 930       if (Ok) {
 931         for (i = 1; i <= nbP; i++) {
 932           (TabV(i)).Coord(Vc(i2), Vc(i2+1), Vc(i2+2));
 933           i2 += 3;
 934         }
 935         for (i = 1; i <= nbP2d; i++) {
 936           (TabV2d(i)).Coord(Vc(i2), Vc(i2+1));
 937           i2 += 2;
 938         }
 939       }
 940     }
 941
 942     i2 = 1;
 943     if (Cons >= AppParCurves_TangencyPoint) {
 944       if (nbP != 0 && nbP2d != 0) {
 945         Ok = ToolLine::Tangency(SSP, Index, TabV, TabV2d);
 946         if (!Ok) { Cons = AppParCurves_PassPoint;}
 947       }
 948       else if (nbP2d != 0) {
 949         Ok = ToolLine::Tangency(SSP, Index, TabV2d);
 950         if (!Ok) { Cons = AppParCurves_PassPoint;}
 951       }
 952       else {
 953         Ok = ToolLine::Tangency(SSP, Index, TabV);
 954         if (!Ok) { Cons = AppParCurves_PassPoint;}
 955       }
 956       if (Ok) {
 957         for (i = 1; i <= nbP; i++) {
 958           (TabV(i)).Coord(Vt(i2), Vt(i2+1), Vt(i2+2));
 959           i2 += 3;
 960         }
 961         for (i = 1; i <= nbP2d; i++) {
 962           (TabV2d(i)).Coord(Vt(i2), Vt(i2+1));
 963           i2 += 2;
 964         }
 965       }
 966     }
 967   }
 968 }
 969
 970
 971
 972 Standard_Integer AppParCurves_LeastSquare::NbBColumns(
 973                                      const MultiLine& SSP) const
 974 {
 975   Standard_Integer BCol;
 976   BCol = (ToolLine::NbP3d(SSP))*3 +
 977          (ToolLine::NbP2d(SSP))*2;
 978   return BCol;
 979 }
 980
 981
 982 void AppParCurves_LeastSquare::Error(Standard_Real& F,
 983                                      Standard_Real& MaxE3d,
 984                                      Standard_Real& MaxE2d)
 985 {
 986
 987   if (!done) {StdFail_NotDone::Raise();}
 988   Standard_Integer i, j, k, i2, indexdeb, indexfin;
 989   Standard_Integer i21, i22;
 990   Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
 991   MaxE3d = MaxE2d = 0.0;
 992   F = 0.0;
 993   i2 = 1;
 994   math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
 995   Standard_Integer l;
 996
 997   for (k = 1 ; k <= nbP+nbP2d; k++) {
 998     i21 = i2+1; i22 = i2+2;
 999     for (i = 1; i <= nbpoles; i++) {
1000       Px(i) = mypoles(i, i2);
1001       Py(i) = mypoles(i, i21);
1002       if (k <= nbP) Pz(i) = mypoles(i, i22);
1003     }
1004     for (i = FirstP; i <= LastP; i++) {
1005       AA = 0.0; BB = 0.0; CC = 0.0;
1006       indexdeb = myindex(i) + 1;
1007       indexfin = indexdeb + deg;
1008       l = (i-1)*(deg+1)-indexdeb+1;
1009       for (j = indexdeb; j <= indexfin; j++) {
1010         AIJ = A(i, j);
1011         AA += AIJ*Px(j);
1012         BB += AIJ*Py(j);
1013         if (k <= nbP) CC += AIJ*Pz(j);
1014       }
1015       FX = AA-mypoints(i, i2);
1016       FY = BB-mypoints(i, i21);
1017       Fi= FX*FX + FY*FY;
1018       if (k <= nbP) {
1019         FZ = CC-mypoints(i, i22);
1020         Fi += FZ*FZ;
1021         if (Fi > MaxE3d) MaxE3d = Fi;
1022       }
1023       else {
1024         if (Fi > MaxE2d) MaxE2d = Fi;
1025       }
1026       theError(i, k) = Fi;
1027       F += Fi;
1028     }
1029     if (k <= nbP) i2 += 3;
1030     else i2 += 2;
1031   }
1032   MaxE3d = Sqrt(MaxE3d);
1033   MaxE2d = Sqrt(MaxE2d);
1034 }
1035
1036
1037 void AppParCurves_LeastSquare::ErrorGradient(math_Vector& Grad,
1038                                              Standard_Real& F,
1039                                              Standard_Real& MaxE3d,
1040                                              Standard_Real& MaxE2d)
1041 {
1042   if (!done) {StdFail_NotDone::Raise();}
1043   Standard_Integer i, j, k, i2, indexdeb, indexfin;
1044   Standard_Real AA, BB, CC, Fi, FX, FY, FZ, AIJ;
1045 //  Standard_Real DAIJ, DAA, DBB, DCC, Gr, gr1= 0.0, gr2= 0.0;
1046   Standard_Real DAIJ, DAA, DBB, DCC, Gr;
1047   MaxE3d = MaxE2d = 0.0;
1048 //  Standard_Integer i21, i22, diff = (deg-1);
1049   Standard_Integer i21, i22;
1050   F = 0.0;
1051   i2 = 1;
1052   math_Vector Px(1, nbpoles), Py(1, nbpoles), Pz(1, nbpoles);
1053
1054   for (k = Grad.Lower(); k <= Grad.Upper(); k++) Grad(k) = 0.0;
1055
1056   for (k = 1 ; k <= nbP+nbP2d; k++) {
1057     i21 = i2+1; i22 = i2+2;
1058     for (i = 1; i <= nbpoles; i++) {
1059       Px(i) = mypoles(i, i2);
1060       Py(i) = mypoles(i, i21);
1061       if (k <= nbP) Pz(i) = mypoles(i, i22);
1062     }
1063     for (i = FirstP; i <= LastP; i++) {
1064       AA = 0.0; BB = 0.0; CC = 0.0; DAA = 0.0; DBB = 0.0; DCC = 0.0;
1065       indexdeb = myindex(i) + 1;
1066       indexfin = indexdeb + deg;
1067       for (j = indexdeb; j <= indexfin; j++) {
1068         AIJ = A(i, j); DAIJ = DA(i, j);
1069         AA += AIJ*Px(j); DAA += DAIJ*Px(j);
1070         BB += AIJ*Py(j); DBB += DAIJ*Py(j);
1071         if (k <= nbP) {
1072           CC += AIJ*Pz(j);
1073           DCC += DAIJ*Pz(j);
1074         }
1075       }
1076       FX = AA-mypoints(i, i2);
1077       FY = BB-mypoints(i, i2+1);
1078       Fi = FX*FX + FY*FY;
1079       Gr = 2.0*(DAA*FX + DBB*FY);
1080
1081       if (k <= nbP) {
1082         FZ = CC-mypoints(i, i2+2);
1083         Fi += FZ*FZ;
1084         Gr += 2.0*DCC*FZ;
1085         if (Fi > MaxE3d) MaxE3d = Fi;
1086       }
1087       else {
1088         if (Fi > MaxE2d) MaxE2d = Fi;
1089       }
1090       theError(i, k) = Fi;
1091       Grad(i) += Gr;
1092       F += Fi;
1093     }
1094     if (k <= nbP) i2 += 3;
1095     else i2 += 2;
1096   }
1097   MaxE3d = Sqrt(MaxE3d);
1098   MaxE2d = Sqrt(MaxE2d);
1099
1100 }
1101
1102
1103 const math_Matrix& AppParCurves_LeastSquare::Distance()
1104 {
1105   if (!iscalculated) {
1106     for (Standard_Integer i = myfirstp; i <= mylastp; i++) {
1107       for (Standard_Integer  j = 1; j <= nbP+nbP2d; j++) {
1108         theError(i, j) = Sqrt(theError(i, j));
1109       }
1110     }
1111     iscalculated = Standard_True;
1112   }
1113   return theError;
1114 }
1115
1116
1117 Standard_Real AppParCurves_LeastSquare::FirstLambda() const
1118 {
1119   return lambda1;
1120 }
1121
1122 Standard_Real AppParCurves_LeastSquare::LastLambda() const
1123 {
1124   return lambda2;
1125 }
1126
1127
1128
1129 Standard_Boolean AppParCurves_LeastSquare::IsDone() const
1130 {
1131   return done;
1132 }
1133
1134
1135 AppParCurves_MultiCurve AppParCurves_LeastSquare::BezierValue()
1136 {
1137   if (!myknots.IsNull()) Standard_NoSuchObject::Raise();
1138   return (AppParCurves_MultiCurve)(BSplineValue());
1139 }
1140
1141
1142 const AppParCurves_MultiBSpCurve& AppParCurves_LeastSquare::BSplineValue()
1143 {
1144   if (!done) {StdFail_NotDone::Raise();}
1145
1146   Standard_Integer i, j, j2, npoints = nbP+nbP2d;;
1147   gp_Pnt Pt;
1148   gp_Pnt2d Pt2d;
1149   Standard_Integer ideb = resinit, ifin = resfin;
1150   if (ideb >= 2) ideb = 2;
1151   if (ifin <= nbpoles-1) ifin = nbpoles-1;
1152
1153   // On met le resultat dans les curves correspondantes
1154   for (i = ideb; i <= ifin; i++) {
1155     j2 = 1;
1156     AppParCurves_MultiPoint MPole(nbP, nbP2d);
1157     for (j = 1; j <= nbP; j++) {
1158       Pt.SetCoord(mypoles(i, j2), mypoles(i, j2+1), mypoles(i,j2+2));
1159       MPole.SetPoint(j, Pt);
1160       j2 += 3;
1161     }
1162     for (j = nbP+1;j <= npoints; j++) {
1163       Pt2d.SetCoord(mypoles(i, j2), mypoles(i, j2+1));
1164       MPole.SetPoint2d(j, Pt2d);
1165       j2 += 2;
1166     }
1167     SCU.SetValue(i, MPole);
1168   }
1169   return SCU;
1170 }
1171
1172
1173
1174 const math_Matrix& AppParCurves_LeastSquare::FunctionMatrix() const
1175 {
1176   if (!done) {StdFail_NotDone::Raise();}
1177   return A;
1178 }
1179
1180
1181 const math_Matrix& AppParCurves_LeastSquare::DerivativeFunctionMatrix() const
1182 {
1183   if (!done) {StdFail_NotDone::Raise();}
1184   return DA;
1185 }
1186
1187
1188
1189
1190 Standard_Integer AppParCurves_LeastSquare::
1191                  TheFirstPoint(const AppParCurves_Constraint FirstCons,
1192                                const Standard_Integer        FirstPoint) const
1193 {
1194   if (FirstCons == AppParCurves_NoConstraint)
1195     return FirstPoint;
1196   else
1197     return FirstPoint + 1;
1198 }
1199
1200
1201
1202 Standard_Integer AppParCurves_LeastSquare::
1203                  TheLastPoint(const AppParCurves_Constraint LastCons,
1204                               const Standard_Integer LastPoint) const
1205 {
1206   if (LastCons == AppParCurves_NoConstraint)
1207     return LastPoint;
1208   else
1209     return LastPoint - 1;
1210 }
1211
1212
1213 void AppParCurves_LeastSquare::ComputeFunction(const math_Vector& Parameters)
1214 {
1215   if (myknots.IsNull()) {
1216     AppParCurves::Bernstein(nbpoles, Parameters, A, DA);
1217   }
1218   else {
1219     AppParCurves::SplineFunction(nbpoles, deg, Parameters,
1220                                  Vflatknots, A, DA, myindex);
1221   }
1222 }
1223
1224
1225
1226 const math_Matrix& AppParCurves_LeastSquare::Points() const
1227 {
1228   return mypoints;
1229 }
1230
1231 const math_Matrix& AppParCurves_LeastSquare::Poles() const
1232 {
1233   return mypoles;
1234 }
1235
1236
1237
1238 const math_IntegerVector& AppParCurves_LeastSquare::KIndex() const
1239 {
1240   return myindex;
1241 }
1242
1243
1244
1245
1246 void AppParCurves_LeastSquare::MakeTAA(math_Vector& TheA,
1247                                        math_Vector& myTAB)
1248 {
1249   Standard_Integer i, j, k, Ci, i2, i21, i22;
1250   Standard_Real xx = 0.0, yy = 0.0;
1251
1252   Standard_Integer Nincx = resfin-resinit+1;
1253   Standard_Integer Nrow, Neq = LastP-FirstP+1;
1254
1255   Standard_Integer Ninc1;
1256
1257   if (FirstConstraint >= AppParCurves_TangencyPoint &&
1258       LastConstraint >= AppParCurves_TangencyPoint)  {
1259     Ninc1 = Ninc-1;
1260   }
1261   else Ninc1 = Ninc;
1262
1263   Standard_Integer myfirst = A.LowerRow();
1264   Standard_Integer ix, iy, iz;
1265   Standard_Integer mylast = myfirst+Nlignes-1;
1266   Standard_Integer k1;
1267   Standard_Real taf1 = 0.0, taf2 = 0.0, taf3 = 0.0,
1268   tab1 = 0.0, tab2 = 0.0;
1269   Standard_Integer nb, inc, jinit, jfin, u;
1270   Standard_Integer indexdeb, indexfin;
1271   Standard_Integer NA1 = NA-1;
1272   Standard_Real v1=0, v2=0, b;
1273   Standard_Real Ai2, Aid, Akj;
1274   math_Vector  myB(myfirst, mylast, 0.0),
1275                myV1(myfirst, mylast, 0.0),
1276                myV2(myfirst, mylast, 0.0);
1277   math_Vector TheV1(1, Ninc, 0.0), TheV2(1, Ninc, 0.0);
1278
1279
1280   for (i = FirstP; i <= LastP; i++) {
1281     Ai2 = A(i, 2);
1282     Aid = A(i, nbpoles-1);
1283     if (FirstConstraint >= AppParCurves_PassPoint) xx = A(i, 1);
1284     if (FirstConstraint >= AppParCurves_TangencyPoint) xx += Ai2;
1285     if (LastConstraint >= AppParCurves_PassPoint) yy = A(i, nbpoles);
1286     if (LastConstraint >= AppParCurves_TangencyPoint) yy += Aid;
1287     i2 = 1;
1288     Nrow = myfirst-FirstP;
1289     for (Ci = 1; Ci <= nbP; Ci++) {
1290       i21 = i2+1; i22 = i2+2;
1291       ix = i+Nrow; iy = ix+Neq; iz = iy+Neq;
1292       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1293         myV1(ix) =  Ai2*Vec1t(i2);
1294         myV1(iy) =  Ai2*Vec1t(i21);
1295         myV1(iz) =  Ai2*Vec1t(i22);
1296       }
1297       if (LastConstraint >= AppParCurves_TangencyPoint) {
1298         myV2(ix) = -Aid*Vec2t(i2);
1299         myV2(iy) = -Aid*Vec2t(i21);
1300         myV2(iz) = -Aid*Vec2t(i22);
1301       }
1302       myB(ix)  = mypoints(i, i2)  - xx*mypoints(myfirstp, i2)
1303                                   - yy*mypoints(mylastp,  i2);
1304       myB(iy)  = mypoints(i, i21) - xx*mypoints(myfirstp, i21)
1305                                   - yy*mypoints(mylastp,  i21);
1306       myB(iz)  = mypoints(i, i22) - xx*mypoints(myfirstp, i22)
1307                                   - yy*mypoints(mylastp,  i22);
1308       i2 += 3;
1309       Nrow = Nrow + 3*Neq;
1310     }
1311
1312     for (Ci = 1; Ci <= nbP2d; Ci++) {
1313       i21 = i2+1; i22 = i2+2;
1314       ix = i+Nrow; iy = ix+Neq;
1315       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1316         myV1(ix) =  Ai2*Vec1t(i2);
1317         myV1(iy) =  Ai2*Vec1t(i21);
1318       }
1319       if (LastConstraint >= AppParCurves_TangencyPoint) {
1320         myV2(ix) = -Aid*Vec2t(i2);
1321         myV2(iy) = -Aid*Vec2t(i21);
1322       }
1323       myB(ix)  = mypoints(i, i2)  - xx*mypoints(myfirstp, i2)
1324                                   - yy*mypoints(mylastp,  i2);
1325       myB(iy)  = mypoints(i, i21) - xx*mypoints(myfirstp, i21)
1326                                   - yy*mypoints(mylastp,  i21);
1327       Nrow = Nrow + 2*Neq;
1328       i2 += 2;
1329     }
1330   }
1331
1332
1333
1334   // Construction de TA*A et TA*B:
1335
1336   for (k = FirstP; k <= LastP; k++) {
1337     indexdeb = myindex(k)+1;
1338     indexfin = indexdeb + deg;
1339     jinit = Max(resinit, indexdeb);
1340     jfin = Min(resfin, indexfin);
1341     k1 = k + myfirst - FirstP;
1342     for (i = 0; i <= NA1; i++) {
1343       nb = i*Neq + k1;
1344       if (FirstConstraint >= AppParCurves_TangencyPoint)
1345         v1 = myV1(nb);
1346       if (LastConstraint >= AppParCurves_TangencyPoint)
1347         v2 = myV2(nb);
1348       b = myB(nb);
1349       inc = i*Nincx-resinit+1;
1350       for (j = jinit; j <= jfin; j++) {
1351         Akj = A(k, j);
1352         u = j+inc;
1353         if (FirstConstraint >= AppParCurves_TangencyPoint)
1354           TheV1(u) += Akj*v1;
1355         if (LastConstraint >= AppParCurves_TangencyPoint)
1356           TheV2(u) += Akj*v2;
1357         myTAB(u) += Akj*b;
1358       }
1359       if (FirstConstraint >= AppParCurves_TangencyPoint) {
1360         taf1 += v1*v1;
1361         tab1 += v1*b;
1362       }
1363       if (LastConstraint >= AppParCurves_TangencyPoint) {
1364         taf2 += v2*v2;
1365         tab2 += v2*b;
1366       }
1367       if (FirstConstraint >= AppParCurves_TangencyPoint &&
1368           LastConstraint >= AppParCurves_TangencyPoint) {
1369         taf3 += v1*v2;
1370       }
1371     }
1372   }
1373
1374
1375   if (FirstConstraint >= AppParCurves_TangencyPoint) {
1376     TheV1(Ninc1) = taf1;
1377     myTAB(Ninc1) = tab1;
1378   }
1379   if (LastConstraint >= AppParCurves_TangencyPoint) {
1380     TheV2(Ninc)  = taf2;
1381     myTAB(Ninc)  = tab2;
1382   }
1383   if (FirstConstraint >= AppParCurves_TangencyPoint &&
1384       LastConstraint >= AppParCurves_TangencyPoint) {
1385     TheV2(Ninc1) = taf3;
1386   }
1387
1388
1389   if (resinit <= resfin) {
1390     math_IntegerVector Index(1, Nincx);
1391     SearchIndex(Index);
1392     math_Vector AA(1, Index(Nincx));
1393     MakeTAA(AA);
1394
1395     Standard_Integer kk = 1;
1396     for (k = 1; k <= NA; k++)   {
1397       for(i = 1; i <= AA.Length(); i++) {
1398         TheA(kk) = AA(i);
1399         kk++;
1400       }
1401     }
1402   }
1403
1404   Standard_Integer length = TheA.Length();
1405
1406   if (FirstConstraint >= AppParCurves_TangencyPoint &&
1407       LastConstraint >= AppParCurves_TangencyPoint) {
1408     for (j = 1; j <= Ninc1; j++)
1409       TheA(length- 2*Ninc+j+1) = TheV1(j);
1410     for (j = 1; j <= Ninc; j++)
1411       TheA(length- Ninc+j) = TheV2(j);
1412   }
1413
1414
1415   else if (FirstConstraint >= AppParCurves_TangencyPoint) {
1416     for (j = 1; j <= Ninc; j++)
1417       TheA(length- Ninc+j) = TheV1(j);
1418   }
1419
1420   else if (LastConstraint >= AppParCurves_TangencyPoint) {
1421     for (j = 1; j <= Ninc; j++)
1422       TheA(length- Ninc+j) = TheV2(j);
1423   }
1424 }
1425
1426
1427
1428
1429 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA,
1430                                        math_Matrix& myTAB)
1431 {
1432   Standard_Integer i, j, k;
1433   Standard_Integer indexdeb, indexfin, jinit, jfin;
1434   Standard_Integer iinit, ifin;
1435   Standard_Real Akj;
1436   math_Matrix TheA(resinit, resfin, resinit, resfin);
1437   TheA.Init(0.0);
1438
1439   for (k = FirstP ; k <= LastP; k++) {
1440     indexdeb = myindex(k)+1;
1441     indexfin = indexdeb + deg;
1442     jinit = Max(resinit, indexdeb);
1443     jfin = Min(resfin, indexfin);
1444     for (i = jinit; i <= jfin; i++) {
1445       Akj = A(k, i);
1446       for (j = jinit; j <= i; j++) {
1447         TheA(i, j) += A(k, j) * Akj;
1448       }
1449       for (j = 1; j <= B2.ColNumber(); j++) {
1450         myTAB(i, j) += Akj*B2(k, j);
1451       }
1452     }
1453   }
1454
1455   Standard_Integer len;
1456   if (!myknots.IsNull()) len = myknots->Length();
1457   else len = 2;
1458   Standard_Integer d, i2 = 1;
1459   iinit = resinit;
1460   jinit = resinit;
1461   ifin = Min(resfin, deg+1);
1462   for (k = 2; k <= len; k++) {
1463     for (i = iinit; i <= ifin; i++) {
1464       for (j = jinit; j <= i; j++) {
1465         AA(i2) = TheA(i, j);
1466         i2++;
1467       }
1468     }
1469     if (!mymults.IsNull()) {
1470       iinit = ifin+1;
1471       d = ifin + mymults->Value(k);
1472       ifin = Min(d, resfin);
1473       jinit = Max(resinit, d - deg);
1474     }
1475   }
1476 }
1477
1478
1479 void AppParCurves_LeastSquare::MakeTAA(math_Vector& AA)
1480 {
1481   Standard_Integer i, j, k;
1482   Standard_Integer indexdeb, indexfin, jinit, jfin;
1483   Standard_Integer iinit, ifin;
1484   Standard_Real Akj;
1485   math_Matrix TheA(resinit, resfin, resinit, resfin, 0.0);
1486
1487
1488   for (k = FirstP ; k <= LastP; k++) {
1489     indexdeb = myindex(k)+1;
1490     indexfin = indexdeb + deg;
1491     jinit = Max(resinit, indexdeb);
1492     jfin = Min(resfin, indexfin);
1493     for (i = jinit; i <= jfin; i++) {
1494       Akj = A(k, i);
1495       for (j = jinit; j <= i; j++) {
1496         TheA(i, j) += A(k, j) * Akj;
1497       }
1498     }
1499   }
1500
1501
1502   Standard_Integer i2 = 1;
1503   iinit = resinit;
1504   jinit = resinit;
1505   ifin = Min(resfin, deg+1);
1506   Standard_Integer len, d;
1507   if (!myknots.IsNull()) len = myknots->Length();
1508   else len = 2;
1509   for (k = 2; k <= len; k++) {
1510     for (i = iinit; i <= ifin; i++) {
1511       for (j = jinit; j <= i; j++) {
1512         AA(i2) = TheA(i, j);
1513         i2++;
1514       }
1515     }
1516     if (!mymults.IsNull()) {
1517       iinit = ifin+1;
1518       d = ifin + mymults->Value(k);
1519       ifin = Min(d, resfin);
1520       jinit = Max(resinit, d - deg);
1521     }
1522   }
1523
1524 }
1525
1526
1527
1528 void AppParCurves_LeastSquare::SearchIndex(math_IntegerVector& Index)
1529 {
1530   Standard_Integer iinit, jinit, ifin;
1531   Standard_Integer i, j, k, d, i2 = 1;
1532   Standard_Integer len;
1533   Standard_Integer Nincx = resfin-resinit+1;
1534   Index(1) = 1;
1535
1536   if (myknots.IsNull()) {
1537     if (resinit <= resfin) {
1538       Standard_Integer l = 1;
1539       for (i = 2; i <= Nincx; i++) {
1540         l++;
1541         Index(l) = Index(l-1)+i;
1542       }
1543     }
1544
1545   }
1546   else {
1547     iinit = resinit;
1548     jinit = resinit;
1549     ifin = Min(resfin, deg+1);
1550     len = myknots->Length();
1551
1552     for (k = 2; k <= len; k++) {
1553       for (i = iinit; i <= ifin; i++) {
1554         for (j = jinit; j <= i; j++) {
1555           if (i2 != 1)
1556             Index(i2) = Index(i2-1) + i-jinit+1;
1557         }
1558         i2++;
1559       }
1560       iinit = ifin+1;
1561       d = ifin + mymults->Value(k);
1562       ifin = Min(d, resfin);
1563       jinit = Max(resinit, d - deg);
1564     }
1565   }
1566 }