1 // Created on: 1996-01-22
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 <FairCurve_Energy.hxx>
24 #include <gp_Pnt2d.hxx>
25 #include <gp_Vec2d.hxx>
26 #include <math_IntegerVector.hxx>
27 #include <math_Matrix.hxx>
29 //=======================================================================
30 FairCurve_Energy::FairCurve_Energy(const Handle(TColgp_HArray1OfPnt2d)& Poles,
31 const Standard_Integer ContrOrder1,
32 const Standard_Integer ContrOrder2,
33 const Standard_Boolean WithAuxValue,
34 const Standard_Real Angle1,
35 const Standard_Real Angle2,
36 const Standard_Integer Degree,
37 const Standard_Real Curvature1,
38 const Standard_Real Curvature2 )
39 //=======================================================================
41 MyContrOrder1(ContrOrder1),
42 MyContrOrder2(ContrOrder2),
43 MyWithAuxValue(WithAuxValue),
44 MyNbVar(2*(Poles->Length()-2) - ContrOrder2 - ContrOrder1 + WithAuxValue),
45 MyNbValues(2*Poles->Length() + WithAuxValue),
48 MyGradient( 0, MyNbValues),
49 MyHessian( 0, MyNbValues + MyNbValues*(MyNbValues+1)/2 )
51 // chesk angles in reference (Ox,Oy)
52 gp_XY L0 (Cos(Angle1), Sin(Angle1)), L1 (-Cos(Angle2), Sin(Angle2));
53 MyLinearForm.SetValue(0, L0);
54 MyLinearForm.SetValue(1, L1);
55 gp_XY Q0(-Sin(Angle1), Cos(Angle1)), Q1 (Sin(Angle2), Cos(Angle2));
56 MyQuadForm.SetValue(0, ((double)Degree) / (Degree-1) * Curvature1 * Q0);
57 MyQuadForm.SetValue(1, ((double)Degree) / (Degree-1) * Curvature2 * Q1);
60 //=======================================================================
61 Standard_Boolean FairCurve_Energy::Value(const math_Vector& X,
63 //=======================================================================
65 Standard_Boolean IsDone;
66 math_Vector Energie(0,0);
68 IsDone = Compute(0, Energie);
73 //=======================================================================
74 Standard_Boolean FairCurve_Energy::Gradient(const math_Vector& X,
76 //=======================================================================
78 Standard_Boolean IsDone;
81 IsDone = Values(X, E, G);
85 //=======================================================================
86 void FairCurve_Energy::Gradient1(const math_Vector& Vect,
88 //=======================================================================
91 DebG = Grad.Lower(), FinG = Grad.Upper();
92 Standard_Integer Vdeb = 3,
93 Vfin = 2*MyPoles->Length()-2;
95 // .... by calculation
96 if (MyContrOrder1 >= 1) {
97 gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
98 Grad(DebG) = MyLinearForm(0) * DPole;
102 if(MyContrOrder1 == 2) {
103 Standard_Real Lambda0 = MyPoles->Value(MyPoles->Lower())
104 .Distance( MyPoles->Value(MyPoles->Lower()+1) );
105 gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
106 Grad(DebG-1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * DPole;
107 Grad(DebG) = MyLinearForm(0) * DPole;
111 if (MyWithAuxValue) {
112 Grad(FinG) = Vect( 2*MyPoles->Length()+1 );
115 if (MyContrOrder2 >= 1) {
116 gp_XY DPole (Vect(Vfin-1), Vect(Vfin));
117 Grad(FinG) = MyLinearForm(1) * DPole;
120 if(MyContrOrder2 == 2) {
121 Standard_Real Lambda1 = MyPoles->Value(MyPoles->Upper())
122 .Distance(MyPoles->Value(MyPoles->Upper()-1) );
123 gp_XY DPole (Vect(Vfin-3), Vect(Vfin-2));
124 Grad(FinG) = Grad(FinG+1) + (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * DPole;
125 Grad(FinG+1) = MyLinearForm(1) * DPole;
129 for (ii=DebG; ii<=FinG; ii++) {
130 Grad(ii) = Vect(Vdeb);
135 //=======================================================================
136 Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
139 //=======================================================================
141 Standard_Boolean IsDone;
144 IsDone = Compute(1, MyGradient);
147 Gradient1(MyGradient, G);
152 //=======================================================================
153 Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
157 //=======================================================================
159 Standard_Boolean IsDone;
162 IsDone = Compute(2, MyHessian);
165 Gradient1(MyHessian, G);
166 Hessian1 (MyHessian, H);
172 //=======================================================================
173 void FairCurve_Energy::Hessian1(const math_Vector& Vect,
175 //=======================================================================
178 Standard_Integer ii, jj, kk, Vk;
179 Standard_Integer Vdeb = 3 + 2*MyContrOrder1,
180 Vfin = 2*MyPoles->Length() - 2*(MyContrOrder2+1),
181 Vup = 2*MyPoles->Length()+MyWithAuxValue;
182 Standard_Integer DebH = 1+MyContrOrder1,
183 FinH = MyNbVar - MyWithAuxValue - MyContrOrder2 ;
184 Standard_Real Cos0 = pow(MyLinearForm(0).X(),2),
185 Sin0 = pow(MyLinearForm(0).Y(),2),
186 CosSin0 = 2 * MyLinearForm(0).X() * MyLinearForm(0).Y(),
187 Cos1 = pow(MyLinearForm(1).X(),2),
188 Sin1 = pow(MyLinearForm(1).Y(),2),
189 CosSin1 = 2 * MyLinearForm(1).X() * MyLinearForm(1).Y() ;
190 Standard_Real Lambda0=0, Lambda1=0;
192 if (MyContrOrder1 >= 1) {
193 Lambda0 = MyPoles->Value(MyPoles->Lower())
194 .Distance( MyPoles->Value(MyPoles->Lower()+1) );}
196 if (MyContrOrder2 >= 1) {
197 Lambda1 = MyPoles->Value(MyPoles->Upper())
198 .Distance(MyPoles->Value(MyPoles->Upper()-1) );}
201 if (MyContrOrder1 >= 1) {
203 // calculate the left lambda column --------------------------------
205 jj = Vdeb-2*MyContrOrder1;
206 kk=Indice(jj, jj); // X2X2
207 ii=Indice(jj+1, jj); // X2Y2
208 H(1, 1) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
210 if (MyContrOrder1 >= 2) {
211 gp_XY Laux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0));
212 jj = Vdeb-2*(MyContrOrder1-1);
213 kk=Indice(jj, jj-2); // X1X2
214 ii=Indice(jj+1, jj-2); //X1Y2
215 gp_XY Aux(Vect(kk+2), Vect(ii+3));
218 ( MyQuadForm(0).X() * Vect(5) + MyQuadForm(0).Y() * Vect(6) )
219 + ( Laux.X() * ( MyLinearForm(0).X()*Vect(kk) + MyLinearForm(0).Y()*Vect(kk+1))
220 + Laux.Y() * ( MyLinearForm(0).X()*Vect(ii) + MyLinearForm(0).Y()*Vect(ii+1)) )
221 + Laux.X() * Laux.Y() * Vect(ii+2) )
222 + ( Pow(Laux.X(),2) * Vect(kk+2) + Pow(Laux.Y(),2) * Vect(ii+3));
224 H(2,1) = (Cos0 * Vect(kk) + CosSin0*(Vect(ii)+Vect(kk+1))/2 + Sin0 * Vect(ii+1))
225 + Laux * MyLinearForm(0).Multiplied(Aux)
226 + (Laux.X()*MyLinearForm(0).Y() + Laux.Y()*MyLinearForm(0).X()) * Vect(ii+2);
230 if (MyWithAuxValue) {
231 kk =Indice(Vup, Vdeb-2*MyContrOrder1);
232 H(MyNbVar, 1) = MyLinearForm(0).X() * Vect(kk)
233 + MyLinearForm(0).Y() * Vect(kk+1);
236 if (MyContrOrder2 >= 1) {
237 H(MyNbVar-MyWithAuxValue, 1) = 0; // correct if there are less than 3 nodes
238 if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 1) = 0;}
243 kk = Indice(Vk, Vdeb-2*MyContrOrder1);
244 for (ii=DebH; ii<=FinH; ii++) {
245 H(ii, 1) = MyLinearForm(0).X() * Vect(kk)
246 + MyLinearForm(0).Y() * Vect(kk+1);
252 // calculate the left line mu ----------------------
253 if (MyContrOrder1 >= 2) {
254 jj = Vdeb-2*(MyContrOrder1-1);
255 kk=Indice(jj, jj); // X3X3
256 ii=Indice(jj+1, jj); // X3Y3
257 H(2, 2) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
259 if (MyWithAuxValue) {
260 kk =Indice(Vup, Vdeb-2*(MyContrOrder1-1));
261 gp_XY Pole (Vect(kk), Vect(kk+1));
262 H(MyNbVar, 1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * Pole;
263 H(MyNbVar, 2) = MyLinearForm(0).X() * Vect(kk)
264 + MyLinearForm(0).Y() * Vect(kk+1);
267 if (MyContrOrder2 >= 1) {
268 H(MyNbVar-MyWithAuxValue, 2) = 0; // correct if there are less than 3 nodes
269 if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 2) = 0;}
273 Standard_Real Xaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).X(),
274 Yaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).Y();
276 kk = Indice(Vk, Vdeb-2*MyContrOrder1+2);
277 for (ii=DebH; ii<=FinH; ii++) {
278 H(ii, 2) = MyLinearForm(0).X() * Vect(kk)
279 + MyLinearForm(0).Y() * Vect(kk+1);
280 H(ii, 1) += Xaux * Vect(kk) + Yaux*Vect(kk+1);
286 // calculate the right lambda line -----------------------
287 if (MyContrOrder2 >= 1) {
290 Vk = Vfin + 2*MyContrOrder2 - 1;
291 kk = Indice(Vk, Vdeb);
292 for (ii=DebH; ii<=FinH; ii++) {
293 H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
294 + MyLinearForm(1).Y() * Vect(kk+Vk);
299 H(jj, jj) = Cos1 * Vect(kk) + CosSin1 * Vect(kk+Vk) + Sin1 * Vect(kk+Vk+1);
301 if (MyContrOrder2 >= 2) {
303 gp_XY Laux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1));
304 jj = Vfin + 2*MyContrOrder2 - 3;
305 kk=Indice(jj+2, jj); // Xn-1Xn-2
306 ii=Indice(jj+3, jj); //Yn-1Xn-2
307 Standard_Integer ll = Indice(jj, jj);
309 H (FinH+1, FinH+1) += 2 * (
310 ( MyQuadForm(1).X() * Vect(jj) + MyQuadForm(1).Y() * Vect(jj+1) )
311 + ( Laux.X() * ( MyLinearForm(1).X()*Vect(kk) + MyLinearForm(1).Y()*Vect(ii))
312 + Laux.Y() * ( MyLinearForm(1).X()*Vect(kk+1) + MyLinearForm(1).Y()*Vect(ii+1)) )
313 + Laux.X() * Laux.Y() * Vect(ll+jj) )
314 + ( Pow(Laux.X(),2) * Vect(ll) + Pow(Laux.Y(),2) * Vect(ll+jj+1));
316 H(FinH+2, FinH+1) = Cos1 * Vect(kk) + CosSin1*(Vect(ii)+Vect(kk+1))/2 + Sin1 * Vect(ii+1);
317 gp_XY Aux(Vect(ll), Vect(ll+jj+1));
318 H(FinH+2, FinH+1) += Laux * MyLinearForm(1).Multiplied(Aux)
319 + (Laux.X()*MyLinearForm(1).Y() + Laux.Y()*MyLinearForm(1).X())
321 // H(FinH+2, FinH+1) = 0; // No better alternative. Bug in the previous expression
325 // calculate the right line mu -----------------------
326 if (MyContrOrder2 >= 2) {
328 Vk = Vfin + 2*MyContrOrder2 - 3;
329 kk = Indice(Vk, Vdeb);
331 Standard_Real Xaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).X(),
332 Yaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).Y();
334 for (ii=DebH; ii<=FinH; ii++) {
335 H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
336 + MyLinearForm(1).Y() * Vect(kk+Vk);
337 // update the right line Lambda
338 H(jj-1,ii) += Xaux*Vect(kk) + Yaux*Vect(kk+Vk);
342 ii = Indice(Vk+1, Vk);
343 H(jj,jj) = Cos1*Vect(kk) + CosSin1*Vect(ii) + Sin1*Vect(ii+1);
346 // calculate the Auxiliary Variable line -----------------------
347 if (MyWithAuxValue) {
349 kk = Indice(Vup, Vdeb);
350 for (ii=DebH; ii<=FinH; ii++) {
351 H(MyNbVar, ii) = Vect(kk);
355 if (MyContrOrder2 >= 1) {
356 kk = Indice(Vup, Vfin+2*MyContrOrder2-1);
358 MyLinearForm(1).X() * Vect(kk) + MyLinearForm(1).Y() * Vect(kk+1);
360 if (MyContrOrder2 >= 2) {
361 kk = Indice(Vup, Vfin+2*MyContrOrder2-3);
362 gp_XY Pole( Vect(kk), Vect(kk+1));
363 H(MyNbVar, FinH+1) += (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * Pole;
364 H(MyNbVar, FinH+2) = MyLinearForm(1) * Pole;
366 kk = Indice(Vup, Vup);
367 H(H.UpperRow(), H.UpperRow()) = Vect(kk);
370 // recopy the internal block -----------------------------------
372 kk = Indice(Vdeb, Vdeb);
373 for (ii = DebH; ii <=FinH; ii++) {
374 for (jj = DebH; jj<=ii; jj++) {
381 for (ii = H.LowerRow(); ii <= H.UpperRow(); ii++)
382 for (jj = ii+1; jj <= H.UpperRow(); jj++) H(ii,jj) = H(jj,ii);
385 //=======================================================================
386 Standard_Boolean FairCurve_Energy::Variable(math_Vector& X) const
387 //=======================================================================
390 IndexDeb1 = MyPoles->Lower()+1,
391 IndexDeb2 = X.Lower(),
392 IndexFin1 = MyPoles->Upper()-1,
393 IndexFin2 = X.Upper() - MyWithAuxValue; // decrease by 1 if the sliding is
394 // free as the last value of X is reserved.
397 // calculation of variables of constraints
398 if (MyContrOrder1 >= 1) {
399 X(IndexDeb2) = MyPoles->Value(MyPoles->Lower())
400 .Distance( MyPoles->Value(MyPoles->Lower()+1) );
404 if (MyContrOrder1 == 2) {
405 gp_Vec2d b1b2( MyPoles->Value(MyPoles->Lower()+1),
406 MyPoles->Value(MyPoles->Lower()+2) );
407 X(IndexDeb2) = b1b2.XY() * MyLinearForm(0);
411 if (MyContrOrder2 == 2) {
412 gp_Vec2d bn2bn1( MyPoles->Value(MyPoles->Upper()-2),
413 MyPoles->Value(MyPoles->Upper()-1));
414 X(IndexFin2) = - bn2bn1.XY() * MyLinearForm(1);
418 if (MyContrOrder2 >= 1) {
419 X(IndexFin2) = MyPoles->Value(MyPoles->Upper())
420 .Distance(MyPoles->Value(MyPoles->Upper()-1) );
424 // Recopy of auxiliary variables is not done in the abstract class
426 // copy poles to variables
427 for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
428 X(IndexDeb2) = MyPoles->Value(ii).X();
429 X(IndexDeb2+1) = MyPoles->Value(ii).Y();
432 return Standard_True;
435 //=======================================================================
436 void FairCurve_Energy::ComputePoles(const math_Vector& X)
437 //=======================================================================
440 IndexDeb1 = MyPoles->Lower()+1,
441 IndexDeb2 = X.Lower(),
442 IndexFin1 = MyPoles->Upper()-1,
443 IndexFin2 = X.Upper() - MyWithAuxValue; // decrease by 1 if the sliding is
444 // is free as the last value of X is reserved.
445 // calculation of pole constraints
446 // for (ii=MyPoles->Lower();ii<=MyPoles->Upper();ii++) {
447 // std::cout << ii << " X = " << MyPoles->Value(ii).X() <<
448 // " Y = " << MyPoles->Value(ii).Y() << std::endl;}
450 if (MyContrOrder1 >= 1) {
453 ComputePolesG1( 0, X(1), MyPoles->Value(MyPoles->Lower()),
454 MyPoles->ChangeValue(MyPoles->Lower()+1) );
456 if (MyContrOrder1 == 2) {
459 ComputePolesG2( 0, X(1), X(2), MyPoles->Value(MyPoles->Lower()),
460 MyPoles->ChangeValue(MyPoles->Lower()+2) );
462 if (MyContrOrder2 == 2) {
465 ComputePolesG2( 1, X(IndexFin2), X(IndexFin2+1),
466 MyPoles->Value(MyPoles->Upper()),
467 MyPoles->ChangeValue(MyPoles->Upper()-2) );
469 if (MyContrOrder2 >= 1) {
471 ComputePolesG1( 1, X(IndexFin2), MyPoles->Value(MyPoles->Upper()),
472 MyPoles->ChangeValue(MyPoles->Upper()-1) );
475 // if (MyWithAuxValue) { MyLengthSliding = X(X.Upper()); }
477 for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
478 MyPoles -> ChangeValue(ii).SetX( X(IndexDeb2) );
479 MyPoles -> ChangeValue(ii).SetY( X(IndexDeb2+1) );
484 //=======================================================================
485 void FairCurve_Energy::ComputePolesG1(const Standard_Integer Side,
486 const Standard_Real Lambda,
489 //=======================================================================
490 { P2.SetXY ( P1.XY() + MyLinearForm(Side) * Lambda ); }
492 //=======================================================================
493 void FairCurve_Energy::ComputePolesG2(const Standard_Integer Side,
494 const Standard_Real Lambda,
495 const Standard_Real Rho,
498 //=======================================================================
500 + MyLinearForm(Side) * (Lambda + Rho )
501 + MyQuadForm(Side) * (Lambda * Lambda) ) ; }