projects / occt.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
0025418: Debug output to be limited to OCC development environment
[occt.git] / src / FairCurve / FairCurve_Energy.cxx
CommitLineData
b311480e 1// Created on: 1996-01-22
2// Created by: Philippe MANGIN
3// Copyright (c) 1996-1999 Matra Datavision
973c2be1 4// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e 5//
973c2be1 6// This file is part of Open CASCADE Technology software library.
b311480e 7//
d5f74e42 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
973c2be1 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.
b311480e 13//
973c2be1 14// Alternatively, this file may be used under the terms of Open CASCADE
15// commercial license or contractual agreement.
7fd59977 16
0797d9d3 17#ifndef OCCT_DEBUG
7fd59977 18#define No_Standard_RangeError
19#define No_Standard_OutOfRange
20#endif
21
22#include <FairCurve_Energy.ixx>
23
24#include <math_IntegerVector.hxx>
25#include <gp_Vec2d.hxx>
26
27//=======================================================================
28FairCurve_Energy::FairCurve_Energy(const Handle(TColgp_HArray1OfPnt2d)& Poles,
29 const Standard_Integer ContrOrder1,
30 const Standard_Integer ContrOrder2,
31 const Standard_Boolean WithAuxValue,
32 const Standard_Real Angle1,
33 const Standard_Real Angle2,
34 const Standard_Integer Degree,
35 const Standard_Real Curvature1,
36 const Standard_Real Curvature2 )
37//=======================================================================
38 : MyPoles (Poles),
39 MyContrOrder1(ContrOrder1),
40 MyContrOrder2(ContrOrder2),
41 MyWithAuxValue(WithAuxValue),
42 MyNbVar(2*(Poles->Length()-2) - ContrOrder2 - ContrOrder1 + WithAuxValue),
43 MyNbValues(2*Poles->Length() + WithAuxValue),
44 MyLinearForm(0, 1),
45 MyQuadForm(0, 1),
46 MyGradient( 0, MyNbValues),
47 MyHessian( 0, MyNbValues + MyNbValues*(MyNbValues+1)/2 )
48{
0d969553 49 // chesk angles in reference (Ox,Oy)
7fd59977 50 gp_XY L0 (Cos(Angle1), Sin(Angle1)), L1 (-Cos(Angle2), Sin(Angle2));
51 MyLinearForm.SetValue(0, L0);
52 MyLinearForm.SetValue(1, L1);
53 gp_XY Q0(-Sin(Angle1), Cos(Angle1)), Q1 (Sin(Angle2), Cos(Angle2));
54 MyQuadForm.SetValue(0, ((double)Degree) / (Degree-1) * Curvature1 * Q0);
55 MyQuadForm.SetValue(1, ((double)Degree) / (Degree-1) * Curvature2 * Q1);
56}
57
58//=======================================================================
59Standard_Boolean FairCurve_Energy::Value(const math_Vector& X,
60 Standard_Real& E)
61//=======================================================================
62{
63 Standard_Boolean IsDone;
64 math_Vector Energie(0,0);
65 ComputePoles(X);
66 IsDone = Compute(0, Energie);
67 E = Energie(0);
68 return IsDone;
69}
70
71//=======================================================================
72Standard_Boolean FairCurve_Energy::Gradient(const math_Vector& X,
73 math_Vector& G)
74//=======================================================================
75{
76 Standard_Boolean IsDone;
77 Standard_Real E;
78
79 IsDone = Values(X, E, G);
80 return IsDone;
81}
82
83//=======================================================================
84void FairCurve_Energy::Gradient1(const math_Vector& Vect,
85 math_Vector& Grad)
86//=======================================================================
87{
88 Standard_Integer ii,
89 DebG = Grad.Lower(), FinG = Grad.Upper();
90 Standard_Integer Vdeb = 3,
91 Vfin = 2*MyPoles->Length()-2;
92
0d969553 93// .... by calculation
7fd59977 94 if (MyContrOrder1 >= 1) {
95 gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
96 Grad(DebG) = MyLinearForm(0) * DPole;
97 Vdeb += 2;
98 DebG += 1;
99 }
100 if(MyContrOrder1 == 2) {
101 Standard_Real Lambda0 = MyPoles->Value(MyPoles->Lower())
102 .Distance( MyPoles->Value(MyPoles->Lower()+1) );
103 gp_XY DPole (Vect(Vdeb), Vect(Vdeb+1));
104 Grad(DebG-1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * DPole;
105 Grad(DebG) = MyLinearForm(0) * DPole;
106 Vdeb += 2;
107 DebG += 1;
108 }
109 if (MyWithAuxValue) {
110 Grad(FinG) = Vect( 2*MyPoles->Length()+1 );
111 FinG -= 1;
112 }
113 if (MyContrOrder2 >= 1) {
114 gp_XY DPole (Vect(Vfin-1), Vect(Vfin));
115 Grad(FinG) = MyLinearForm(1) * DPole;
116 FinG -= 1;
117 }
118 if(MyContrOrder2 == 2) {
119 Standard_Real Lambda1 = MyPoles->Value(MyPoles->Upper())
120 .Distance(MyPoles->Value(MyPoles->Upper()-1) );
121 gp_XY DPole (Vect(Vfin-3), Vect(Vfin-2));
122 Grad(FinG) = Grad(FinG+1) + (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * DPole;
123 Grad(FinG+1) = MyLinearForm(1) * DPole;
124 FinG -= 1;
125 }
0d969553 126// ... by recopy
7fd59977 127 for (ii=DebG; ii<=FinG; ii++) {
128 Grad(ii) = Vect(Vdeb);
129 Vdeb += 1;
130 }
131}
132
133//=======================================================================
134Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
135 Standard_Real& E,
136 math_Vector& G)
137//=======================================================================
138{
139 Standard_Boolean IsDone;
140
141 ComputePoles(X);
142 IsDone = Compute(1, MyGradient);
143 if (IsDone) {
144 E = MyGradient(0);
145 Gradient1(MyGradient, G);
146 }
147 return IsDone;
148}
149
150//=======================================================================
151Standard_Boolean FairCurve_Energy::Values(const math_Vector& X,
152 Standard_Real& E,
153 math_Vector& G,
154 math_Matrix& H)
155//=======================================================================
156{
157 Standard_Boolean IsDone;
158
159 ComputePoles(X);
160 IsDone = Compute(2, MyHessian);
161 if (IsDone) {
162 E = MyHessian(0);
163 Gradient1(MyHessian, G);
164 Hessian1 (MyHessian, H);
165 }
166 return IsDone;
167}
168
169
170//=======================================================================
171void FairCurve_Energy::Hessian1(const math_Vector& Vect,
172 math_Matrix& H)
173//=======================================================================
174{
175
176 Standard_Integer ii, jj, kk, Vk;
177 Standard_Integer Vdeb = 3 + 2*MyContrOrder1,
178 Vfin = 2*MyPoles->Length() - 2*(MyContrOrder2+1),
179 Vup = 2*MyPoles->Length()+MyWithAuxValue;
180 Standard_Integer DebH = 1+MyContrOrder1,
181 FinH = MyNbVar - MyWithAuxValue - MyContrOrder2 ;
182 Standard_Real Cos0 = pow(MyLinearForm(0).X(),2),
183 Sin0 = pow(MyLinearForm(0).Y(),2),
184 CosSin0 = 2 * MyLinearForm(0).X() * MyLinearForm(0).Y(),
185 Cos1 = pow(MyLinearForm(1).X(),2),
186 Sin1 = pow(MyLinearForm(1).Y(),2),
187 CosSin1 = 2 * MyLinearForm(1).X() * MyLinearForm(1).Y() ;
188 Standard_Real Lambda0=0, Lambda1=0;
189
190 if (MyContrOrder1 >= 1) {
191 Lambda0 = MyPoles->Value(MyPoles->Lower())
192 .Distance( MyPoles->Value(MyPoles->Lower()+1) );}
193
194 if (MyContrOrder2 >= 1) {
195 Lambda1 = MyPoles->Value(MyPoles->Upper())
196 .Distance(MyPoles->Value(MyPoles->Upper()-1) );}
197
198
199 if (MyContrOrder1 >= 1) {
200
0d969553 201// calculate the left lambda column --------------------------------
7fd59977 202
203 jj = Vdeb-2*MyContrOrder1;
204 kk=Indice(jj, jj); // X2X2
205 ii=Indice(jj+1, jj); // X2Y2
206 H(1, 1) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
207
208 if (MyContrOrder1 >= 2) {
209 gp_XY Laux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0));
210 jj = Vdeb-2*(MyContrOrder1-1);
211 kk=Indice(jj, jj-2); // X1X2
212 ii=Indice(jj+1, jj-2); //X1Y2
213 gp_XY Aux(Vect(kk+2), Vect(ii+3));
214
215 H (1, 1) += 2 * (
216 ( MyQuadForm(0).X() * Vect(5) + MyQuadForm(0).Y() * Vect(6) )
217 + ( Laux.X() * ( MyLinearForm(0).X()*Vect(kk) + MyLinearForm(0).Y()*Vect(kk+1))
218 + Laux.Y() * ( MyLinearForm(0).X()*Vect(ii) + MyLinearForm(0).Y()*Vect(ii+1)) )
219 + Laux.X() * Laux.Y() * Vect(ii+2) )
220 + ( Pow(Laux.X(),2) * Vect(kk+2) + Pow(Laux.Y(),2) * Vect(ii+3));
221
222 H(2,1) = (Cos0 * Vect(kk) + CosSin0*(Vect(ii)+Vect(kk+1))/2 + Sin0 * Vect(ii+1))
223 + Laux * MyLinearForm(0).Multiplied(Aux)
224 + (Laux.X()*MyLinearForm(0).Y() + Laux.Y()*MyLinearForm(0).X()) * Vect(ii+2);
225 }
226
227
228 if (MyWithAuxValue) {
229 kk =Indice(Vup, Vdeb-2*MyContrOrder1);
230 H(MyNbVar, 1) = MyLinearForm(0).X() * Vect(kk)
231 + MyLinearForm(0).Y() * Vect(kk+1);
232 }
233
234 if (MyContrOrder2 >= 1) {
0d969553 235 H(MyNbVar-MyWithAuxValue, 1) = 0; // correct if there are less than 3 nodes
7fd59977 236 if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 1) = 0;}
237 }
238
239
240 Vk = Vdeb;
241 kk = Indice(Vk, Vdeb-2*MyContrOrder1);
242 for (ii=DebH; ii<=FinH; ii++) {
243 H(ii, 1) = MyLinearForm(0).X() * Vect(kk)
244 + MyLinearForm(0).Y() * Vect(kk+1);
245 kk += Vk;
246 Vk++;
247 }
248 }
249
0d969553 250// calculate the left line mu ----------------------
7fd59977 251 if (MyContrOrder1 >= 2) {
252 jj = Vdeb-2*(MyContrOrder1-1);
253 kk=Indice(jj, jj); // X3X3
254 ii=Indice(jj+1, jj); // X3Y3
255 H(2, 2) = Cos0 * Vect(kk) + CosSin0*Vect(ii) + Sin0 * Vect(ii+1);
256
257 if (MyWithAuxValue) {
258 kk =Indice(Vup, Vdeb-2*(MyContrOrder1-1));
259 gp_XY Pole (Vect(kk), Vect(kk+1));
260 H(MyNbVar, 1) += (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)) * Pole;
261 H(MyNbVar, 2) = MyLinearForm(0).X() * Vect(kk)
262 + MyLinearForm(0).Y() * Vect(kk+1);
263 }
264
265 if (MyContrOrder2 >= 1) {
0d969553 266 H(MyNbVar-MyWithAuxValue, 2) = 0; // correct if there are less than 3 nodes
7fd59977 267 if (MyContrOrder2 == 2) {H(MyNbVar-MyWithAuxValue-1, 2) = 0;}
268 }
269 Vk = Vdeb;
270
271 Standard_Real Xaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).X(),
272 Yaux = (MyLinearForm(0) + 2*Lambda0*MyQuadForm(0)).Y();
273
274 kk = Indice(Vk, Vdeb-2*MyContrOrder1+2);
275 for (ii=DebH; ii<=FinH; ii++) {
276 H(ii, 2) = MyLinearForm(0).X() * Vect(kk)
277 + MyLinearForm(0).Y() * Vect(kk+1);
278 H(ii, 1) += Xaux * Vect(kk) + Yaux*Vect(kk+1);
279 kk += Vk;
280 Vk++;
281 }
282 }
283
0d969553 284// calculate the right lambda line -----------------------
7fd59977 285 if (MyContrOrder2 >= 1) {
286
287 jj = FinH + 1;
288 Vk = Vfin + 2*MyContrOrder2 - 1;
289 kk = Indice(Vk, Vdeb);
290 for (ii=DebH; ii<=FinH; ii++) {
291 H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
292 + MyLinearForm(1).Y() * Vect(kk+Vk);
293 kk++;
294 }
295
296 kk = Indice(Vk, Vk);
297 H(jj, jj) = Cos1 * Vect(kk) + CosSin1 * Vect(kk+Vk) + Sin1 * Vect(kk+Vk+1);
298
299 if (MyContrOrder2 >= 2) {
300 // H(jj,jj) +=
301 gp_XY Laux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1));
302 jj = Vfin + 2*MyContrOrder2 - 3;
303 kk=Indice(jj+2, jj); // Xn-1Xn-2
304 ii=Indice(jj+3, jj); //Yn-1Xn-2
305 Standard_Integer ll = Indice(jj, jj);
306
307 H (FinH+1, FinH+1) += 2 * (
308 ( MyQuadForm(1).X() * Vect(jj) + MyQuadForm(1).Y() * Vect(jj+1) )
309 + ( Laux.X() * ( MyLinearForm(1).X()*Vect(kk) + MyLinearForm(1).Y()*Vect(ii))
310 + Laux.Y() * ( MyLinearForm(1).X()*Vect(kk+1) + MyLinearForm(1).Y()*Vect(ii+1)) )
311 + Laux.X() * Laux.Y() * Vect(ll+jj) )
312 + ( Pow(Laux.X(),2) * Vect(ll) + Pow(Laux.Y(),2) * Vect(ll+jj+1));
313
314 H(FinH+2, FinH+1) = Cos1 * Vect(kk) + CosSin1*(Vect(ii)+Vect(kk+1))/2 + Sin1 * Vect(ii+1);
315 gp_XY Aux(Vect(ll), Vect(ll+jj+1));
316 H(FinH+2, FinH+1) += Laux * MyLinearForm(1).Multiplied(Aux)
317 + (Laux.X()*MyLinearForm(1).Y() + Laux.Y()*MyLinearForm(1).X())
318 * Vect(ll+jj);
0d969553 319// H(FinH+2, FinH+1) = 0; // No better alternative. Bug in the previous expression
7fd59977 320 }
321 }
322
0d969553 323// calculate the right line mu -----------------------
7fd59977 324 if (MyContrOrder2 >= 2) {
325 jj = FinH + 2;
326 Vk = Vfin + 2*MyContrOrder2 - 3;
327 kk = Indice(Vk, Vdeb);
328
329 Standard_Real Xaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).X(),
330 Yaux = (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)).Y();
331
332 for (ii=DebH; ii<=FinH; ii++) {
333 H(jj, ii) = MyLinearForm(1).X() * Vect(kk)
334 + MyLinearForm(1).Y() * Vect(kk+Vk);
0d969553 335 // update the right line Lambda
7fd59977 336 H(jj-1,ii) += Xaux*Vect(kk) + Yaux*Vect(kk+Vk);
337 kk++;
338 }
339 kk = Indice(Vk, Vk);
340 ii = Indice(Vk+1, Vk);
341 H(jj,jj) = Cos1*Vect(kk) + CosSin1*Vect(ii) + Sin1*Vect(ii+1);
342 }
343
0d969553 344// calculate the Auxiliary Variable line -----------------------
7fd59977 345 if (MyWithAuxValue) {
346
347 kk = Indice(Vup, Vdeb);
348 for (ii=DebH; ii<=FinH; ii++) {
349 H(MyNbVar, ii) = Vect(kk);
350 kk++;
351 }
352
353 if (MyContrOrder2 >= 1) {
354 kk = Indice(Vup, Vfin+2*MyContrOrder2-1);
355 H(MyNbVar, FinH+1) =
356 MyLinearForm(1).X() * Vect(kk) + MyLinearForm(1).Y() * Vect(kk+1);
357 }
358 if (MyContrOrder2 >= 2) {
359 kk = Indice(Vup, Vfin+2*MyContrOrder2-3);
360 gp_XY Pole( Vect(kk), Vect(kk+1));
361 H(MyNbVar, FinH+1) += (MyLinearForm(1) + 2*Lambda1*MyQuadForm(1)) * Pole;
362 H(MyNbVar, FinH+2) = MyLinearForm(1) * Pole;
363 }
364 kk = Indice(Vup, Vup);
365 H(H.UpperRow(), H.UpperRow()) = Vect(kk);
366 }
367
0d969553 368// recopy the internal block -----------------------------------
7fd59977 369
370 kk = Indice(Vdeb, Vdeb);
371 for (ii = DebH; ii <=FinH; ii++) {
372 for (jj = DebH; jj<=ii; jj++) {
373 H(ii,jj) = Vect(kk);
374 kk++;
375 }
376 kk += Vdeb-1;
377 }
0d969553 378// symmetry
7fd59977 379 for (ii = H.LowerRow(); ii <= H.UpperRow(); ii++)
380 for (jj = ii+1; jj <= H.UpperRow(); jj++) H(ii,jj) = H(jj,ii);
381}
382
383//=======================================================================
384Standard_Boolean FairCurve_Energy::Variable(math_Vector& X) const
385//=======================================================================
386{
387 Standard_Integer ii,
388 IndexDeb1 = MyPoles->Lower()+1,
389 IndexDeb2 = X.Lower(),
390 IndexFin1 = MyPoles->Upper()-1,
0d969553
Y		 391 IndexFin2 = X.Upper() - MyWithAuxValue; // decrease by 1 if the sliding is
392 // free as the last value of X is reserved.
7fd59977 393
394
0d969553 395// calculation of variables of constraints
7fd59977 396 if (MyContrOrder1 >= 1) {
397 X(IndexDeb2) = MyPoles->Value(MyPoles->Lower())
398 .Distance( MyPoles->Value(MyPoles->Lower()+1) );
399 IndexDeb1 += 1;
400 IndexDeb2 += 1;
401 }
402 if (MyContrOrder1 == 2) {
403 gp_Vec2d b1b2( MyPoles->Value(MyPoles->Lower()+1),
404 MyPoles->Value(MyPoles->Lower()+2) );
405 X(IndexDeb2) = b1b2.XY() * MyLinearForm(0);
406 IndexDeb1 += 1;
407 IndexDeb2 += 1;
408 }
409 if (MyContrOrder2 == 2) {
410 gp_Vec2d bn2bn1( MyPoles->Value(MyPoles->Upper()-2),
411 MyPoles->Value(MyPoles->Upper()-1));
412 X(IndexFin2) = - bn2bn1.XY() * MyLinearForm(1);
413 IndexFin1 -= 1;
414 IndexFin2 -= 1;
415 }
416 if (MyContrOrder2 >= 1) {
417 X(IndexFin2) = MyPoles->Value(MyPoles->Upper())
418 .Distance(MyPoles->Value(MyPoles->Upper()-1) );
419 IndexFin1 -= 1;
420 }
421
0d969553 422// Recopy of auxiliary variables is not done in the abstract class
7fd59977 423
0d969553 424// copy poles to variables
7fd59977 425 for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
426 X(IndexDeb2) = MyPoles->Value(ii).X();
427 X(IndexDeb2+1) = MyPoles->Value(ii).Y();
428 IndexDeb2 +=2;
429 }
430 return Standard_True;
431}
432
433//=======================================================================
434void FairCurve_Energy::ComputePoles(const math_Vector& X)
435//=======================================================================
436{
437 Standard_Integer ii,
438 IndexDeb1 = MyPoles->Lower()+1,
439 IndexDeb2 = X.Lower(),
440 IndexFin1 = MyPoles->Upper()-1,
0d969553
Y		 441 IndexFin2 = X.Upper() - MyWithAuxValue; // decrease by 1 if the sliding is
442 // is free as the last value of X is reserved.
443// calculation of pole constraints
444// for (ii=MyPoles->Lower();ii<=MyPoles->Upper();ii++) {
445// cout << ii << " X = " << MyPoles->Value(ii).X() <<
7fd59977 446// " Y = " << MyPoles->Value(ii).Y() << endl;}
447
448 if (MyContrOrder1 >= 1) {
449 IndexDeb1 += 1;
450 IndexDeb2 += 1;
451 ComputePolesG1( 0, X(1), MyPoles->Value(MyPoles->Lower()),
452 MyPoles->ChangeValue(MyPoles->Lower()+1) );
453 }
454 if (MyContrOrder1 == 2) {
455 IndexDeb1 += 1;
456 IndexDeb2 += 1;
457 ComputePolesG2( 0, X(1), X(2), MyPoles->Value(MyPoles->Lower()),
458 MyPoles->ChangeValue(MyPoles->Lower()+2) );
459 }
460 if (MyContrOrder2 == 2) {
461 IndexFin1 -= 1;
462 IndexFin2 -= 1;
463 ComputePolesG2( 1, X(IndexFin2), X(IndexFin2+1),
464 MyPoles->Value(MyPoles->Upper()),
465 MyPoles->ChangeValue(MyPoles->Upper()-2) );
466 }
467 if (MyContrOrder2 >= 1) {
468 IndexFin1 -= 1;
469 ComputePolesG1( 1, X(IndexFin2), MyPoles->Value(MyPoles->Upper()),
470 MyPoles->ChangeValue(MyPoles->Upper()-1) );
471 }
472
473// if (MyWithAuxValue) { MyLengthSliding = X(X.Upper()); }
0d969553 474// recopy others
7fd59977 475 for (ii=IndexDeb1; ii<=IndexFin1; ii++) {
476 MyPoles -> ChangeValue(ii).SetX( X(IndexDeb2) );
477 MyPoles -> ChangeValue(ii).SetY( X(IndexDeb2+1) );
478 IndexDeb2 += 2;
479 }
480}
481
482//=======================================================================
483void FairCurve_Energy::ComputePolesG1(const Standard_Integer Side,
484 const Standard_Real Lambda,
485 const gp_Pnt2d& P1,
486 gp_Pnt2d& P2) const
487//=======================================================================
488{ P2.SetXY ( P1.XY() + MyLinearForm(Side) * Lambda ); }
489
490//=======================================================================
491void FairCurve_Energy::ComputePolesG2(const Standard_Integer Side,
492 const Standard_Real Lambda,
493 const Standard_Real Rho,
494 const gp_Pnt2d& P1,
495 gp_Pnt2d& P2) const
496//=======================================================================
497{ P2.SetXY ( P1.XY()
498 + MyLinearForm(Side) * (Lambda + Rho )
499 + MyQuadForm(Side) * (Lambda * Lambda) ) ; }
500
501
502
503
Open CASCADE Technology repository