src/AppCont/AppCont_SurfLeastSquare.gxx

   1 // Copyright (c) 1995-1999 Matra Datavision
   2 // Copyright (c) 1999-2012 OPEN CASCADE SAS
   3 //
   4 // The content of this file is subject to the Open CASCADE Technology Public
   5 // License Version 6.5 (the "License"). You may not use the content of this file
   6 // except in compliance with the License. Please obtain a copy of the License
   7 // at http://www.opencascade.org and read it completely before using this file.
   8 //
   9 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
  10 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
  11 //
  12 // The Original Code and all software distributed under the License is
  13 // distributed on an "AS IS" basis, without warranty of any kind, and the
  14 // Initial Developer hereby disclaims all such warranties, including without
  15 // limitation, any warranties of merchantability, fitness for a particular
  16 // purpose or non-infringement. Please see the License for the specific terms
  17 // and conditions governing the rights and limitations under the License.
  18
  19 // Lpa, le 19/05/93
  20
  21
  22 #ifndef DEB
  23 #define No_Standard_OutOfRange
  24 #define No_Standard_RangeError
  25 #endif
  26
  27 #include <math_GaussPoints.hxx>
  28 #include <math_Vector.hxx>
  29 #include <math_Matrix.hxx>
  30 #include <gp_Pnt.hxx>
  31 #include <gp_Vec.hxx>
  32 #include <AppParCurves_MultiPoint.hxx>
  33 #include <AppCont_ContMatrices.hxx>
  34 #include <Standard_ConstructionError.hxx>
  35 #include <Standard_NotImplemented.hxx>
  36 #include <StdFail_NotDone.hxx>
  37 #include <PLib.hxx>
  38
  39
  40
  41
  42 static void InvMSurfMatrix(const Standard_Integer classeU,
  43                            const Standard_Integer classeV,
  44                            math_Matrix&           InvM)
  45 {
  46   math_Matrix Inv1(1, classeU);
  47   InvMMatrix(classeU, Inv1);
  48   math_Matrix Inv2(1, classeV);
  49   InvMMatrix(classeV, Inv2);
  50
  51   // math_Matrix InvM(1, classeU*classeV, 1, classeU*classeV);
  52   Standard_Integer i, j, k, l;
  53
  54   for (i = 1; i <= classeU; i++) {
  55     for (j= 1; j <= classeU; j++) {
  56       for (k =1; k<= classeV; k++) {
  57         for (l = 1; l<= classeV; l++) {
  58           InvM(k+(i-1)*classeV,l+(j-1)*classeV) = Inv1(i,j)*Inv2(k,l);
  59         }
  60       }
  61     }
  62   }
  63 }
  64
  65
  66 static void MSurfMatrix(const Standard_Integer classeU,
  67                         const Standard_Integer classeV,
  68                         math_Matrix&           M)
  69 {
  70   math_Matrix M1(1, classeU, 1, classeU);
  71   MMatrix(classeU, M1);
  72   math_Matrix M2(1, classeV, 1, classeV);
  73   MMatrix(classeV, M2);
  74
  75   // math_Matrix M(1, classeU*classeV, 1, classeU*classeV);
  76   Standard_Integer i, j, k, l;
  77
  78   for (i = 1; i <= classeU; i++) {
  79     for (j= 1; j <= classeU; j++) {
  80       for (k =1; k<= classeV; k++) {
  81         for (l = 1; l<= classeV; l++) {
  82           M(k+(i-1)*classeV,l+(j-1)*classeV) = M1(i,j)*M2(k,l);
  83         }
  84       }
  85     }
  86   }
  87 }
  88
  89
  90
  91
  92
  93
  94 AppCont_SurfLeastSquare::
  95   AppCont_SurfLeastSquare(const Surface& Surf,
  96                           const Standard_Real U0,
  97                           const Standard_Real U1,
  98                           const Standard_Real V0,
  99                           const Standard_Real V1,
 100                           const AppParCurves_Constraint FirstCons,
 101                           const AppParCurves_Constraint LastUCons,
 102                           const AppParCurves_Constraint LastVCons,
 103                           const AppParCurves_Constraint LastCons,
 104                           const Standard_Integer DegU,
 105                           const Standard_Integer DegV,
 106                           const Standard_Integer NbPoints):
 107                           PointsX(1, NbPoints, 1 , NbPoints),
 108                           PointsY(1, NbPoints, 1 , NbPoints),
 109                           PointsZ(1, NbPoints, 1 , NbPoints),
 110                           PolesX(1, (DegU+1)*(DegV+1), 0.0),
 111                           PolesY(1, (DegU+1)*(DegV+1), 0.0),
 112                           PolesZ(1, (DegU+1)*(DegV+1), 0.0),
 113                           myUParam(1, NbPoints),
 114                           myVParam(1, NbPoints),
 115                           VBU(1, DegU+1, 1, NbPoints),
 116                           VBV(1, DegV+1, 1, NbPoints)
 117 {
 118   DegreU = DegU;
 119   DegreV = DegV;
 120   Nbdiscret = NbPoints;
 121   Standard_Integer c, c1, c2, classeU = DegU+1, classeV = DegV+1;
 122   Standard_Integer cla = classeU*classeV;
 123   Standard_Integer bdeb = 1, bfin = cla, clav = cla-classeV+1;
 124   Standard_Integer bint = 0, bint1 = 0, bint2 = 0, bintfin = 0;
 125   Standard_Integer bint3 = 0, bint4 = 0, bint5 = 0, bint6 = 0;
 126   Standard_Integer bint7 = 0, bint8 = 0;
 127   math_Vector GaussP(1, NbPoints), GaussW(1, NbPoints);
 128   Standard_Integer i, j, FirstP = 1, LastP = NbPoints;
 129   Standard_Real U, dU, V, dV, ISS, Coeff, Coeff2;
 130   Done = Standard_False;
 131   gp_Pnt Pt;
 132   gp_Vec VU, VV;
 133   math_Vector BX(1, cla, 0.0),
 134               BY(1, cla, 0.0),
 135               BZ(1, cla, 0.0);
 136
 137   GaussP = GPoints(NbPoints);
 138   GaussW = GWeights(NbPoints);
 139   math_Vector TheWeights(1, NbPoints), VBParam(1, NbPoints);
 140
 141   dU = 0.5*(U1-U0);
 142   dV = 0.5*(V1-V0);
 143
 144   // calcul et mise en ordre des parametres et des poids:
 145   for (i = FirstP; i <= LastP; i++) {
 146     U  = 0.5*(U1+U0) + dU*GaussP(i);
 147     V  = 0.5*(V1+V0) + dV*GaussP(i);
 148     if (i <=  (NbPoints+1)/2) {
 149       myUParam(LastP-i+1)  = U;
 150       myVParam(LastP-i+1)  = V;
 151       VBParam(LastP-i+1)  = 0.5*(1 + GaussP(i));
 152       TheWeights(LastP-i+1) = 0.5*GaussW(i);
 153     }
 154     else {
 155       myUParam(i-(NbPoints+1)/2) = U;
 156       myVParam(i-(NbPoints+1)/2) = V;
 157       VBParam(i-(NbPoints+1)/2)  = 0.5*(1 + GaussP(i));
 158       TheWeights(i-(NbPoints+1)/2) = 0.5*GaussW(i);
 159     }
 160   }
 161
 162
 163
 164   // Stockage des Points de Gauss:
 165   for (i = FirstP; i <= LastP; i++) {
 166     U = myUParam(i);
 167     for (j = FirstP; j <= LastP; j++) {
 168       V = myVParam(j);
 169       SurfTool::D0(Surf, U, V, Pt);
 170       Pt.Coord(PointsX(i, j), PointsY(i, j), PointsZ(i, j));
 171     }
 172   }
 173
 174
 175   // Calcul des VB ( fonctions de Bernstein):
 176   for (i = 1; i <= classeU; i++) {
 177     for (j = 1; j <= NbPoints; j++) {
 178       VBU(i,j) = PLib::Binomial(classeU-1,i-1)*
 179         Pow((1-VBParam(j)),classeU-i)*Pow(VBParam(j),i-1);
 180     }
 181   }
 182
 183   for (i = 1; i <= classeV; i++) {
 184     for (j = 1; j <= NbPoints; j++) {
 185       VBV(i,j) = PLib::Binomial(classeV-1,i-1)*
 186         Pow((1-VBParam(j)),classeV-i)*Pow(VBParam(j),i-1);
 187     }
 188   }
 189
 190   // Traitement du second membre:
 191   c = 0;
 192   for (c1 = 1; c1 <= classeU; c1++) {
 193     for (c2 = 1; c2 <= classeV; c2++) {
 194       c++;
 195       for (i = 1; i <= NbPoints; i++) {
 196         for (j = 1; j <= NbPoints; j++) {
 197           Coeff = TheWeights(i)*TheWeights(j)*VBU(c1, i)*VBV(c2, j);
 198           BX(c) += PointsX(i, j)*Coeff;
 199           BY(c) += PointsY(i, j)*Coeff;
 200           BZ(c) += PointsZ(i, j)*Coeff;
 201         }
 202       }
 203     }
 204   }
 205
 206   math_Matrix InvM(1, classeU*classeV, 1, classeU*classeV);
 207   InvMSurfMatrix(classeU, classeV, InvM);
 208   TColgp_Array2OfPnt Poles(1, classeU, 1, classeV);
 209
 210   // Calcul des poles:
 211   // =================
 212   if (FirstCons == AppParCurves_NoConstraint &&
 213       LastCons  == AppParCurves_NoConstraint &&
 214       LastUCons == AppParCurves_NoConstraint &&
 215       LastVCons == AppParCurves_NoConstraint) {
 216
 217     for (i = 1; i <= cla; i++) {
 218       for (j = 1; j <= cla; j++) {
 219         ISS = InvM(i, j);
 220         PolesX(i) += ISS * BX(j);
 221         PolesY(i) += ISS * BY(j);
 222         PolesZ(i) += ISS * BZ(j);
 223       }
 224     }
 225   }
 226
 227   else {
 228     // Traitement du second membre:
 229     math_Matrix M(1, classeU*classeV, 1, classeU*classeV);
 230     MSurfMatrix(classeU, classeV, M);
 231
 232
 233     if (FirstCons == AppParCurves_PassPoint ||
 234         FirstCons == AppParCurves_TangencyPoint) {
 235       bdeb = 2;
 236       SurfTool::D0(Surf, U0, V0, Pt);
 237       Pt.Coord(PolesX(1), PolesY(1), PolesZ(1));
 238
 239       for (c = 1; c <= cla; c++) {
 240         Coeff = M(c, 1);
 241         BX(c) = BX(c) - PolesX(1)*Coeff;
 242         BY(c) = BY(c) - PolesY(1)*Coeff;
 243         BZ(c) = BZ(c) - PolesZ(1)*Coeff;
 244       }
 245     }
 246
 247     if (LastCons == AppParCurves_PassPoint ||
 248         LastCons == AppParCurves_TangencyPoint) {
 249       bfin = cla-1;
 250       SurfTool::D0(Surf, U1, V1, Pt);
 251       Pt.Coord(PolesX(cla), PolesY(cla), PolesZ(cla));
 252
 253       for (c = 1; c <= cla; c++) {
 254         Coeff = M(c, cla);
 255         BX(c) = BX(c) - PolesX(cla)*Coeff;
 256         BY(c) = BY(c) - PolesY(cla)*Coeff;
 257         BZ(c) = BZ(c) - PolesZ(cla)*Coeff;
 258       }
 259     }
 260
 261     if (LastUCons == AppParCurves_PassPoint ||
 262         LastUCons == AppParCurves_TangencyPoint) {
 263       bint++; bint1 = clav;
 264       SurfTool::D0(Surf, U1, V0, Pt);
 265       Pt.Coord(PolesX(clav), PolesY(clav), PolesZ(clav));
 266
 267       for (c = 1; c <= cla; c++) {
 268         Coeff = M(c, clav);
 269         BX(c) = BX(c) - PolesX(clav)*Coeff;
 270         BY(c) = BY(c) - PolesY(clav)*Coeff;
 271         BZ(c) = BZ(c) - PolesZ(clav)*Coeff;
 272       }
 273     }
 274
 275     if (LastVCons == AppParCurves_PassPoint ||
 276         LastVCons == AppParCurves_TangencyPoint) {
 277       bint++; bint2 = classeV;
 278       SurfTool::D0(Surf, U0, V1, Pt);
 279       Pt.Coord(PolesX(classeV), PolesY(classeV), PolesZ(classeV));
 280
 281       for (c = 1; c <= cla; c++) {
 282         Coeff = M(c, classeV);
 283         BX(c) = BX(c) - PolesX(classeV)*Coeff;
 284         BY(c) = BY(c) - PolesY(classeV)*Coeff;
 285         BZ(c) = BZ(c) - PolesZ(classeV)*Coeff;
 286       }
 287     }
 288
 289
 290
 291
 292
 293     if (FirstCons == AppParCurves_TangencyPoint) {
 294       SurfTool::D1(Surf, U0, V0, Pt, VU, VV);
 295       bdeb = 3; bint++; bint3 = classeV+1;
 296
 297       PolesX(bint3) = PolesX(1) + VU.X()/DegU*(U1-U0);
 298       PolesY(bint3) = PolesY(1) + VU.Y()/DegU*(U1-U0);
 299       PolesZ(bint3) = PolesZ(1) + VU.Z()/DegU*(U1-U0);
 300
 301       PolesX(2) = PolesX(1) + VV.X()/DegV*(V1-V0);
 302       PolesY(2) = PolesY(1) + VV.Y()/DegV*(V1-V0);
 303       PolesZ(2) = PolesZ(1) + VV.Z()/DegV*(V1-V0);
 304
 305       for (c = 1; c <= cla; c++) {
 306         Coeff = M(c, 2); Coeff2 = M(c, bint3);
 307         BX(c) = BX(c) - PolesX(2)*Coeff - PolesX(bint3)*Coeff2;
 308         BY(c) = BY(c) - PolesY(2)*Coeff - PolesY(bint3)*Coeff2;
 309         BZ(c) = BZ(c) - PolesZ(2)*Coeff - PolesZ(bint3)*Coeff2;
 310       }
 311     }
 312
 313
 314     if (LastCons == AppParCurves_TangencyPoint) {
 315       SurfTool::D1(Surf, U1, V1, Pt, VU, VV);
 316       bfin = cla-2; bint++; bint4 = cla-classeV;
 317
 318       PolesX(bint4) = PolesX(cla) - VU.X()/DegU*(U1-U0);
 319       PolesY(bint4) = PolesY(cla) - VU.Y()/DegU*(U1-U0);
 320       PolesZ(bint4) = PolesZ(cla) - VU.Z()/DegU*(U1-U0);
 321
 322       PolesX(cla-1) = PolesX(cla) - VV.X()/DegV*(V1-V0);
 323       PolesY(cla-1) = PolesY(cla) - VV.Y()/DegV*(V1-V0);
 324       PolesZ(cla-1) = PolesZ(cla) - VV.Z()/DegV*(V1-V0);
 325
 326       for (c = 1; c <= cla; c++) {
 327         Coeff = M(c, cla-1); Coeff2 = M(c, bint4);
 328         BX(c) = BX(c) - PolesX(cla-1)*Coeff - PolesX(bint4)*Coeff2;
 329         BY(c) = BY(c) - PolesY(cla-1)*Coeff - PolesY(bint4)*Coeff2;
 330         BZ(c) = BZ(c) - PolesZ(cla-1)*Coeff - PolesZ(bint4)*Coeff2;
 331       }
 332     }
 333
 334
 335     if (LastVCons == AppParCurves_TangencyPoint) {
 336       SurfTool::D1(Surf, U0, V1, Pt, VU, VV);
 337       bint += 2; bint5 = classeV-1; bint6 = 2*classeV;
 338
 339       PolesX(bint5) = PolesX(classeV) - VV.X()/DegV*(V1-V0);
 340       PolesY(bint5) = PolesY(classeV) - VV.Y()/DegV*(V1-V0);
 341       PolesZ(bint5) = PolesZ(classeV) - VV.Z()/DegV*(V1-V0);
 342
 343       PolesX(bint6) = PolesX(classeV) + VU.X()/DegU*(U1-U0);
 344       PolesY(bint6) = PolesY(classeV) + VU.Y()/DegU*(U1-U0);
 345       PolesZ(bint6) = PolesZ(classeV) + VU.Z()/DegU*(U1-U0);
 346
 347       for (c = 1; c <= cla; c++) {
 348         Coeff = M(c, bint5); Coeff2 = M(c, bint6);
 349         BX(c) = BX(c) - PolesX(bint5)*Coeff - PolesX(bint6)*Coeff2;
 350         BY(c) = BY(c) - PolesY(bint5)*Coeff - PolesY(bint6)*Coeff2;
 351         BZ(c) = BZ(c) - PolesZ(bint5)*Coeff - PolesZ(bint6)*Coeff2;
 352       }
 353     }
 354
 355
 356     if (LastUCons == AppParCurves_TangencyPoint) {
 357       SurfTool::D1(Surf, U1, V0, Pt, VU, VV);
 358       bint += 2; bint7 = clav-classeV; bint8 = clav+1;
 359
 360       PolesX(bint8) = PolesX(clav) + VV.X()/DegV*(V1-V0);
 361       PolesY(bint8) = PolesY(clav) + VV.Y()/DegV*(V1-V0);
 362       PolesZ(bint8) = PolesZ(clav) + VV.Z()/DegV*(V1-V0);
 363
 364       PolesX(bint7) = PolesX(clav) - VU.X()/DegU*(U1-U0);
 365       PolesY(bint7) = PolesY(clav) - VU.Y()/DegU*(U1-U0);
 366       PolesZ(bint7) = PolesZ(clav) - VU.Z()/DegU*(U1-U0);
 367
 368       for (c = 1; c <= cla; c++) {
 369         Coeff = M(c, bint8); Coeff2 = M(c, bint7);
 370         BX(c) = BX(c)- PolesX(bint8)*Coeff - PolesX(bint7)*Coeff2;
 371         BY(c) = BY(c)- PolesY(bint8)*Coeff - PolesY(bint7)*Coeff2;
 372         BZ(c) = BZ(c)- PolesZ(bint8)*Coeff - PolesZ(bint7)*Coeff2;
 373       }
 374     }
 375
 376
 377     math_Vector B2X(bdeb, bfin-bint, 0.0);
 378     math_Vector B2Y(bdeb, bfin-bint, 0.0);
 379     math_Vector B2Z(bdeb, bfin-bint, 0.0);
 380
 381     Standard_Integer i2 = bdeb;
 382     for (i = bdeb; i <= bfin; i++) {
 383       if (i != bint1 && i != bint2 && i != bint3 && i != bint4 &&
 384           i != bint5 && i != bint6 && i != bint7 && i != bint8) {
 385         for (j = 1; j <= cla; j++) {
 386           Coeff = M(i, j);
 387           B2X(i2) = B2X(i2) + BX(j)*Coeff;
 388           B2Y(i2) = B2Y(i2) + BY(j)*Coeff;
 389           B2Z(i2) = B2Z(i2) + BZ(j)*Coeff;
 390         }
 391         i2 ++;
 392       }
 393     }
 394
 395     math_Matrix MP(1, cla, bdeb, bfin-bint);
 396     math_Matrix IBP(bdeb, bfin-bint, bdeb, bfin-bint);
 397
 398     Standard_Integer j2 = bdeb;
 399     for (i = 1; i <= cla; i++) {
 400       j2 = bdeb;
 401       for (j = bdeb; j <= bfin; j++) {
 402         if (j != bint1 && j != bint2 && j != bint3 && j != bint4 &&
 403             j != bint5 && j != bint6 && j != bint7 && j != bint8) {
 404           MP(i, j2) = M(i, j);
 405           j2++;
 406         }
 407       }
 408     }
 409     math_Matrix IBP1 = MP.Transposed()*MP;
 410     IBP = IBP1.Inverse();
 411
 412     i2 = bdeb;
 413     for (i = bdeb; i <= bfin; i++) {
 414       if (i != bint1 && i != bint2 && i != bint3 && i != bint4 &&
 415           i != bint5 && i != bint6 && i != bint7 && i != bint8) {
 416         for (j = bdeb; j <= bfin-bint; j++) {
 417           ISS = IBP(i2, j);
 418           PolesX(i) += ISS * B2X(j);
 419           PolesY(i) += ISS * B2Y(j);
 420           PolesZ(i) += ISS * B2Z(j);
 421         }
 422         i2++;
 423       }
 424     }
 425   }
 426
 427   for (j = 1; j <= classeV; j++) {
 428     for (i = 1; i <= classeU; i++) {
 429       Poles(i, j).SetCoord(PolesX(j+ (i-1)*classeV),
 430                            PolesY(j+ (i-1)*classeV),
 431                            PolesZ(j+ (i-1)*classeV));
 432     }
 433   }
 434
 435   SCU = new Geom_BezierSurface(Poles);
 436   Done = Standard_True;
 437 }
 438
 439
 440
 441 Standard_Boolean AppCont_SurfLeastSquare::IsDone() const
 442 {
 443   return Done;
 444 }
 445
 446
 447 const Handle(Geom_BezierSurface)& AppCont_SurfLeastSquare::Value()
 448 {
 449   return SCU;
 450 }
 451
 452
 453
 454 void AppCont_SurfLeastSquare::Error(Standard_Real& F,
 455                                     Standard_Real& MaxE3d) const
 456 {
 457
 458   Standard_Integer i, j, c, cu, cv, classeU = DegreU+1, classeV = DegreV+1;
 459   Standard_Real Coeff, err3d = 0.0;
 460   math_Matrix MyPointsX(1, Nbdiscret, 1, Nbdiscret);
 461   math_Matrix MyPointsY(1, Nbdiscret, 1, Nbdiscret);
 462   math_Matrix MyPointsZ(1, Nbdiscret, 1, Nbdiscret);
 463   MyPointsX = PointsX;
 464   MyPointsY = PointsY;
 465   MyPointsZ = PointsZ;
 466   MaxE3d = 0.0;
 467   F = 0.0;
 468   c = 0;
 469
 470   for (cu = 1; cu <= classeU; cu++) {
 471     for (cv = 1; cv <= classeV; cv++) {
 472       c++;
 473       for (i = 1; i <= Nbdiscret; i++) {
 474         for (j = 1; j <= Nbdiscret; j++) {
 475           Coeff = VBU(cu, i)*VBV(cv, j);
 476           MyPointsX(i, j) = MyPointsX(i, j) - PolesX(c)*Coeff;
 477           MyPointsY(i, j) = MyPointsY(i, j) - PolesY(c)*Coeff;
 478           MyPointsZ(i, j) = MyPointsZ(i, j) - PolesZ(c)*Coeff;
 479         }
 480       }
 481     }
 482   }
 483
 484
 485   for (i = 1; i <= Nbdiscret; i++) {
 486     for (j = 1; j <= Nbdiscret; j++) {
 487       err3d = MyPointsX(i, j) * MyPointsX(i, j) +
 488               MyPointsY(i, j) * MyPointsY(i, j) +
 489               MyPointsZ(i, j) * MyPointsZ(i, j);
 490       MaxE3d = Max(MaxE3d, err3d);
 491       F += err3d;
 492     }
 493   }
 494
 495   MaxE3d = Sqrt(MaxE3d);
 496
 497 }