src/AppParCurves/AppParCurves_BSpGradient.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 11/09/91
  20
  21
  22 // Application de la methode du gradient corrige pour minimiser
  23 // F  = somme(||C(ui, Poles(ui)) - ptli||2.
  24 // La methode de gradient conjugue est programmee dans la bibliotheque
  25 // mathematique: math_BFGS.
  26
  27
  28 #define No_Standard_RangeError
  29 #define No_Standard_OutOfRange
  30
  31 #include <AppParCurves_Constraint.hxx>
  32 #include <AppParCurves_ConstraintCouple.hxx>
  33 #include <math_BFGS.hxx>
  34 #include <StdFail_NotDone.hxx>
  35 #include <AppParCurves_MultiPoint.hxx>
  36 #include <gp_Pnt.hxx>
  37 #include <gp_Pnt2d.hxx>
  38 #include <gp_Vec.hxx>
  39 #include <gp_Vec2d.hxx>
  40 #include <TColgp_Array1OfPnt.hxx>
  41 #include <TColgp_Array1OfPnt2d.hxx>
  42 #include <TColgp_Array1OfVec.hxx>
  43 #include <TColgp_Array1OfVec2d.hxx>
  44
  45 #include <OSD_Chronometer.hxx>
  46
  47 static OSD_Chronometer chr1;
  48
  49
  50 static Standard_Boolean islambdadefined = Standard_False;
  51
  52
  53
  54 static AppParCurves_Constraint FirstConstraint
  55   (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
  56    const Standard_Integer FirstPoint)
  57 {
  58   Standard_Integer i, myindex;
  59   Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper();
  60   AppParCurves_ConstraintCouple mycouple;
  61   AppParCurves_Constraint Cons = AppParCurves_NoConstraint;
  62
  63   for (i = low; i <= upp; i++) {
  64     mycouple = TheConstraints->Value(i);
  65     Cons = mycouple.Constraint();
  66     myindex = mycouple.Index();
  67     if (myindex == FirstPoint) {
  68       break;
  69     }
  70   }
  71   return Cons;
  72 }
  73
  74
  75 static AppParCurves_Constraint LastConstraint
  76   (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
  77    const Standard_Integer LastPoint)
  78 {
  79   Standard_Integer i, myindex;
  80   Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper();
  81   AppParCurves_ConstraintCouple mycouple;
  82   AppParCurves_Constraint Cons = AppParCurves_NoConstraint;
  83
  84   for (i = low; i <= upp; i++) {
  85     mycouple = TheConstraints->Value(i);
  86     Cons = mycouple.Constraint();
  87     myindex = mycouple.Index();
  88     if (myindex == LastPoint) {
  89       break;
  90     }
  91   }
  92   return Cons;
  93 }
  94
  95
  96
  97
  98
  99 AppParCurves_BSpGradient::
 100    AppParCurves_BSpGradient(const MultiLine& SSP,
 101          const Standard_Integer FirstPoint,
 102          const Standard_Integer LastPoint,
 103          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
 104          math_Vector& Parameters,
 105          const TColStd_Array1OfReal& Knots,
 106          const TColStd_Array1OfInteger& Mults,
 107          const Standard_Integer Deg,
 108          const Standard_Real Tol3d,
 109          const Standard_Real Tol2d,
 110          const Standard_Integer NbIterations):
 111          ParError(FirstPoint, LastPoint,0.0)
 112 {
 113   Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters,
 114           Knots, Mults, Deg, Tol3d, Tol2d, NbIterations);
 115 }
 116
 117
 118 AppParCurves_BSpGradient::
 119    AppParCurves_BSpGradient(const MultiLine& SSP,
 120          const Standard_Integer FirstPoint,
 121          const Standard_Integer LastPoint,
 122          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
 123          math_Vector& Parameters,
 124          const TColStd_Array1OfReal& Knots,
 125          const TColStd_Array1OfInteger& Mults,
 126          const Standard_Integer Deg,
 127          const Standard_Real Tol3d,
 128          const Standard_Real Tol2d,
 129          const Standard_Integer NbIterations,
 130          const Standard_Real lambda1,
 131          const Standard_Real lambda2):
 132          ParError(FirstPoint, LastPoint,0.0)
 133 {
 134   mylambda1 = lambda1;
 135   mylambda2 = lambda2;
 136   islambdadefined = Standard_True;
 137   Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters,
 138           Knots, Mults, Deg, Tol3d, Tol2d, NbIterations);
 139 }
 140
 141
 142
 143
 144
 145
 146
 147 void AppParCurves_BSpGradient::
 148   Perform(const MultiLine& SSP,
 149          const Standard_Integer FirstPoint,
 150          const Standard_Integer LastPoint,
 151          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
 152          math_Vector& Parameters,
 153          const TColStd_Array1OfReal& Knots,
 154          const TColStd_Array1OfInteger& Mults,
 155          const Standard_Integer Deg,
 156          const Standard_Real Tol3d,
 157          const Standard_Real Tol2d,
 158          const Standard_Integer NbIterations)
 159 {
 160
 161 //  Standard_Boolean grad = Standard_True;
 162   Standard_Integer i, j, k, i2, l;
 163   Standard_Real UF, DU, Fval = 0.0, FU, DFU;
 164   Standard_Integer nbP3d = ToolLine::NbP3d(SSP);
 165   Standard_Integer nbP2d = ToolLine::NbP2d(SSP);
 166   Standard_Integer mynbP3d=nbP3d, mynbP2d=nbP2d;
 167   Standard_Integer nbP = nbP3d + nbP2d;
 168 //  gp_Pnt Pt, P1, P2;
 169   gp_Pnt Pt;
 170 //  gp_Pnt2d Pt2d, P12d, P22d;
 171   gp_Pnt2d Pt2d;
 172 //  gp_Vec V1, V2, MyV;
 173   gp_Vec V1, MyV;
 174 //  gp_Vec2d V12d, V22d, MyV2d;
 175   gp_Vec2d V12d, MyV2d;
 176   Done = Standard_False;
 177
 178   if (nbP3d == 0) mynbP3d = 1;
 179   if (nbP2d == 0) mynbP2d = 1;
 180   TColgp_Array1OfPnt TabP(1, mynbP3d);
 181   TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
 182   TColgp_Array1OfVec TabV(1, mynbP3d);
 183   TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
 184
 185   // Calcul de la fonction F= somme(||C(ui)-Ptli||2):
 186   // Appel a une fonction heritant de MultipleVarFunctionWithGradient
 187   // pour calculer F et grad_F.
 188   // ================================================================
 189
 190   Standard_Integer nbpoles = -Deg -1;
 191   for (i = Mults.Lower() ;i <= Mults.Upper(); i++) {
 192     nbpoles += Mults(i);
 193   }
 194
 195   TColgp_Array1OfPnt   TabPole(1, nbpoles);
 196   TColgp_Array1OfPnt2d TabPole2d(1, nbpoles);
 197   TColgp_Array1OfPnt    ThePoles(1, (nbpoles)*mynbP3d);
 198   TColgp_Array1OfPnt2d  ThePoles2d(1, (nbpoles)*mynbP2d);
 199
 200
 201   AppParCurves_Constraint
 202     FirstCons = FirstConstraint(TheConstraints, FirstPoint),
 203     LastCons  = LastConstraint(TheConstraints, LastPoint);
 204
 205
 206   AppParCurves_BSpParFunction MyF(SSP, FirstPoint,LastPoint, TheConstraints,
 207                                   Parameters, Knots, Mults, nbpoles);
 208
 209
 210
 211   if (FirstCons >= AppParCurves_TangencyPoint ||
 212       LastCons >= AppParCurves_TangencyPoint) {
 213
 214     if (!islambdadefined) {
 215       AppParCurves_BSpParLeastSquare thefitt(SSP, Knots, Mults, FirstPoint,
 216                                              LastPoint, FirstCons, LastCons,
 217                                              Parameters, nbpoles);
 218       if (FirstCons >= AppParCurves_TangencyPoint) {
 219         mylambda1 = thefitt.FirstLambda();
 220         MyF.SetFirstLambda(mylambda1);
 221       }
 222       if (LastCons >= AppParCurves_TangencyPoint) {
 223         mylambda2 = thefitt.LastLambda();
 224         MyF.SetLastLambda(mylambda2);
 225       }
 226     }
 227     else {
 228       MyF.SetFirstLambda(mylambda1);
 229       MyF.SetLastLambda(mylambda2);
 230     }
 231   }
 232
 233
 234   MyF.Value(Parameters, Fval);
 235   MError3d = MyF.MaxError3d();
 236   MError2d = MyF.MaxError2d();
 237   SCU = MyF.CurveValue();
 238
 239   if (MError3d > Tol3d || MError2d > Tol2d) {
 240
 241     // Stockage des Poles des courbes pour projeter:
 242     // ============================================
 243     i2 = 0;
 244     for (k = 1; k <= nbP3d; k++) {
 245       SCU.Curve(k, TabPole);
 246       for (j=1; j<=nbpoles; j++) ThePoles(j+i2) = TabPole(j);
 247       i2 += nbpoles;
 248     }
 249     i2 = 0;
 250     for (k = 1; k <= nbP2d; k++) {
 251       SCU.Curve(nbP3d+k, TabPole2d);
 252       for (j=1; j<=nbpoles; j++) ThePoles2d(j+i2) = TabPole2d(j);
 253       i2 += nbpoles;
 254     }
 255
 256     //  Une iteration rapide de projection est faite par la methode de
 257     //  Rogers & Fog 89, methode equivalente a Hoschek 88 qui ne necessite pas
 258     //  le calcul de D2.
 259
 260     const math_Matrix& A = MyF.FunctionMatrix();
 261     const math_Matrix& DA = MyF.DerivativeFunctionMatrix();
 262     const math_IntegerVector& Index = MyF.Index();
 263     Standard_Real aa, da, a, b, c, d , e , f, px, py, pz;
 264     Standard_Integer indexdeb, indexfin;
 265
 266     for (j = FirstPoint+1; j <= LastPoint-1; j++) {
 267
 268       UF = Parameters(j);
 269       if (nbP3d != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP, TabP2d);
 270       else if (nbP2d != 0)          ToolLine::Value(SSP, j, TabP2d);
 271       else                          ToolLine::Value(SSP, j, TabP);
 272
 273       FU = 0.0;
 274       DFU = 0.0;
 275       i2 = 0;
 276
 277       indexdeb = Index(j) + 1;
 278       indexfin = indexdeb + Deg;
 279
 280       for (k = 1; k <= nbP3d; k++) {
 281         a = b = c = d = e = f = 0.0;
 282         for (l = indexdeb; l <= indexfin; l++) {
 283           Pt = ThePoles(l+i2);
 284           px = Pt.X(); py = Pt.Y(); pz = Pt.Z();
 285           aa = A(j, l); da = DA(j, l);
 286           a += aa* px; d += da* px;
 287           b += aa* py; e += da* py;
 288           c += aa* pz; f += da* pz;
 289         }
 290         Pt.SetCoord(a, b, c);
 291         V1.SetCoord(d, e, f);
 292         i2 += nbpoles;
 293
 294         MyV = gp_Vec(Pt, TabP(k));
 295         FU += MyV*V1;
 296         DFU += V1.SquareMagnitude();
 297       }
 298       i2 = 0;
 299       for (k = 1; k <= nbP2d; k++) {
 300         a = b = d = e = 0.0;
 301         for (l = indexdeb; l <= indexfin; l++) {
 302           Pt2d = ThePoles2d(l+i2);
 303           px = Pt2d.X(); py = Pt2d.Y();
 304           aa = A(j, l); da = DA(j, l);
 305           a += aa* px; d += da* px;
 306           b += aa* py; e += da* py;
 307         }
 308         Pt2d.SetCoord(a, b);
 309         V12d.SetCoord(d, e);
 310         i2 += nbpoles;
 311
 312         MyV2d = gp_Vec2d(Pt2d, TabP2d(k));
 313         FU += MyV2d*V12d;
 314         DFU += V12d.SquareMagnitude();
 315       }
 316
 317       if (DFU >= RealEpsilon()) {
 318         DU = FU/DFU;
 319         DU = Sign(Min(5.e-02, Abs(DU)), DU);
 320         UF += DU;
 321         Parameters(j) = UF;
 322       }
 323     }
 324
 325     MyF.Value(Parameters, Fval);
 326     MError3d = MyF.MaxError3d();
 327     MError2d = MyF.MaxError2d();
 328   }
 329
 330
 331   if (MError3d<= Tol3d && MError2d <= Tol2d) {
 332     Done = Standard_True;
 333   }
 334   else if (NbIterations != 0) {
 335     // NbIterations de gradient conjugue:
 336     // =================================
 337     Standard_Real Eps = 1.e-07;
 338     AppParCurves_BSpGradient_BFGS FResol(MyF, Parameters, Tol3d,
 339                                          Tol2d, Eps, NbIterations);
 340   }
 341
 342   SCU = MyF.CurveValue();
 343
 344
 345   AvError = 0.;
 346   for (j = FirstPoint; j <= LastPoint; j++) {
 347     Parameters(j) = MyF.NewParameters()(j);
 348     // Recherche des erreurs maxi et moyenne a un index donne:
 349     for (k = 1; k <= nbP; k++) {
 350       ParError(j) = Max(ParError(j), MyF.Error(j, k));
 351     }
 352     AvError += ParError(j);
 353   }
 354   AvError = AvError/(LastPoint-FirstPoint+1);
 355
 356
 357   MError3d = MyF.MaxError3d();
 358   MError2d = MyF.MaxError2d();
 359   if (MError3d <= Tol3d && MError2d <= Tol2d) {
 360     Done = Standard_True;
 361   }
 362
 363 }
 364
 365
 366
 367 AppParCurves_MultiBSpCurve AppParCurves_BSpGradient::Value() const {
 368   return SCU;
 369 }
 370
 371
 372 Standard_Boolean AppParCurves_BSpGradient::IsDone() const {
 373   return Done;
 374 }
 375
 376
 377 Standard_Real AppParCurves_BSpGradient::Error(const Standard_Integer Index) const {
 378   return ParError(Index);
 379 }
 380
 381 Standard_Real AppParCurves_BSpGradient::AverageError() const {
 382   return AvError;
 383 }
 384
 385 Standard_Real AppParCurves_BSpGradient::MaxError3d() const {
 386   return MError3d;
 387 }
 388
 389 Standard_Real AppParCurves_BSpGradient::MaxError2d() const {
 390   return MError2d;
 391 }
 392
 393