src/AppParCurves/AppParCurves_LeastSquare.gxx

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