src/AppCont/AppCont_LeastSquare.gxx

   1 // Created on: 1995-03-14
   2 // Created by: Modelistation
   3 // Copyright (c) 1995-1999 Matra Datavision
   4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   5 //
   6 // This file is part of Open CASCADE Technology software library.
   7 //
   8 // This library is free software; you can redistribute it and/or modify it under
   9 // the terms of the GNU Lesser General Public License version 2.1 as published
  10 // by the Free Software Foundation, with special exception defined in the file
  11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  12 // distribution for complete text of the license and disclaimer of any warranty.
  13 //
  14 // Alternatively, this file may be used under the terms of Open CASCADE
  15 // commercial license or contractual agreement.
  16
  17 #ifndef OCCT_DEBUG
  18 #define No_Standard_OutOfRange
  19 #define No_Standard_RangeError
  20 #endif
  21
  22
  23
  24 #include <math.hxx>
  25 #include <math_Vector.hxx>
  26 #include <math_Matrix.hxx>
  27 #include <TColgp_Array1OfPnt.hxx>
  28 #include <TColgp_Array1OfPnt2d.hxx>
  29 #include <gp_Pnt2d.hxx>
  30 #include <gp_Pnt.hxx>
  31 #include <gp_Vec.hxx>
  32 #include <gp_Vec2d.hxx>
  33 #include <TColgp_Array1OfVec.hxx>
  34 #include <TColgp_Array1OfVec2d.hxx>
  35 #include <AppParCurves_MultiPoint.hxx>
  36 #include <AppCont_ContMatrices.hxx>
  37 #include <PLib.hxx>
  38
  39
  40
  41
  42 //=======================================================================
  43 //function : AppCont_LeastSquare
  44 //purpose  :
  45 //=======================================================================
  46
  47 AppCont_LeastSquare::AppCont_LeastSquare
  48                           (const MultiLine&              SSP,
  49                            const Standard_Real           U0,
  50                            const Standard_Real           U1,
  51                            const AppParCurves_Constraint FirstCons,
  52                            const AppParCurves_Constraint LastCons,
  53                            const Standard_Integer        Deg,
  54                            const Standard_Integer        NbPoints):
  55                            SCU(Deg+1),
  56                            Points(1, NbPoints, 1, NbBColumns(SSP)),
  57                            Poles(1, Deg+1, 1, NbBColumns(SSP), 0.0),
  58                            myParam(1, NbPoints),
  59                            VB(1, Deg+1, 1, NbPoints)
  60
  61 {
  62   Done = Standard_False;
  63   Degre = Deg;
  64   math_Matrix InvM(1, Deg+1, 1, Deg+1);
  65   Standard_Integer i, j, k, c, i2;
  66   Standard_Integer classe = Deg+1, cl1 = Deg;
  67   Standard_Real U, dU, Coeff, Coeff2;
  68   Standard_Real IBij, IBPij;
  69
  70   Standard_Integer FirstP = 1, LastP = NbPoints;
  71   Standard_Integer nbcol = NbBColumns(SSP);
  72   math_Matrix B(1, classe, 1, nbcol, 0.0);
  73   Standard_Integer bdeb = 1, bfin = classe;
  74   AppParCurves_Constraint myFirstC = FirstCons, myLastC = LastCons;
  75   nbP = LineTool::NbP3d(SSP);
  76   nbP2d = LineTool::NbP2d(SSP);
  77   Standard_Integer mynbP = nbP, mynbP2d = nbP2d;
  78   if (nbP == 0) mynbP = 1;
  79   if (nbP2d == 0) mynbP2d = 1;
  80
  81   Standard_Integer i2plus1, i2plus2;
  82   Nbdiscret = NbPoints;
  83   TColgp_Array1OfPnt TabP(1, mynbP);
  84   TColgp_Array1OfVec TabV(1, mynbP);
  85   TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
  86   TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
  87
  88   Standard_Boolean Ok;
  89   if (myFirstC == AppParCurves_TangencyPoint) {
  90     if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U0, TabV, TabV2d);
  91     else if (nbP != 0)          Ok=LineTool::D1(SSP, U0, TabV);
  92     else                        Ok=LineTool::D1(SSP, U0, TabV2d);
  93     if (!Ok) myFirstC = AppParCurves_PassPoint;
  94   }
  95
  96   if (myLastC == AppParCurves_TangencyPoint) {
  97     if (nbP != 0 && nbP2d != 0) Ok=LineTool::D1(SSP, U1, TabV, TabV2d);
  98     else if (nbP != 0)          Ok=LineTool::D1(SSP, U1, TabV);
  99     else                        Ok=LineTool::D1(SSP, U1, TabV2d);
 100     if (!Ok) myLastC = AppParCurves_PassPoint;
 101   }
 102
 103   math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
 104   math::GaussPoints(NbPoints, GaussP);
 105   math::GaussWeights(NbPoints, GaussW);
 106
 107   math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
 108
 109   dU = 0.5*(U1-U0);
 110
 111   // calcul et mise en ordre des parametres et des poids:
 112   for (i = FirstP; i <= LastP; i++) {
 113     U  = 0.5*(U1+U0) + dU*GaussP(i);
 114     if (i <=  (NbPoints+1)/2) {
 115       myParam(LastP-i+1)  = U;
 116       VBParam(LastP-i+1)  = 0.5*(1 + GaussP(i));
 117       TheWeights(LastP-i+1) = 0.5*GaussW(i);
 118     }
 119     else {
 120       VBParam(i-(NbPoints+1)/2)  = 0.5*(1 + GaussP(i));
 121       myParam(i-(NbPoints+1)/2) = U;
 122       TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
 123     }
 124   }
 125
 126
 127   for (i = FirstP; i <= LastP; i++) {
 128     U = myParam(i);
 129     if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U, TabP, TabP2d);
 130     else if (nbP != 0)          LineTool::Value(SSP, U, TabP);
 131     else                        LineTool::Value(SSP, U, TabP2d);
 132
 133     i2 = 1;
 134     for (j = 1; j <= nbP; j++) {
 135       (TabP(j)).Coord(Points(i, i2), Points(i, i2+1), Points(i, i2+2));
 136       i2 += 3;
 137     }
 138     for (j = 1; j <= nbP2d; j++) {
 139       (TabP2d(j)).Coord(Points(i, i2), Points(i, i2+1));
 140       i2 += 2;
 141     }
 142   }
 143
 144   // Calcul du VB ( Valeur des fonctions de Bernstein):
 145
 146 //  for (i = 1; i <= classe; i++) {
 147 //    for (j = 1; j <= NbPoints; j++) {
 148 //    VB(i,j)=PLib::Binomial(cl1,i-1)*Pow((1-VBParam(j)),classe-i)*
 149 //      Pow(VBParam(j),i-1);
 150 //   }
 151 //  }
 152
 153   VBernstein(classe, NbPoints, VB);
 154
 155   // Traitement du second membre:
 156
 157   Standard_Real *tmppoints, *tmpbis;
 158   tmppoints = new Standard_Real[nbcol];
 159
 160   for (c = 1; c <= classe; c++) {
 161     tmpbis = tmppoints;
 162     for (k = 1; k <= nbcol; k++, tmpbis++) {
 163       *tmpbis = 0.0;
 164     }
 165     for (i = 1; i <= NbPoints; i++) {
 166       Coeff = TheWeights(i)*VB(c, i);
 167       tmpbis = tmppoints;
 168       for (j = 1; j <= nbcol; j++, tmpbis++) {
 169         *tmpbis += Points(i, j)*Coeff;
 170         //B(c, j) += Points(i, j)*Coeff;
 171       }
 172     }
 173     tmpbis = tmppoints;
 174     for (k = 1; k <= nbcol; k++, tmpbis++) {
 175       B(c, k) += *tmpbis;
 176     }
 177   }
 178
 179   delete [] tmppoints;
 180
 181   if (myFirstC == AppParCurves_NoConstraint &&
 182       myLastC  == AppParCurves_NoConstraint) {
 183
 184     math_Matrix InvM(1, classe, 1, classe);
 185     InvMMatrix(classe, InvM);
 186     // Calcul direct des poles:
 187
 188     for (i = 1; i <= classe; i++) {
 189       for (j = 1; j <= classe; j++) {
 190         IBij = InvM(i, j);
 191         for (k = 1; k <= nbcol; k++) {
 192           Poles(i, k)   += IBij * B(j, k);
 193         }
 194       }
 195     }
 196   }
 197
 198
 199   else {
 200     math_Matrix M(1, classe, 1, classe);
 201     MMatrix(classe, M);
 202
 203     if (myFirstC == AppParCurves_PassPoint ||
 204         myFirstC == AppParCurves_TangencyPoint) {
 205
 206       if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U0, TabP, TabP2d);
 207       else if (nbP != 0)          LineTool::Value(SSP, U0, TabP);
 208       else                        LineTool::Value(SSP, U0, TabP2d);
 209       i2 =1;
 210       for (k = 1; k<= nbP; k++) {
 211         (TabP(k)).Coord(Poles(1, i2), Poles(1, i2+1), Poles(1, i2+2));
 212         i2 += 3;
 213       }
 214       for (k = 1; k<= nbP2d; k++) {
 215         (TabP2d(k)).Coord(Poles(1, i2), Poles(1, i2+1));
 216         i2 += 2;
 217       }
 218
 219     }
 220
 221     if (myLastC == AppParCurves_PassPoint ||
 222         myLastC == AppParCurves_TangencyPoint) {
 223
 224       i2 = 1;
 225       if (nbP != 0 && nbP2d != 0) LineTool::Value(SSP, U1, TabP, TabP2d);
 226       else if (nbP != 0)          LineTool::Value(SSP, U1, TabP);
 227       else                        LineTool::Value(SSP, U1, TabP2d);
 228       for (k = 1; k<= nbP; k++) {
 229         (TabP(k)).Coord(Poles(classe,i2),
 230                         Poles(classe,i2+1),
 231                         Poles(classe,i2+2));
 232         i2 += 3;
 233       }
 234       for (k = 1; k<= nbP2d; k++) {
 235         (TabP2d(k)).Coord(Poles(classe, i2), Poles(classe, i2+1));
 236         i2 += 2;
 237       }
 238     }
 239
 240
 241
 242     if (myFirstC == AppParCurves_PassPoint) {
 243       bdeb = 2;
 244       // mise a jour du second membre:
 245       for (i = 1; i <= classe; i++) {
 246         Coeff = M(i, 1);
 247         for (k = 1; k <= nbcol; k++) {
 248           B(i, k) -= Poles(1, k)*Coeff;
 249         }
 250       }
 251     }
 252
 253
 254     if (myLastC == AppParCurves_PassPoint) {
 255       bfin = cl1;
 256       for (i = 1; i <= classe; i++) {
 257         Coeff = M(i, classe);
 258         for (k = 1; k <= nbcol; k++) {
 259           B(i, k) -= Poles(classe, k)*Coeff;
 260         }
 261       }
 262     }
 263
 264
 265     if (myFirstC == AppParCurves_TangencyPoint) {
 266       // On fixe le second pole::
 267       bdeb = 3;
 268       if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U0, TabV, TabV2d);
 269       else if (nbP != 0)          LineTool::D1(SSP, U0, TabV);
 270       else                        LineTool::D1(SSP, U0, TabV2d);
 271       i2 = 1;
 272       Coeff = (U1-U0)/Degre;
 273       for (k = 1; k<= nbP; k++) {
 274         i2plus1 = i2+1; i2plus2 = i2+2;
 275         Poles(2, i2)      = Poles(1, i2)      + TabV(k).X()*Coeff;
 276         Poles(2, i2plus1) = Poles(1, i2plus1) + TabV(k).Y()*Coeff;
 277         Poles(2, i2plus2) = Poles(1, i2plus2) + TabV(k).Z()*Coeff;
 278         i2 += 3;
 279       }
 280       for (k = 1; k<= nbP2d; k++) {
 281         i2plus1 = i2+1;
 282         Poles(2, i2)      = Poles(1, i2)      + TabV2d(k).X()*Coeff;
 283         Poles(2, i2plus1) = Poles(1, i2plus1) + TabV2d(k).Y()*Coeff;
 284         i2 += 2;
 285       }
 286
 287
 288       for (i = 1; i <= classe; i++) {
 289         Coeff = M(i, 1); Coeff2 = M(i, 2);
 290         for (k = 1; k <= nbcol; k++) {
 291           B(i, k) -= Poles(1, k)*Coeff+Poles(2, k)*Coeff2;
 292         }
 293       }
 294     }
 295
 296     if (myLastC == AppParCurves_TangencyPoint) {
 297       bfin = classe-2;
 298
 299       if (nbP != 0 && nbP2d != 0) LineTool::D1(SSP, U1, TabV, TabV2d);
 300       else if (nbP != 0)          LineTool::D1(SSP, U1, TabV);
 301       else                        LineTool::D1(SSP, U1, TabV2d);
 302       i2 = 1;
 303       Coeff = (U1-U0)/Degre;
 304       for (k = 1; k<= nbP; k++) {
 305         i2plus1 = i2+1; i2plus2 = i2+2;
 306         Poles(cl1,i2)      = Poles(classe, i2)      - TabV(k).X()*Coeff;
 307         Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV(k).Y()*Coeff;
 308         Poles(cl1,i2plus2) = Poles(classe, i2plus2) - TabV(k).Z()*Coeff;
 309         i2 += 3;
 310       }
 311       for (k = 1; k<= nbP2d; k++) {
 312         i2plus1 = i2+1;
 313         Poles(cl1,i2)      = Poles(classe, i2)      - TabV2d(k).X()*Coeff;
 314         Poles(cl1,i2plus1) = Poles(classe, i2plus1) - TabV2d(k).Y()*Coeff;
 315         i2 += 2;
 316       }
 317
 318       for (i = 1; i <= classe; i++) {
 319         Coeff = M(i, classe); Coeff2 = M(i, cl1);
 320         for (k = 1; k <= nbcol; k++) {
 321           B(i, k) -= Poles(classe, k)*Coeff + Poles(cl1, k)*Coeff2;
 322         }
 323       }
 324     }
 325
 326     if (bdeb <= bfin) {
 327       math_Matrix B2(bdeb, bfin, 1, B.UpperCol(), 0.0);
 328
 329       for (i = bdeb; i <= bfin; i++) {
 330         for (j = 1; j <= classe; j++) {
 331           Coeff = M(i, j);
 332           for (k = 1; k <= nbcol; k++) {
 333             B2(i, k) += B(j, k)*Coeff;
 334           }
 335         }
 336       }
 337
 338
 339       // Resolution:
 340       // ===========
 341       math_Matrix IBP(bdeb, bfin, bdeb, bfin);
 342
 343       // dans IBPMatrix at IBTMatrix ne sont stockees que les resultats pour
 344       // une classe inferieure ou egale a 26 (pour l instant du moins.)
 345
 346       if (bdeb == 2 && bfin == classe-1 && classe <= 26) {
 347         IBPMatrix(classe, IBP);
 348       }
 349       else if (bdeb == 3 && bfin == classe-2 && classe <= 26) {
 350         IBTMatrix(classe, IBP);
 351       }
 352       else {
 353         math_Matrix MP(1, classe, bdeb, bfin);
 354
 355         for (i = 1; i <= classe; i++) {
 356           for (j = bdeb; j <= bfin; j++) {
 357             MP(i, j) = M(i, j);
 358           }
 359         }
 360         math_Matrix IBP1(bdeb, bfin, bdeb, bfin);
 361         IBP1 = MP.Transposed()*MP;
 362         IBP = IBP1.Inverse();
 363       }
 364
 365
 366       Done = Standard_True;
 367       for (i = bdeb; i <= bfin; i++) {
 368         for (j = bdeb; j <= bfin; j++) {
 369           IBPij = IBP(i, j);;
 370           for (k = 1; k<= nbcol; k++) {
 371             Poles(i, k)   += IBPij * B2(j, k);
 372           }
 373         }
 374       }
 375     }
 376   }
 377 }
 378
 379
 380
 381
 382 //=======================================================================
 383 //function : NbBColumns
 384 //purpose  :
 385 //=======================================================================
 386
 387 Standard_Integer AppCont_LeastSquare::NbBColumns(
 388                                      const MultiLine& SSP) const
 389 {
 390   Standard_Integer BCol;
 391   BCol = (LineTool::NbP3d(SSP))*3 +
 392          (LineTool::NbP2d(SSP))*2;
 393   return BCol;
 394 }
 395
 396
 397 //=======================================================================
 398 //function : Value
 399 //purpose  :
 400 //=======================================================================
 401
 402 const AppParCurves_MultiCurve& AppCont_LeastSquare::Value()
 403 {
 404
 405   Standard_Integer i, j, j2;
 406   gp_Pnt Pt;
 407   gp_Pnt2d Pt2d;
 408   Standard_Integer ideb = 1, ifin = Degre+1;
 409
 410   // On met le resultat dans les curves correspondantes
 411   for (i = ideb; i <= ifin; i++) {
 412     j2 = 1;
 413     AppParCurves_MultiPoint MPole(nbP, nbP2d);
 414     for (j = 1; j <= nbP; j++) {
 415       Pt.SetCoord(Poles(i, j2), Poles(i, j2+1), Poles(i,j2+2));
 416       MPole.SetPoint(j, Pt);
 417       j2 += 3;
 418     }
 419     for (j = nbP+1;j <= nbP+nbP2d; j++) {
 420       Pt2d.SetCoord(Poles(i, j2), Poles(i, j2+1));
 421       MPole.SetPoint2d(j, Pt2d);
 422       j2 += 2;
 423     }
 424     SCU.SetValue(i, MPole);
 425   }
 426   return SCU;
 427 }
 428
 429
 430
 431 //=======================================================================
 432 //function : Error
 433 //purpose  :
 434 //=======================================================================
 435
 436 void AppCont_LeastSquare::Error(Standard_Real& F,
 437                                 Standard_Real& MaxE3d,
 438                                 Standard_Real& MaxE2d) const
 439 {
 440   Standard_Integer i, j, k, c, i2, classe = Degre+1;
 441 //  Standard_Real Coeff, val = 0.0, err3d = 0.0, err2d =0.0;
 442   Standard_Real Coeff, err3d = 0.0, err2d =0.0;
 443   Standard_Integer ncol = Points.UpperCol()-Points.LowerCol()+1;
 444
 445   math_Matrix MyPoints(1, Nbdiscret, 1, ncol);
 446   MyPoints = Points;
 447
 448   MaxE3d = MaxE2d = F = 0.0;
 449
 450   Standard_Real *tmppoles, *tmpbis;
 451   tmppoles = new Standard_Real[ncol];
 452
 453   for (c = 1; c <= classe; c++) {
 454     tmpbis = tmppoles;
 455     for (k = 1; k <= ncol; k++, tmpbis++) {
 456       *tmpbis = Poles(c, k);
 457     }
 458     for (i = 1; i <= Nbdiscret; i++) {
 459       Coeff = VB(c, i);
 460       tmpbis = tmppoles;
 461       for (j = 1; j <= ncol; j++, tmpbis++) {
 462         MyPoints(i, j) -= (*tmpbis)*Coeff;  // Poles(c, j)*Coeff;
 463       }
 464     }
 465   }
 466   delete [] tmppoles;
 467
 468   Standard_Real e1, e2, e3;
 469   for (i = 1; i <= Nbdiscret; i++) {
 470     i2 = 1;
 471     for (j = 1; j<= nbP; j++) {
 472       e1 = MyPoints(i, i2);
 473       e2 = MyPoints(i, i2+1);
 474       e3 = MyPoints(i, i2+2);
 475       err3d = e1*e1+e2*e2+e3*e3;
 476       MaxE3d = Max(MaxE3d, err3d);
 477       F += err3d;
 478       i2 += 3;
 479     }
 480     for (j = 1; j<= nbP2d; j++) {
 481       e1 = MyPoints(i, i2);
 482       e2 = MyPoints(i, i2+1);
 483       err2d = e1*e1+e2*e2;
 484       MaxE2d = Max(MaxE2d, err2d);
 485       F += err2d;
 486       i2 += 2;
 487     }
 488   }
 489
 490   MaxE3d = Sqrt(MaxE3d);
 491   MaxE2d = Sqrt(MaxE2d);
 492
 493 }
 494
 495
 496 //=======================================================================
 497 //function : IsDone
 498 //purpose  :
 499 //=======================================================================
 500
 501 Standard_Boolean AppCont_LeastSquare::IsDone() const
 502 {
 503   return Done;
 504 }