// File: FairCurve_MinimalVariation.cxx // Created: Mon Feb 26 13:54:16 1996 // Author: Philippe MANGIN #ifndef DEB #define No_Standard_RangeError #define No_Standard_OutOfRange #endif #include #include #include #include #include #include #include #include #include #include #include #include //====================================================================================== FairCurve_MinimalVariation::FairCurve_MinimalVariation(const gp_Pnt2d& P1, const gp_Pnt2d& P2, const Standard_Real Heigth, const Standard_Real Slope, const Standard_Real PhysicalRatio) //====================================================================================== :FairCurve_Batten(P1, P2, Heigth, Slope), OldCurvature1(0), OldCurvature2(0), OldPhysicalRatio(PhysicalRatio), NewCurvature1(0), NewCurvature2(0), NewPhysicalRatio(PhysicalRatio) { } //====================================================================================== Standard_Boolean FairCurve_MinimalVariation::Compute(FairCurve_AnalysisCode& ACode, const Standard_Integer NbIterations, const Standard_Real Tolerance) //====================================================================================== { Standard_Boolean Ok=Standard_True, End=Standard_False; Standard_Real AngleMax = 0.7; // parametre reglant la fonction d'increment // ( 40 degrees ) Standard_Real AngleMin = 2*PI/100; // parametre reglant la fonction d'increment // un tour complet ne doit pas couter plus de // 100 pas. Standard_Real DAngle1, DAngle2, DRho1, DRho2, Ratio, Fraction, Toler; Standard_Real OldDist, NewDist; // Boucle d'Homotopie : calcul du pas et optimisation while (Ok && !End) { DAngle1 = NewAngle1-OldAngle1; DAngle2 = NewAngle2-OldAngle2; DRho1 = NewCurvature1 - OldCurvature1; DRho2 = NewCurvature2 - OldCurvature2; Ratio = 1; if (NewConstraintOrder1>0) { Fraction = Abs(DAngle1) / (AngleMax * Exp (-Abs(OldAngle1)/AngleMax) + AngleMin); if (Fraction > 1) Ratio = 1 / Fraction; } if (NewConstraintOrder2>0) { Fraction = Abs(DAngle2) / (AngleMax * Exp (-Abs(OldAngle2)/AngleMax) + AngleMin); if (Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction); } OldDist = OldP1.Distance(OldP2); NewDist = NewP1.Distance(NewP2); Fraction = Abs(OldDist-NewDist) / (OldDist/3); if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction); if (NewConstraintOrder1>1) { Fraction = Abs(DRho1)*OldDist / (2+Abs(OldAngle1) + Abs(OldAngle2)); if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction); } if (NewConstraintOrder2>1) { Fraction = Abs(DRho2)*OldDist/ (2+Abs(OldAngle1) + Abs(OldAngle2)); if ( Fraction > 1) Ratio = (Ratio < 1 / Fraction ? Ratio : 1 / Fraction); } gp_Vec2d DeltaP1(OldP1, NewP1) , DeltaP2(OldP2, NewP2); if ( Ratio == 1) { End = Standard_True; Toler = Tolerance; } else { DeltaP1 *= Ratio; DeltaP2 *= Ratio; DAngle1 *= Ratio; DAngle2 *= Ratio; DRho1 *= Ratio; DRho2 *= Ratio; Toler = 10 * Tolerance; } Ok = Compute( DeltaP1, DeltaP2, DAngle1, DAngle2, DRho1, DRho2, ACode, NbIterations, Toler); if (ACode != FairCurve_OK) End = Standard_True; if (NewFreeSliding) NewSlidingFactor = OldSlidingFactor; if (NewConstraintOrder1 == 0) NewAngle1 = OldAngle1; if (NewConstraintOrder1 < 2) NewCurvature1 = OldCurvature1; if (NewConstraintOrder2 == 0) NewAngle2 = OldAngle2; if (NewConstraintOrder2 < 2) NewCurvature2 = OldCurvature2; } myCode = ACode; return Ok; } //====================================================================================== Standard_Boolean FairCurve_MinimalVariation::Compute(const gp_Vec2d& DeltaP1, const gp_Vec2d& DeltaP2, const Standard_Real DeltaAngle1, const Standard_Real DeltaAngle2, const Standard_Real DeltaCurvature1, const Standard_Real DeltaCurvature2, FairCurve_AnalysisCode& ACode, const Standard_Integer NbIterations, const Standard_Real Tolerance) //====================================================================================== { Standard_Boolean Ok, OkCompute=Standard_True; ACode = FairCurve_OK; // Deformation de la courbe par ajout d'un polynome d'interpolation Standard_Integer L = 2 + NewConstraintOrder1 + NewConstraintOrder2, kk, ii; // NbP1 = Poles->Length()-1, kk, ii; #ifdef DEB Standard_Integer NbP1 = #endif Poles->Length() ; #ifdef DEB NbP1 = NbP1 - 1 ; #endif TColStd_Array1OfReal knots (1,2); knots(1) = 0; knots(2) = 1; TColStd_Array1OfInteger mults (1,2); TColgp_Array1OfPnt2d HermitePoles(1,L); TColgp_Array1OfPnt2d Interpolation(1,L); Handle(TColgp_HArray1OfPnt2d) NPoles = new TColgp_HArray1OfPnt2d(1, Poles->Length()); // Polynomes d'Hermites math_Matrix HermiteCoef(1, L, 1, L); Ok = PLib::HermiteCoefficients(0,1, NewConstraintOrder1, NewConstraintOrder2, HermiteCoef); if (!Ok) return Standard_False; // Definition des contraintes d'interpolation TColgp_Array1OfXY ADelta(1,L); gp_Vec2d VOld(OldP1, OldP2), VNew( -(OldP1.XY()+DeltaP1.XY()) + (OldP2.XY()+DeltaP2.XY()) ); Standard_Real DAngleRef = VNew.Angle(VOld); Standard_Real DAngle1 = DeltaAngle1 - DAngleRef, DAngle2 = DAngleRef - DeltaAngle2; // Correction du Delta par le Delta induit par les points. ADelta(1) = DeltaP1.XY(); kk = 2; if (NewConstraintOrder1>0) { // rotation de la derive premiereDeltaAngle1 gp_Vec2d OldDerive( Poles->Value(Poles->Lower()), Poles->Value(Poles->Lower()+1) ); OldDerive *= Degree / (Knots->Value(Knots->Lower()+1)-Knots->Value(Knots->Lower()) ); ADelta(kk) = (OldDerive.Rotated(DAngle1) - OldDerive).XY(); kk += 1; if (NewConstraintOrder1>1) { // rotation de la derive seconde + ajout gp_Vec2d OldSeconde( Poles->Value(Poles->Lower()).XY() + Poles->Value(Poles->Lower()+2).XY() - 2*Poles->Value(Poles->Lower()+1).XY() ); OldSeconde *= Degree*( Degree-1) / pow (Knots->Value(Knots->Lower()+1)-Knots->Value(Knots->Lower()), 2); Standard_Real CPrim = OldDerive.Magnitude(); ADelta(kk) = ( OldSeconde.Rotated(DAngle1) - OldSeconde + DeltaCurvature1*CPrim*OldDerive.Rotated(PI/2+DAngle1) ).XY(); kk += 1; } } ADelta(kk) = DeltaP2.XY(); kk += 1; if (NewConstraintOrder2>0) { gp_Vec2d OldDerive( Poles->Value(Poles->Upper()-1), Poles->Value(Poles->Upper()) ); OldDerive *= Degree / (Knots->Value(Knots->Upper()) - Knots->Value(Knots->Upper()-1) ); ADelta(kk) = (OldDerive.Rotated(DAngle2) - OldDerive).XY(); kk += 1; if (NewConstraintOrder2>1) { // rotation de la derive seconde + ajout gp_Vec2d OldSeconde( Poles->Value(Poles->Upper()).XY() + Poles->Value(Poles->Upper()-2).XY() - 2*Poles->Value(Poles->Upper()-1).XY() ); OldSeconde *= Degree*( Degree-1) / pow (Knots->Value(Knots->Upper())-Knots->Value(Knots->Upper()-1), 2); Standard_Real CPrim = OldDerive.Magnitude(); ADelta(kk) = ( OldSeconde.Rotated(DAngle2) - OldSeconde + DeltaCurvature2*CPrim*OldDerive.Rotated(PI/2+DAngle2) ).XY(); kk += 1; } } // Interpolation gp_XY AuxXY (0,0); for (ii=1; ii<=L; ii++) { AuxXY.SetCoord(0.0, 0); for (kk=1; kk<=L; kk++) { AuxXY += HermiteCoef(kk, ii) * ADelta(kk); } Interpolation(ii).SetXY(AuxXY); } // Conversion en BSpline de meme structure que le batten courant. PLib::CoefficientsPoles(Interpolation, PLib::NoWeights(), HermitePoles, PLib::NoWeights()); mults.Init(L); Handle(Geom2d_BSplineCurve) DeltaCurve = new Geom2d_BSplineCurve( HermitePoles, knots, mults, L-1); DeltaCurve->IncreaseDegree(Degree); if (Mults->Length()>2) { DeltaCurve->InsertKnots(Knots->Array1(), Mults->Array1(), 1.e-10); } // Sommation DeltaCurve->Poles( NPoles->ChangeArray1() ); for (kk= NPoles->Lower(); kk<=NPoles->Upper(); kk++) { NPoles->ChangeValue(kk).ChangeCoord() += Poles->Value(kk).Coord(); } // Donnees intermediaires Standard_Real Angle1, Angle2, SlidingLength, Alph1 = OldAngle1 + DeltaAngle1, Alph2 = OldAngle2 + DeltaAngle2, Rho1 = OldCurvature1 + DeltaCurvature1, Rho2 = OldCurvature2 + DeltaCurvature2, Dist = NPoles->Value(NPoles->Upper()) . Distance( NPoles->Value( NPoles->Lower() ) ), LReference = SlidingOfReference(Dist, Alph1, Alph2); gp_Vec2d Ox(1, 0), P1P2 ( NPoles->Value(NPoles->Upper()).Coord() - NPoles->Value(NPoles->Lower()).Coord() ); // Angles par rapport a l'axe ox Angle1 = Ox.Angle(P1P2) + Alph1; Angle2 = -Ox.Angle(P1P2) + Alph2; // Calcul de la longeur de glissement (impose ou intiale); if (!NewFreeSliding) { SlidingLength = NewSlidingFactor * LReference; } else { if (OldFreeSliding) { SlidingLength = OldSlidingFactor * LReference; } else { SlidingLength = SlidingOfReference(Dist, Alph1, Alph2); } } // Energie et vecteurs d'initialisation FairCurve_BattenLaw LBatten (NewHeight, NewSlope, SlidingLength ); FairCurve_EnergyOfMVC EMVC (Degree+1, Flatknots, NPoles, NewConstraintOrder1, NewConstraintOrder2, LBatten, NewPhysicalRatio, SlidingLength, NewFreeSliding, Angle1, Angle2, Rho1, Rho2); math_Vector VInit (1, EMVC.NbVariables()); // La valeur ci-dessous donne une idee de la plus petie valeur propre // du critere de flexion. Standard_Real VConvex = 0.01 * pow(NewHeight / SlidingLength, 3); if (VConvex < 1.e-12) {VConvex = 1.e-12;} Ok = EMVC.Variable(VInit); // Minimisation FairCurve_Newton Newton(EMVC, Tolerance*(P1P2.Magnitude()/10), Tolerance, NbIterations, VConvex); Newton.Perform(EMVC, VInit); Ok = Newton.IsDone(); if (Ok) { gp_Vec2d Tangente, PseudoNormale; Poles = NPoles; Newton.Location(VInit); if (NewFreeSliding) { OldSlidingFactor = VInit(VInit.Upper()) / LReference;} else { OldSlidingFactor = NewSlidingFactor; } if (NewConstraintOrder1 < 2) { Tangente.SetXY( Poles->Value(Poles->Lower()+1).XY() - Poles->Value(Poles->Lower()).XY() ); if (NewConstraintOrder1 == 0) {OldAngle1 = P1P2.Angle(Tangente);} else {OldAngle1 = Alph1;} PseudoNormale.SetXY ( Poles->Value(Poles->Lower()).XY() - 2 * Poles->Value(Poles->Lower()+1).XY() + Poles->Value(Poles->Lower()+2).XY()); OldCurvature1 = (((double)Degree-1) /Degree) * (Tangente.Normalized()^PseudoNormale) / Tangente.SquareMagnitude(); } else {OldCurvature1 = Rho1; OldAngle1 = Alph1; } if (NewConstraintOrder2 < 2) { Tangente.SetXY ( Poles->Value(Poles->Upper()).XY() - Poles->Value(Poles->Upper()-1).XY() ); if (NewConstraintOrder2 == 0) OldAngle2 = (-Tangente).Angle(-P1P2); else { OldAngle2 = Alph2;} PseudoNormale.SetXY ( Poles->Value(Poles->Upper()).XY() - 2 * Poles->Value(Poles->Upper()-1).XY() + Poles->Value(Poles->Upper()-2).XY()); OldCurvature2 = (((double)Degree-1) /Degree) * (Tangente.Normalized()^PseudoNormale) / Tangente.SquareMagnitude(); } else { OldAngle2 = Alph2; OldCurvature2 = Rho2; } OldP1 = Poles->Value(Poles->Lower()); OldP2 = Poles->Value(Poles->Upper()); OldConstraintOrder1 = NewConstraintOrder1; OldConstraintOrder2 = NewConstraintOrder2; OldFreeSliding = NewFreeSliding; OldSlope = NewSlope; OldHeight = NewHeight; OldPhysicalRatio = NewPhysicalRatio; } else { Standard_Real V; ACode = EMVC.Status(); if (!LBatten.Value(0, V) || !LBatten.Value(1, V)) { ACode = FairCurve_NullHeight; } else { OkCompute = Standard_False;} return OkCompute; } Ok = EMVC.Variable(VInit); // Traitement de la non convergence if (!Newton.IsConverged()) { ACode = FairCurve_NotConverged; } // Prevention du glissement infinie if (NewFreeSliding && VInit(VInit.Upper()) > 2*LReference) ACode = FairCurve_InfiniteSliding; // Insertion eventuelle de Noeuds Standard_Boolean NewKnots = Standard_False; Standard_Integer NbKnots = Knots->Length(); Standard_Real ValAngles = (Abs(OldAngle1) + Abs(OldAngle2) + 2 * Abs(OldAngle2 - OldAngle1) ) ; while ( ValAngles > (2*(NbKnots-2) + 1)*(1+2*NbKnots) ) { NewKnots = Standard_True; NbKnots += NbKnots-1; } if (NewKnots) { Handle(Geom2d_BSplineCurve) NewBS = new Geom2d_BSplineCurve( NPoles->Array1(), Knots->Array1(), Mults->Array1(), Degree); Handle(TColStd_HArray1OfInteger) NMults = new TColStd_HArray1OfInteger (1,NbKnots); NMults->Init(Degree-3); Handle(TColStd_HArray1OfReal) NKnots = new TColStd_HArray1OfReal (1,NbKnots); for (ii=1; ii<=NbKnots; ii++) { NKnots->ChangeValue(ii) = (double) (ii-1) / (NbKnots-1); } NewBS -> InsertKnots(NKnots->Array1(), NMults->Array1(), 1.e-10); Handle(TColgp_HArray1OfPnt2d) NPoles = new TColgp_HArray1OfPnt2d(1, NewBS->NbPoles()); NewBS -> Poles( NPoles->ChangeArray1() ); NewBS -> Multiplicities( NMults->ChangeArray1() ); NewBS -> Knots( NKnots->ChangeArray1() ); Handle(TColStd_HArray1OfReal) FKnots = new TColStd_HArray1OfReal (1, NewBS->NbPoles() + Degree+1); NewBS -> KnotSequence( FKnots->ChangeArray1()); Poles = NPoles; Mults = NMults; Knots = NKnots; Flatknots = FKnots; } // Pour d'eventuelle debug // Newton.Dump(cout); return OkCompute; } //====================================================================================== void FairCurve_MinimalVariation::Dump(Standard_OStream& o) const //====================================================================================== { o << " MVCurve |"; o.width(7); o<< "Old " << " | " << " New" << endl; o << " P1 X |"; o.width(7); o<< OldP1.X() << " | " << NewP1.X() << endl; o << " Y |"; o.width(7); o<< OldP1.Y() << " | " << NewP1.Y() << endl; o << " P2 X |"; o.width(7); o<< OldP2.X() << " | " << NewP2.X() << endl; o << " Y |"; o.width(7); o<< OldP2.Y() << " | " << NewP2.Y() << endl; o << " Angle1 |"; o.width(7); o<< OldAngle1 << " | " << NewAngle1 << endl; o << " Angle2 |"; o.width(7); o<< OldAngle2 << " | " << NewAngle2 << endl; o << " Curvature1 |"; o.width(7); o<< OldCurvature1 << " | " << NewCurvature1 << endl; o << " Curvature2 |"; o.width(7); o<< OldCurvature2 << " | " << NewCurvature2 << endl; o << " Height |"; o.width(7); o<< OldHeight << " | " << NewHeight << endl; o << " Slope |"; o.width(7); o<< OldSlope << " | " << NewSlope << endl; o << " PhysicalRatio |"; o.width(7); o<< OldPhysicalRatio << " | " << NewPhysicalRatio << endl; o << " SlidingFactor |"; o.width(7); o<< OldSlidingFactor << " | " << NewSlidingFactor << endl; o << " FreeSliding |"; o.width(7); o<< OldFreeSliding << " | " << NewFreeSliding << endl; o << " ConstrOrder1 |"; o.width(7); o<< OldConstraintOrder1 << " | " << NewConstraintOrder1 << endl; o << " ConstrOrder2 |"; o.width(7); o<< OldConstraintOrder2 << " | " << NewConstraintOrder2 << endl; switch (myCode) { case FairCurve_OK : o << "AnalysisCode : Ok" << endl; break; case FairCurve_NotConverged : o << "AnalysisCode : NotConverged" << endl; break; case FairCurve_InfiniteSliding : o << "AnalysisCode : InfiniteSliding" << endl; break; case FairCurve_NullHeight : o << "AnalysisCode : NullHeight" << endl; break; } }