src/AppParCurves/AppParCurves_LeastSquare.gxx

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