src/FairCurve/FairCurve_MinimalVariation.cxx

   1 // Created on: 1996-02-26
   2 // Created by: Philippe MANGIN
   3 // Copyright (c) 1996-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_RangeError
  19 #define No_Standard_OutOfRange
  20 #endif
  21
  22 #include <FairCurve_MinimalVariation.ixx>
  23
  24 #include <BSplCLib.hxx>
  25 #include <PLib.hxx>
  26 #include <Geom2d_BSplineCurve.hxx>
  27 #include <FairCurve_BattenLaw.hxx>
  28 #include <FairCurve_EnergyOfMVC.hxx>
  29 #include <FairCurve_Newton.hxx>
  30 #include <math_Matrix.hxx>
  31 #include <Precision.hxx>
  32 #include <TColgp_HArray1OfPnt2d.hxx>
  33 #include <TColStd_HArray1OfInteger.hxx>
  34 #include <TColStd_HArray1OfReal.hxx>
  35
  36 //======================================================================================
  37 FairCurve_MinimalVariation::FairCurve_MinimalVariation(const gp_Pnt2d& P1,
  38                                                        const gp_Pnt2d& P2,
  39                                                        const Standard_Real Heigth,
  40                                                        const Standard_Real Slope,
  41                                                        const Standard_Real PhysicalRatio)
  42 //======================================================================================
  43                                        :FairCurve_Batten(P1, P2, Heigth, Slope),
  44                                         OldCurvature1(0), OldCurvature2(0),
  45                                         OldPhysicalRatio(PhysicalRatio),
  46                                         NewCurvature1(0),  NewCurvature2(0),
  47                                         NewPhysicalRatio(PhysicalRatio)
  48 {
  49 }
  50
  51 //======================================================================================
  52 Standard_Boolean FairCurve_MinimalVariation::Compute(FairCurve_AnalysisCode& ACode,
  53                                                      const Standard_Integer NbIterations,
  54                                                      const Standard_Real Tolerance)
  55 //======================================================================================
  56 {
  57   Standard_Boolean Ok=Standard_True, End=Standard_False;
  58   Standard_Real AngleMax = 0.7;      // parameter regulating the function of increment ( 40 degrees )
  59   Standard_Real AngleMin = 2*M_PI/100; // parameter regulating the function of increment
  60                                      // full passage should not contain more than 100 steps.
  61   Standard_Real DAngle1, DAngle2,  DRho1, DRho2, Ratio, Fraction, Toler;
  62   Standard_Real OldDist, NewDist;
  63
  64 //  Loop of Homotopy : calculation of the step and optimisation
  65
  66   while (Ok && !End) {
  67      DAngle1 = NewAngle1-OldAngle1;
  68      DAngle2 = NewAngle2-OldAngle2;
  69      DRho1 = NewCurvature1 - OldCurvature1;
  70      DRho2 = NewCurvature2 - OldCurvature2;
  71      Ratio = 1;
  72
  73      if (NewConstraintOrder1>0) {
  74         Fraction = Abs(DAngle1) / (AngleMax * Exp (-Abs(OldAngle1)/AngleMax) + AngleMin);
  75         if (Fraction > 1) Ratio = 1 / Fraction;
  76      }
  77      if (NewConstraintOrder2>0) {
  78         Fraction = Abs(DAngle2) / (AngleMax * Exp (-Abs(OldAngle2)/AngleMax) + AngleMin);
  79         if (Fraction > 1)  Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
  80      }
  81
  82      OldDist = OldP1.Distance(OldP2);
  83      NewDist = NewP1.Distance(NewP2);
  84      Fraction = Abs(OldDist-NewDist) / (OldDist/3);
  85      if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
  86
  87      if (NewConstraintOrder1>1) {
  88        Fraction = Abs(DRho1)*OldDist / (2+Abs(OldAngle1) + Abs(OldAngle2));
  89        if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
  90      }
  91
  92      if (NewConstraintOrder2>1) {
  93        Fraction = Abs(DRho2)*OldDist/ (2+Abs(OldAngle1) + Abs(OldAngle2));
  94        if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction);
  95      }
  96
  97      gp_Vec2d DeltaP1(OldP1, NewP1) , DeltaP2(OldP2, NewP2);
  98      if ( Ratio == 1) {
  99         End = Standard_True;
 100         Toler = Tolerance;
 101       }
 102      else {
 103        DeltaP1 *= Ratio;
 104        DeltaP2 *= Ratio;
 105        DAngle1 *= Ratio;
 106        DAngle2 *= Ratio;
 107        DRho1 *= Ratio;
 108        DRho2 *= Ratio;
 109        Toler =  10 * Tolerance;
 110      }
 111
 112      Ok = Compute( DeltaP1, DeltaP2,
 113                    DAngle1, DAngle2,
 114                    DRho1,   DRho2,
 115                    ACode,
 116                    NbIterations,
 117                    Toler);
 118
 119      if (ACode != FairCurve_OK) End = Standard_True;
 120      if (NewFreeSliding) NewSlidingFactor = OldSlidingFactor;
 121      if (NewConstraintOrder1 == 0) NewAngle1 = OldAngle1;
 122      if (NewConstraintOrder1 < 2)  NewCurvature1 = OldCurvature1;
 123      if (NewConstraintOrder2 == 0) NewAngle2 = OldAngle2;
 124      if (NewConstraintOrder2 < 2)  NewCurvature2 = OldCurvature2;
 125   }
 126   myCode = ACode;
 127   return Ok;
 128 }
 129
 130 //======================================================================================
 131 Standard_Boolean FairCurve_MinimalVariation::Compute(const gp_Vec2d& DeltaP1,
 132                                                      const gp_Vec2d& DeltaP2,
 133                                                      const Standard_Real DeltaAngle1,
 134                                                      const Standard_Real DeltaAngle2,
 135                                                      const Standard_Real DeltaCurvature1,
 136                                                      const Standard_Real DeltaCurvature2,
 137                                                            FairCurve_AnalysisCode& ACode,
 138                                                      const Standard_Integer NbIterations,
 139                                                      const Standard_Real Tolerance)
 140 //======================================================================================
 141 {
 142  Standard_Boolean Ok, OkCompute=Standard_True;
 143  ACode = FairCurve_OK;
 144
 145 // Deformation of the curve by adding a polynom of interpolation
 146    Standard_Integer L = 2 + NewConstraintOrder1 + NewConstraintOrder2,
 147                     kk, ii;
 148 //                    NbP1 = Poles->Length()-1, kk, ii;
 149 #ifdef OCCT_DEBUG
 150    Standard_Integer NbP1 =
 151 #endif
 152                            Poles->Length() ;
 153 #ifdef OCCT_DEBUG
 154    NbP1 = NbP1 - 1 ;
 155 #endif
 156    TColStd_Array1OfReal knots (1,2);
 157    knots(1) = 0;
 158    knots(2) = 1;
 159    TColStd_Array1OfInteger mults (1,2);
 160    TColgp_Array1OfPnt2d HermitePoles(1,L);
 161    TColgp_Array1OfPnt2d Interpolation(1,L);
 162    Handle(TColgp_HArray1OfPnt2d) NPoles = new  TColgp_HArray1OfPnt2d(1, Poles->Length());
 163
 164 // Polynomes of Hermite
 165    math_Matrix HermiteCoef(1, L, 1, L);
 166    Ok = PLib::HermiteCoefficients(0,1, NewConstraintOrder1,  NewConstraintOrder2,
 167                                   HermiteCoef);
 168    if (!Ok) return Standard_False;
 169
 170 // Definition of constraints of interpolation
 171    TColgp_Array1OfXY ADelta(1,L);
 172    gp_Vec2d VOld(OldP1, OldP2), VNew( -(OldP1.XY()+DeltaP1.XY()) + (OldP2.XY()+DeltaP2.XY()) );
 173    Standard_Real DAngleRef = VNew.Angle(VOld);
 174    Standard_Real DAngle1 = DeltaAngle1 - DAngleRef,
 175                  DAngle2 = DAngleRef   - DeltaAngle2; // Correction of Delta by the Delta induced by the points.
 176
 177
 178    ADelta(1) = DeltaP1.XY();
 179    kk = 2;
 180    if (NewConstraintOrder1>0) {
 181       // rotation of the derivative premiereDeltaAngle1
 182       gp_Vec2d OldDerive( Poles->Value(Poles->Lower()),
 183                           Poles->Value(Poles->Lower()+1) );
 184       OldDerive *= Degree / (Knots->Value(Knots->Lower()+1)-Knots->Value(Knots->Lower()) );
 185       ADelta(kk) = (OldDerive.Rotated(DAngle1) -  OldDerive).XY();
 186       kk += 1;
 187
 188       if (NewConstraintOrder1>1) {
 189          // rotation of the second derivative + adding
 190          gp_Vec2d OldSeconde( Poles->Value(Poles->Lower()).XY() + Poles->Value(Poles->Lower()+2).XY()
 191                             - 2*Poles->Value(Poles->Lower()+1).XY() );
 192          OldSeconde *=  Degree*( Degree-1)
 193                      /  pow (Knots->Value(Knots->Lower()+1)-Knots->Value(Knots->Lower()), 2);
 194          Standard_Real CPrim = OldDerive.Magnitude();
 195          ADelta(kk) = ( OldSeconde.Rotated(DAngle1) -  OldSeconde
 196                       + DeltaCurvature1*CPrim*OldDerive.Rotated(M_PI/2+DAngle1) ).XY();
 197          kk += 1;
 198       }
 199    }
 200    ADelta(kk) = DeltaP2.XY();
 201    kk += 1;
 202    if (NewConstraintOrder2>0) {
 203       gp_Vec2d OldDerive( Poles->Value(Poles->Upper()-1),
 204                           Poles->Value(Poles->Upper()) );
 205       OldDerive *= Degree / (Knots->Value(Knots->Upper()) - Knots->Value(Knots->Upper()-1) );
 206       ADelta(kk) = (OldDerive.Rotated(DAngle2) -  OldDerive).XY();
 207       kk += 1;
 208       if (NewConstraintOrder2>1) {
 209          // rotation of the second derivative + adding
 210          gp_Vec2d OldSeconde( Poles->Value(Poles->Upper()).XY() + Poles->Value(Poles->Upper()-2).XY()
 211                             - 2*Poles->Value(Poles->Upper()-1).XY() );
 212          OldSeconde *=  Degree*( Degree-1)
 213                      /  pow (Knots->Value(Knots->Upper())-Knots->Value(Knots->Upper()-1), 2);
 214          Standard_Real CPrim = OldDerive.Magnitude();
 215          ADelta(kk) = ( OldSeconde.Rotated(DAngle2) -  OldSeconde
 216                       + DeltaCurvature2*CPrim*OldDerive.Rotated(M_PI/2+DAngle2) ).XY();
 217          kk += 1;
 218       }
 219    }
 220
 221 // Interpolation
 222   gp_XY AuxXY (0,0);
 223   for (ii=1; ii<=L; ii++) {
 224       AuxXY.SetCoord(0.0, 0);
 225       for (kk=1; kk<=L; kk++) {
 226           AuxXY +=  HermiteCoef(kk, ii) * ADelta(kk);
 227       }
 228       Interpolation(ii).SetXY(AuxXY);
 229   }
 230 // Conversion into BSpline of the same structure as the current batten.
 231   PLib::CoefficientsPoles(Interpolation,  PLib::NoWeights(),
 232                           HermitePoles,  PLib::NoWeights());
 233
 234   mults.Init(L);
 235
 236   Handle(Geom2d_BSplineCurve) DeltaCurve =
 237     new  Geom2d_BSplineCurve( HermitePoles,
 238                               knots, mults, L-1);
 239
 240   DeltaCurve->IncreaseDegree(Degree);
 241   if (Mults->Length()>2) {
 242      DeltaCurve->InsertKnots(Knots->Array1(), Mults->Array1(), 1.e-10);
 243   }
 244
 245 // Summing
 246   DeltaCurve->Poles( NPoles->ChangeArray1() );
 247   for (kk= NPoles->Lower(); kk<=NPoles->Upper(); kk++) {
 248      NPoles->ChangeValue(kk).ChangeCoord() += Poles->Value(kk).Coord();
 249    }
 250
 251 // Intermediaires
 252
 253  Standard_Real Angle1, Angle2, SlidingLength,
 254                Alph1 =  OldAngle1 + DeltaAngle1,
 255                Alph2 =  OldAngle2 + DeltaAngle2,
 256                Rho1 =   OldCurvature1 + DeltaCurvature1,
 257                Rho2 =   OldCurvature2 + DeltaCurvature2,
 258                Dist  =  NPoles->Value(NPoles->Upper())
 259                       . Distance( NPoles->Value( NPoles->Lower() ) ),
 260                LReference = SlidingOfReference(Dist, Alph1, Alph2);
 261  gp_Vec2d Ox(1, 0),
 262                 P1P2 (  NPoles->Value(NPoles->Upper()).Coord()
 263                       - NPoles->Value(NPoles->Lower()).Coord() );
 264
 265 // Angles corresponding to axis ox
 266
 267  Angle1 =  Ox.Angle(P1P2) + Alph1;
 268  Angle2 = -Ox.Angle(P1P2) + Alph2;
 269
 270 // Calculation of the length of sliding (imposed or intial);
 271
 272  if (!NewFreeSliding) {
 273     SlidingLength = NewSlidingFactor * LReference;
 274   }
 275  else {
 276    if (OldFreeSliding) {
 277      SlidingLength = OldSlidingFactor *  LReference;
 278    }
 279    else {
 280      SlidingLength = SlidingOfReference(Dist, Alph1, Alph2);
 281    }
 282  }
 283
 284
 285
 286 // Energy and vectors of initialization
 287  FairCurve_BattenLaw LBatten (NewHeight, NewSlope, SlidingLength );
 288  FairCurve_EnergyOfMVC EMVC (Degree+1, Flatknots, NPoles,
 289                              NewConstraintOrder1,  NewConstraintOrder2,
 290                              LBatten, NewPhysicalRatio, SlidingLength, NewFreeSliding,
 291                              Angle1, Angle2, Rho1, Rho2);
 292  math_Vector VInit (1, EMVC.NbVariables());
 293
 294  // The value below gives an idea about the smallest value of the criterion of flexion.
 295  Standard_Real VConvex = 0.01 * pow(NewHeight / SlidingLength, 3);
 296  if (VConvex < 1.e-12) {VConvex = 1.e-12;}
 297
 298  Ok = EMVC.Variable(VInit);
 299
 300 // Minimisation
 301  FairCurve_Newton Newton(EMVC,
 302                          Tolerance*(P1P2.Magnitude()/10),
 303                          Tolerance,
 304                          NbIterations,
 305                          VConvex);
 306  Newton.Perform(EMVC, VInit);
 307  Ok = Newton.IsDone();
 308
 309  if (Ok) {
 310     gp_Vec2d Tangente, PseudoNormale;
 311     Poles = NPoles;
 312     Newton.Location(VInit);
 313
 314     if (NewFreeSliding) { OldSlidingFactor = VInit(VInit.Upper()) / LReference;}
 315     else                { OldSlidingFactor = NewSlidingFactor; }
 316
 317     if (NewConstraintOrder1 < 2) {
 318        Tangente.SetXY(  Poles->Value(Poles->Lower()+1).XY()
 319                       - Poles->Value(Poles->Lower()).XY() );
 320
 321        if (NewConstraintOrder1 == 0) {OldAngle1 = P1P2.Angle(Tangente);}
 322        else {OldAngle1 = Alph1;}
 323
 324        PseudoNormale.SetXY ( Poles->Value(Poles->Lower()).XY()
 325                             - 2 * Poles->Value(Poles->Lower()+1).XY()
 326                             + Poles->Value(Poles->Lower()+2).XY());
 327        OldCurvature1 = (((double)Degree-1) /Degree) * (Tangente.Normalized()^PseudoNormale)
 328                        / Tangente.SquareMagnitude();
 329      }
 330     else {OldCurvature1 = Rho1;
 331           OldAngle1 = Alph1; }
 332
 333     if (NewConstraintOrder2 < 2) {
 334        Tangente.SetXY ( Poles->Value(Poles->Upper()).XY()
 335                       - Poles->Value(Poles->Upper()-1).XY() );
 336        if (NewConstraintOrder2 == 0) OldAngle2 = (-Tangente).Angle(-P1P2);
 337        else { OldAngle2 = Alph2;}
 338        PseudoNormale.SetXY ( Poles->Value(Poles->Upper()).XY()
 339                             - 2 * Poles->Value(Poles->Upper()-1).XY()
 340                             + Poles->Value(Poles->Upper()-2).XY());
 341        OldCurvature2 = (((double)Degree-1) /Degree) * (Tangente.Normalized()^PseudoNormale)
 342                      / Tangente.SquareMagnitude();
 343      }
 344     else { OldAngle2 = Alph2;
 345            OldCurvature2 = Rho2;
 346     }
 347
 348     OldP1 = Poles->Value(Poles->Lower());
 349     OldP2 = Poles->Value(Poles->Upper());
 350     OldConstraintOrder1 = NewConstraintOrder1;
 351     OldConstraintOrder2 = NewConstraintOrder2;
 352     OldFreeSliding      = NewFreeSliding;
 353     OldSlope = NewSlope;
 354     OldHeight = NewHeight;
 355     OldPhysicalRatio =  NewPhysicalRatio;
 356   }
 357   else {
 358     Standard_Real V;
 359     ACode = EMVC.Status();
 360     if (!LBatten.Value(0, V) || !LBatten.Value(1, V)) {
 361        ACode = FairCurve_NullHeight;
 362     }
 363     else { OkCompute = Standard_False;}
 364     return OkCompute;
 365   }
 366
 367  Ok = EMVC.Variable(VInit);
 368
 369  // Processing of non convergence
 370  if (!Newton.IsConverged()) {
 371     ACode = FairCurve_NotConverged;
 372   }
 373
 374
 375  // Prevention of infinite sliding
 376  if (NewFreeSliding &&  VInit(VInit.Upper()) > 2*LReference) ACode = FairCurve_InfiniteSliding;
 377
 378
 379 // Eventual insertion of Nodes
 380  Standard_Boolean  NewKnots = Standard_False;
 381  Standard_Integer NbKnots = Knots->Length();
 382  Standard_Real ValAngles = (Abs(OldAngle1) +  Abs(OldAngle2)
 383                          + 2 * Abs(OldAngle2 - OldAngle1) ) ;
 384  while ( ValAngles > (2*(NbKnots-2) + 1)*(1+2*NbKnots) ) {
 385    NewKnots = Standard_True;
 386    NbKnots += NbKnots-1;
 387  }
 388
 389  if  (NewKnots) {
 390    Handle(Geom2d_BSplineCurve) NewBS =
 391     new  Geom2d_BSplineCurve( NPoles->Array1(), Knots->Array1(),
 392                               Mults->Array1(), Degree);
 393
 394    Handle(TColStd_HArray1OfInteger) NMults  =
 395       new TColStd_HArray1OfInteger (1,NbKnots);
 396    NMults->Init(Degree-3);
 397
 398     Handle(TColStd_HArray1OfReal) NKnots  =
 399       new TColStd_HArray1OfReal (1,NbKnots);
 400    for (ii=1; ii<=NbKnots; ii++) {
 401        NKnots->ChangeValue(ii) = (double) (ii-1) / (NbKnots-1);
 402    }
 403
 404    NewBS -> InsertKnots(NKnots->Array1(), NMults->Array1(), 1.e-10);
 405    Handle(TColgp_HArray1OfPnt2d) NPoles =
 406       new  TColgp_HArray1OfPnt2d(1, NewBS->NbPoles());
 407    NewBS -> Poles( NPoles->ChangeArray1() );
 408    NewBS -> Multiplicities( NMults->ChangeArray1() );
 409    NewBS -> Knots( NKnots->ChangeArray1() );
 410    Handle(TColStd_HArray1OfReal) FKnots  =
 411       new TColStd_HArray1OfReal (1, NewBS->NbPoles() + Degree+1);
 412    NewBS -> KnotSequence( FKnots->ChangeArray1());
 413
 414    Poles = NPoles;
 415    Mults = NMults;
 416    Knots = NKnots;
 417    Flatknots = FKnots;
 418  }
 419
 420
 421 // For eventual debug Newton.Dump(cout);
 422
 423  return OkCompute;
 424 }
 425
 426
 427 //======================================================================================
 428 void FairCurve_MinimalVariation::Dump(Standard_OStream& o) const
 429 //======================================================================================
 430 {
 431
 432 o << "  MVCurve      |"; o.width(7); o<< "Old " << " | " << "  New" << endl;
 433 o << "  P1    X      |"; o.width(7); o<<  OldP1.X() << " | " << NewP1.X() << endl;
 434 o << "        Y      |"; o.width(7); o<<  OldP1.Y() << " | " << NewP1.Y() << endl;
 435 o << "  P2    X      |"; o.width(7); o<<  OldP2.X() << " | " << NewP2.X() << endl;
 436 o << "        Y      |"; o.width(7); o<<  OldP2.Y() << " | " << NewP2.Y() << endl;
 437 o << "      Angle1   |"; o.width(7); o<<  OldAngle1 << " | " << NewAngle1 << endl;
 438 o << "      Angle2   |"; o.width(7); o<<  OldAngle2 << " | " << NewAngle2 << endl;
 439 o << " Curvature1    |"; o.width(7); o<<  OldCurvature1 << " | " << NewCurvature1 << endl;
 440 o << " Curvature2    |"; o.width(7); o<<  OldCurvature2 << " | " << NewCurvature2 << endl;
 441 o << "      Height   |"; o.width(7); o<<  OldHeight << " | " << NewHeight << endl;
 442 o << "      Slope    |"; o.width(7); o<<  OldSlope  << " | " << NewSlope << endl;
 443 o << " PhysicalRatio |"; o.width(7); o<<  OldPhysicalRatio << " | " << NewPhysicalRatio << endl;
 444 o << " SlidingFactor |"; o.width(7); o<<  OldSlidingFactor << " | " << NewSlidingFactor << endl;
 445 o << " FreeSliding   |"; o.width(7); o<<  OldFreeSliding << " | " << NewFreeSliding << endl;
 446 o << " ConstrOrder1  |"; o.width(7); o<<  OldConstraintOrder1 << " | " << NewConstraintOrder1 << endl;
 447 o << " ConstrOrder2  |"; o.width(7); o<<  OldConstraintOrder2 << " | " << NewConstraintOrder2 << endl;
 448  switch (myCode) {
 449    case  FairCurve_OK :
 450      o << "AnalysisCode : Ok" << endl;
 451      break;
 452    case  FairCurve_NotConverged :
 453      o << "AnalysisCode : NotConverged" << endl;
 454      break;
 455    case  FairCurve_InfiniteSliding :
 456      o << "AnalysisCode : InfiniteSliding" << endl;
 457      break;
 458    case  FairCurve_NullHeight :
 459      o << "AnalysisCode : NullHeight" << endl;
 460      break;
 461      }
 462 }
 463
 464
 465
 466
 467
 468
 469
 470
 471
 472