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