// File: FairCurve_Batten.cxx // Created: Mon Feb 5 09:34:00 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 // ================================================================== FairCurve_Batten::FairCurve_Batten(const gp_Pnt2d& P1, const gp_Pnt2d& P2, const Standard_Real Height, const Standard_Real Slope) // ================================================================== : myCode(FairCurve_OK), OldP1(P1), OldP2(P2), OldAngle1(0), OldAngle2(0), OldHeight(Height), OldSlope(Slope), OldSlidingFactor(1), OldFreeSliding(0), OldConstraintOrder1(1), OldConstraintOrder2(1), NewP1(P1), NewP2(P2), NewAngle1(0), NewAngle2(0), NewHeight(Height), NewSlope(Slope), NewSlidingFactor(1), NewFreeSliding(0), NewConstraintOrder1(1), NewConstraintOrder2(1), Degree(9) { if (P1.IsEqual(P2, Precision::Confusion())) Standard_NullValue::Raise("FairCurve : P1 and P2 are confused"); if (Height <= 0) Standard_NegativeValue::Raise("FairCurve : Height is no positive"); // // On initialise par une droite (2 poles) // Handle(TColStd_HArray1OfReal) Iknots = new TColStd_HArray1OfReal(1,2); Handle(TColStd_HArray1OfInteger) Imults = new TColStd_HArray1OfInteger(1,2); Handle(TColgp_HArray1OfPnt2d) Ipoles = new TColgp_HArray1OfPnt2d(1,2); Iknots->SetValue(1, 0); Iknots->SetValue(2, 1); Imults->SetValue(1, 2); Imults->SetValue(2, 2); Ipoles->SetValue(1, P1); Ipoles->SetValue(2, P2); // On incremente le degree Handle(TColgp_HArray1OfPnt2d) Npoles = new TColgp_HArray1OfPnt2d(1, Degree+1); Handle(TColStd_HArray1OfReal) Nweight = new TColStd_HArray1OfReal(1, 2); Handle(TColStd_HArray1OfReal) Nknots = new TColStd_HArray1OfReal(1, 2); Handle(TColStd_HArray1OfInteger) Nmults = new TColStd_HArray1OfInteger(1, 2); BSplCLib::IncreaseDegree (1, Degree, Standard_False, Ipoles->Array1(), BSplCLib::NoWeights(), Iknots->Array1(), Imults->Array1(), Npoles->ChangeArray1(), Nweight->ChangeArray1(), Nknots->ChangeArray1(), Nmults->ChangeArray1() ); // et on affecte le resultat dans nos champs Poles = Npoles; Knots = Nknots; Mults = Nmults; // calcul des noeuds "plats". Flatknots = new TColStd_HArray1OfReal (1, BSplCLib::KnotSequenceLength(Mults->Array1(), Degree, Standard_False)); BSplCLib::KnotSequence (Knots->Array1(), Mults->Array1(), Degree, Standard_False, Flatknots->ChangeArray1()); } // ================================================================== void FairCurve_Batten::Delete() {} // ================================================================== void FairCurve_Batten::Angles(const gp_Pnt2d& P1, const gp_Pnt2d& P2) // ================================================================== { gp_Vec2d VOld(NewP1, NewP2), VNew(P1, P2); Standard_Real Dangle = VOld.Angle(VNew); NewAngle1 -= Dangle; NewAngle2 += Dangle; } // ================================================================== void FairCurve_Batten::SetP1(const gp_Pnt2d& P1) // ================================================================== { if (P1.IsEqual(NewP2, Precision::Confusion())) Standard_NullValue::Raise("FairCurve : P1 and P2 are confused"); Angles(P1, NewP2); NewP1 = P1; } // ================================================================== void FairCurve_Batten::SetP2(const gp_Pnt2d& P2) // ================================================================== { if (NewP1.IsEqual(P2, Precision::Confusion())) Standard_NullValue::Raise("FairCurve : P1 and P2 are confused"); Angles(NewP1, P2); NewP2 = P2; } // ================================================================== Standard_Boolean FairCurve_Batten::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, Ratio, Fraction, Toler; Standard_Real OldDist, NewDist; // Boucle d'Homotopie : calcul du pas et optimisation while (Ok && !End) { DAngle1 = NewAngle1-OldAngle1; DAngle2 = NewAngle2-OldAngle2; 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); 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; Toler = 10 * Tolerance; } Ok = Compute( DeltaP1, DeltaP2, DAngle1, DAngle2, ACode, NbIterations, Toler); if (ACode != FairCurve_OK) End = Standard_True; if (NewFreeSliding) NewSlidingFactor = OldSlidingFactor; if (NewConstraintOrder1 == 0) NewAngle1 = OldAngle1; if (NewConstraintOrder2 == 0) NewAngle2 = OldAngle2; } myCode = ACode; return Ok; } // ============================================================================= Standard_Boolean FairCurve_Batten::Compute(const gp_Vec2d& DeltaP1, const gp_Vec2d& DeltaP2, const Standard_Real DeltaAngle1, const Standard_Real DeltaAngle2, 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; 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); ADelta(1) = DeltaP1.XY(); kk = 2; if (NewConstraintOrder1>0) { gp_Vec2d OldDerive( Poles->Value(Poles->Lower()), Poles->Value(Poles->Lower()+1) ); OldDerive *= Degree / (Knots->Value(2) - Knots->Value(1)); ADelta(kk) = (OldDerive.Rotated(DeltaAngle1-DAngleRef) - OldDerive).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(DAngleRef-DeltaAngle2) - OldDerive).XY(); } // 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, 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_EnergyOfBatten EBatten (Degree+1, Flatknots, NPoles, NewConstraintOrder1, NewConstraintOrder2, LBatten, SlidingLength, NewFreeSliding, Angle1, Angle2); math_Vector VInit (1, EBatten.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 = EBatten.Variable(VInit); // Minimisation FairCurve_Newton Newton(EBatten, Tolerance*P1P2.Magnitude()/10, Tolerance, NbIterations, VConvex); Newton.Perform(EBatten, VInit); Ok = Newton.IsDone(); if (Ok) { Poles = NPoles; Newton.Location(VInit); if (NewFreeSliding) { OldSlidingFactor = VInit(VInit.Upper()) / LReference;} else { OldSlidingFactor = NewSlidingFactor; } if (NewConstraintOrder1 == 0) { gp_Vec2d Tangente ( Poles->Value(Poles->Lower()+1).Coord() - Poles->Value(Poles->Lower()).Coord() ); OldAngle1 = P1P2.Angle(Tangente); } else {OldAngle1 = Alph1;} if (NewConstraintOrder2 == 0) { gp_Vec2d Tangente ( Poles->Value(Poles->Upper()).Coord() - Poles->Value(Poles->Upper()-1).Coord() ); OldAngle2 = (-Tangente).Angle(-P1P2); } else {OldAngle2 = Alph2;} OldP1 = Poles->Value(Poles->Lower()); OldP2 = Poles->Value(Poles->Upper()); OldConstraintOrder1 = NewConstraintOrder1; OldConstraintOrder2 = NewConstraintOrder2; OldFreeSliding = NewFreeSliding; OldSlope = NewSlope; OldHeight = NewHeight; } else { Standard_Real V; ACode = EBatten.Status(); if (!LBatten.Value(0, V) || !LBatten.Value(1, V)) { ACode = FairCurve_NullHeight; } else { OkCompute = Standard_False;} return OkCompute; } Ok = EBatten.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'eventuels debug // Newton.Dump(cout); return OkCompute; } // ================================================================== Standard_Real FairCurve_Batten::SlidingOfReference() const // ================================================================== { return SlidingOfReference(NewP1.Distance(NewP2), NewAngle1, NewAngle2 ); } // ================================================================== Standard_Real FairCurve_Batten::SlidingOfReference(const Standard_Real Dist, const Standard_Real Angle1, const Standard_Real Angle2) const // ================================================================== { Standard_Real a1, a2; // cas d'angle non contraints if ( (NewConstraintOrder1 == 0) && (NewConstraintOrder2 == 0)) return Dist; if (NewConstraintOrder1 == 0) a1 = Abs( Abs(NewAngle2)= 0 ) { return Compute(Dist, a1, a2); } // cas d'angle de signe opposes else { Standard_Real Ratio = a1 / ( a1 + a2 ); Standard_Real AngleMilieu = pow(1-Ratio,2) * a1 + pow(Ratio,2) * a2; if (AngleMilieu > PI/2) AngleMilieu = PI/2; return Ratio * Compute(Dist, a1, AngleMilieu ) + (1-Ratio) * Compute(Dist, a2, AngleMilieu ); } } // ================================================================== Standard_Real FairCurve_Batten::Compute(const Standard_Real Dist, const Standard_Real Angle1, const Standard_Real Angle2) const // ================================================================== { Standard_Real L1 = Compute(Dist, Angle1); Standard_Real L2 = Compute(Dist, Angle2), Aux; if (L1 < L2) { Aux = L2; L2 = L1; L1 = Aux; } return 0.3 * L1 + 0.7 * L2; } // ================================================================== Standard_Real FairCurve_Batten::Compute(const Standard_Real Dist, const Standard_Real Angle) const // ================================================================== { if (Angle < Precision::Angular() ) { return Dist; } // longeur du segment P1P2 if (Angle < PI/2) { return Angle*Dist / sin(Angle); } // longueur du cercle P1P2 respectant ANGLE if (Angle > PI) { return Sqrt(Angle*PI) * Dist;} else { return Angle * Dist; } // interpolation lineaire } // ================================================================== Handle(Geom2d_BSplineCurve) FairCurve_Batten::Curve() const // ================================================================== { Handle(Geom2d_BSplineCurve) TheCurve = new Geom2d_BSplineCurve ( Poles->Array1(), Knots->Array1(), Mults->Array1(), Degree); return TheCurve; } // ================================================================== void FairCurve_Batten::Dump(Standard_OStream& o) const // ================================================================== { o << " Batten |"; 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 << " Height |"; o.width(7); o<< OldHeight << " | " << NewHeight << endl; o << " Slope |"; o.width(7); o<< OldSlope << " | " << NewSlope << 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; } }