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
6 // This file is part of Open CASCADE Technology software library.
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.
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
18 #define No_Standard_RangeError
19 #define No_Standard_OutOfRange
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>
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>
41 //======================================================================================
42 FairCurve_MinimalVariation::FairCurve_MinimalVariation(const gp_Pnt2d& P1,
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)
56 //======================================================================================
57 Standard_Boolean FairCurve_MinimalVariation::Compute(FairCurve_AnalysisCode& ACode,
58 const Standard_Integer NbIterations,
59 const Standard_Real Tolerance)
60 //======================================================================================
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;
69 // Loop of Homotopy : calculation of the step and optimisation
72 DAngle1 = NewAngle1-OldAngle1;
73 DAngle2 = NewAngle2-OldAngle2;
74 DRho1 = NewCurvature1 - OldCurvature1;
75 DRho2 = NewCurvature2 - OldCurvature2;
78 if (NewConstraintOrder1>0) {
79 Fraction = Abs(DAngle1) / (AngleMax * Exp (-Abs(OldAngle1)/AngleMax) + AngleMin);
80 if (Fraction > 1) Ratio = 1 / Fraction;
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);
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);
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);
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);
102 gp_Vec2d DeltaP1(OldP1, NewP1) , DeltaP2(OldP2, NewP2);
114 Toler = 10 * Tolerance;
117 Ok = Compute( DeltaP1, DeltaP2,
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;
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 //======================================================================================
147 Standard_Boolean Ok, OkCompute=Standard_True;
148 ACode = FairCurve_OK;
150 // Deformation of the curve by adding a polynom of interpolation
151 Standard_Integer L = 2 + NewConstraintOrder1 + NewConstraintOrder2,
153 // NbP1 = Poles->Length()-1, kk, ii;
155 Standard_Integer NbP1 =
161 TColStd_Array1OfReal knots (1,2);
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());
169 // Polynomes of Hermite
170 math_Matrix HermiteCoef(1, L, 1, L);
171 Ok = PLib::HermiteCoefficients(0,1, NewConstraintOrder1, NewConstraintOrder2,
173 if (!Ok) return Standard_False;
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.
183 ADelta(1) = DeltaP1.XY();
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();
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();
205 ADelta(kk) = DeltaP2.XY();
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();
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();
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);
233 Interpolation(ii).SetXY(AuxXY);
235 // Conversion into BSpline of the same structure as the current batten.
236 PLib::CoefficientsPoles(Interpolation, PLib::NoWeights(),
237 HermitePoles, PLib::NoWeights());
241 Handle(Geom2d_BSplineCurve) DeltaCurve =
242 new Geom2d_BSplineCurve( HermitePoles,
245 DeltaCurve->IncreaseDegree(Degree);
246 if (Mults->Length()>2) {
247 DeltaCurve->InsertKnots(Knots->Array1(), Mults->Array1(), 1.e-10);
251 DeltaCurve->Poles( NPoles->ChangeArray1() );
252 for (kk= NPoles->Lower(); kk<=NPoles->Upper(); kk++) {
253 NPoles->ChangeValue(kk).ChangeCoord() += Poles->Value(kk).Coord();
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);
267 P1P2 ( NPoles->Value(NPoles->Upper()).Coord()
268 - NPoles->Value(NPoles->Lower()).Coord() );
270 // Angles corresponding to axis ox
272 Angle1 = Ox.Angle(P1P2) + Alph1;
273 Angle2 = -Ox.Angle(P1P2) + Alph2;
275 // Calculation of the length of sliding (imposed or intial);
277 if (!NewFreeSliding) {
278 SlidingLength = NewSlidingFactor * LReference;
281 if (OldFreeSliding) {
282 SlidingLength = OldSlidingFactor * LReference;
285 SlidingLength = SlidingOfReference(Dist, Alph1, Alph2);
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());
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;}
303 Ok = EMVC.Variable(VInit);
306 FairCurve_Newton Newton(EMVC,
307 Tolerance*(P1P2.Magnitude()/10),
311 Newton.Perform(EMVC, VInit);
312 Ok = Newton.IsDone();
315 gp_Vec2d Tangente, PseudoNormale;
317 Newton.Location(VInit);
319 if (NewFreeSliding) { OldSlidingFactor = VInit(VInit.Upper()) / LReference;}
320 else { OldSlidingFactor = NewSlidingFactor; }
322 if (NewConstraintOrder1 < 2) {
323 Tangente.SetXY( Poles->Value(Poles->Lower()+1).XY()
324 - Poles->Value(Poles->Lower()).XY() );
326 if (NewConstraintOrder1 == 0) {OldAngle1 = P1P2.Angle(Tangente);}
327 else {OldAngle1 = Alph1;}
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();
335 else {OldCurvature1 = Rho1;
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();
349 else { OldAngle2 = Alph2;
350 OldCurvature2 = Rho2;
353 OldP1 = Poles->Value(Poles->Lower());
354 OldP2 = Poles->Value(Poles->Upper());
355 OldConstraintOrder1 = NewConstraintOrder1;
356 OldConstraintOrder2 = NewConstraintOrder2;
357 OldFreeSliding = NewFreeSliding;
359 OldHeight = NewHeight;
360 OldPhysicalRatio = NewPhysicalRatio;
364 ACode = EMVC.Status();
365 if (!LBatten.Value(0, V) || !LBatten.Value(1, V)) {
366 ACode = FairCurve_NullHeight;
368 else { OkCompute = Standard_False;}
372 Ok = EMVC.Variable(VInit);
374 // Processing of non convergence
375 if (!Newton.IsConverged()) {
376 ACode = FairCurve_NotConverged;
380 // Prevention of infinite sliding
381 if (NewFreeSliding && VInit(VInit.Upper()) > 2*LReference) ACode = FairCurve_InfiniteSliding;
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;
395 Handle(Geom2d_BSplineCurve) NewBS =
396 new Geom2d_BSplineCurve( NPoles->Array1(), Knots->Array1(),
397 Mults->Array1(), Degree);
399 Handle(TColStd_HArray1OfInteger) NMults =
400 new TColStd_HArray1OfInteger (1,NbKnots);
401 NMults->Init(Degree-3);
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);
409 NewBS -> InsertKnots(NKnots->Array1(), NMults->Array1(), 1.e-10);
410 Handle(TColgp_HArray1OfPnt2d) NPoles =
411 new TColgp_HArray1OfPnt2d(1, NewBS->NbPoles());
412 NewBS -> Poles( NPoles->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());
426 // For eventual debug Newton.Dump(cout);
432 //======================================================================================
433 void FairCurve_MinimalVariation::Dump(Standard_OStream& o) const
434 //======================================================================================
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;
455 o << "AnalysisCode : Ok" << endl;
457 case FairCurve_NotConverged :
458 o << "AnalysisCode : NotConverged" << endl;
460 case FairCurve_InfiniteSliding :
461 o << "AnalysisCode : InfiniteSliding" << endl;
463 case FairCurve_NullHeight :
464 o << "AnalysisCode : NullHeight" << endl;