0024428: Implementation of LGPL license
[occt.git] / src / AppParCurves / AppParCurves_BSpGradient.gxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and / or modify it
7 // under the terms of the GNU Lesser General Public version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 // lpa, le 11/09/91
16
17
18 // Application de la methode du gradient corrige pour minimiser 
19 // F  = somme(||C(ui, Poles(ui)) - ptli||2.
20 // La methode de gradient conjugue est programmee dans la bibliotheque 
21 // mathematique: math_BFGS.
22
23
24 #define No_Standard_RangeError
25 #define No_Standard_OutOfRange
26
27 #include <AppParCurves_Constraint.hxx>
28 #include <AppParCurves_ConstraintCouple.hxx>
29 #include <math_BFGS.hxx>
30 #include <StdFail_NotDone.hxx>
31 #include <AppParCurves_MultiPoint.hxx>
32 #include <gp_Pnt.hxx>
33 #include <gp_Pnt2d.hxx>
34 #include <gp_Vec.hxx>
35 #include <gp_Vec2d.hxx>
36 #include <TColgp_Array1OfPnt.hxx>
37 #include <TColgp_Array1OfPnt2d.hxx>
38 #include <TColgp_Array1OfVec.hxx>
39 #include <TColgp_Array1OfVec2d.hxx>
40
41 #include <OSD_Chronometer.hxx>
42
43 static OSD_Chronometer chr1;
44
45
46 static Standard_Boolean islambdadefined = Standard_False;
47
48
49
50 static AppParCurves_Constraint FirstConstraint
51   (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
52    const Standard_Integer FirstPoint)
53 {
54   Standard_Integer i, myindex;
55   Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper();
56   AppParCurves_ConstraintCouple mycouple;
57   AppParCurves_Constraint Cons = AppParCurves_NoConstraint;
58
59   for (i = low; i <= upp; i++) {
60     mycouple = TheConstraints->Value(i);
61     Cons = mycouple.Constraint();
62     myindex = mycouple.Index();
63     if (myindex == FirstPoint) {
64       break;
65     }
66   }
67   return Cons;
68 }
69
70
71 static AppParCurves_Constraint LastConstraint
72   (const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
73    const Standard_Integer LastPoint)
74 {
75   Standard_Integer i, myindex;
76   Standard_Integer low = TheConstraints->Lower(), upp= TheConstraints->Upper();
77   AppParCurves_ConstraintCouple mycouple;
78   AppParCurves_Constraint Cons = AppParCurves_NoConstraint;
79
80   for (i = low; i <= upp; i++) {
81     mycouple = TheConstraints->Value(i);
82     Cons = mycouple.Constraint();
83     myindex = mycouple.Index();
84     if (myindex == LastPoint) {
85       break;
86     }
87   }
88   return Cons;
89 }
90
91
92
93
94
95 AppParCurves_BSpGradient::
96    AppParCurves_BSpGradient(const MultiLine& SSP,
97          const Standard_Integer FirstPoint,
98          const Standard_Integer LastPoint,
99          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
100          math_Vector& Parameters,
101          const TColStd_Array1OfReal& Knots,
102          const TColStd_Array1OfInteger& Mults,
103          const Standard_Integer Deg,
104          const Standard_Real Tol3d,
105          const Standard_Real Tol2d,
106          const Standard_Integer NbIterations):
107          ParError(FirstPoint, LastPoint,0.0) 
108 {
109   Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters, 
110           Knots, Mults, Deg, Tol3d, Tol2d, NbIterations);
111 }
112
113
114 AppParCurves_BSpGradient::
115    AppParCurves_BSpGradient(const MultiLine& SSP,
116          const Standard_Integer FirstPoint,
117          const Standard_Integer LastPoint,
118          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
119          math_Vector& Parameters,
120          const TColStd_Array1OfReal& Knots,
121          const TColStd_Array1OfInteger& Mults,
122          const Standard_Integer Deg,
123          const Standard_Real Tol3d,
124          const Standard_Real Tol2d,
125          const Standard_Integer NbIterations,
126          const Standard_Real lambda1,
127          const Standard_Real lambda2):
128          ParError(FirstPoint, LastPoint,0.0) 
129 {
130   mylambda1 = lambda1;
131   mylambda2 = lambda2;
132   islambdadefined = Standard_True;
133   Perform(SSP, FirstPoint, LastPoint, TheConstraints, Parameters, 
134           Knots, Mults, Deg, Tol3d, Tol2d, NbIterations);
135 }
136
137
138
139
140
141
142
143 void AppParCurves_BSpGradient::
144   Perform(const MultiLine& SSP,
145          const Standard_Integer FirstPoint,
146          const Standard_Integer LastPoint,
147          const Handle(AppParCurves_HArray1OfConstraintCouple)& TheConstraints,
148          math_Vector& Parameters,
149          const TColStd_Array1OfReal& Knots,
150          const TColStd_Array1OfInteger& Mults,
151          const Standard_Integer Deg,
152          const Standard_Real Tol3d,
153          const Standard_Real Tol2d,
154          const Standard_Integer NbIterations)
155 {
156
157 //  Standard_Boolean grad = Standard_True;
158   Standard_Integer i, j, k, i2, l;
159   Standard_Real UF, DU, Fval = 0.0, FU, DFU;
160   Standard_Integer nbP3d = ToolLine::NbP3d(SSP);
161   Standard_Integer nbP2d = ToolLine::NbP2d(SSP);
162   Standard_Integer mynbP3d=nbP3d, mynbP2d=nbP2d;
163   Standard_Integer nbP = nbP3d + nbP2d;
164 //  gp_Pnt Pt, P1, P2;
165   gp_Pnt Pt;
166 //  gp_Pnt2d Pt2d, P12d, P22d;
167   gp_Pnt2d Pt2d;
168 //  gp_Vec V1, V2, MyV;
169   gp_Vec V1, MyV;
170 //  gp_Vec2d V12d, V22d, MyV2d;
171   gp_Vec2d V12d, MyV2d;
172   Done = Standard_False;
173   
174   if (nbP3d == 0) mynbP3d = 1;
175   if (nbP2d == 0) mynbP2d = 1;
176   TColgp_Array1OfPnt TabP(1, mynbP3d);
177   TColgp_Array1OfPnt2d TabP2d(1, mynbP2d);
178   TColgp_Array1OfVec TabV(1, mynbP3d);
179   TColgp_Array1OfVec2d TabV2d(1, mynbP2d);
180
181   // Calcul de la fonction F= somme(||C(ui)-Ptli||2):
182   // Appel a une fonction heritant de MultipleVarFunctionWithGradient
183   // pour calculer F et grad_F.
184   // ================================================================
185
186   Standard_Integer nbpoles = -Deg -1;
187   for (i = Mults.Lower() ;i <= Mults.Upper(); i++) {
188     nbpoles += Mults(i);
189   }
190
191   TColgp_Array1OfPnt   TabPole(1, nbpoles);
192   TColgp_Array1OfPnt2d TabPole2d(1, nbpoles);
193   TColgp_Array1OfPnt    ThePoles(1, (nbpoles)*mynbP3d);
194   TColgp_Array1OfPnt2d  ThePoles2d(1, (nbpoles)*mynbP2d);
195
196
197   AppParCurves_Constraint 
198     FirstCons = FirstConstraint(TheConstraints, FirstPoint), 
199     LastCons  = LastConstraint(TheConstraints, LastPoint);
200
201
202   AppParCurves_BSpParFunction MyF(SSP, FirstPoint,LastPoint, TheConstraints, 
203                                   Parameters, Knots, Mults, nbpoles);
204
205
206
207   if (FirstCons >= AppParCurves_TangencyPoint ||
208       LastCons >= AppParCurves_TangencyPoint) {
209
210     if (!islambdadefined) {
211       AppParCurves_BSpParLeastSquare thefitt(SSP, Knots, Mults, FirstPoint,
212                                              LastPoint, FirstCons, LastCons, 
213                                              Parameters, nbpoles);
214       if (FirstCons >= AppParCurves_TangencyPoint) {
215         mylambda1 = thefitt.FirstLambda();
216         MyF.SetFirstLambda(mylambda1);
217       }
218       if (LastCons >= AppParCurves_TangencyPoint) {
219         mylambda2 = thefitt.LastLambda();
220         MyF.SetLastLambda(mylambda2);
221       }
222     }
223     else {
224       MyF.SetFirstLambda(mylambda1);
225       MyF.SetLastLambda(mylambda2);
226     }
227   }
228
229
230   MyF.Value(Parameters, Fval);
231   MError3d = MyF.MaxError3d();
232   MError2d = MyF.MaxError2d();
233   SCU = MyF.CurveValue();
234
235   if (MError3d > Tol3d || MError2d > Tol2d) {
236
237     // Stockage des Poles des courbes pour projeter:
238     // ============================================
239     i2 = 0;
240     for (k = 1; k <= nbP3d; k++) {
241       SCU.Curve(k, TabPole);
242       for (j=1; j<=nbpoles; j++) ThePoles(j+i2) = TabPole(j);
243       i2 += nbpoles;
244     }
245     i2 = 0;
246     for (k = 1; k <= nbP2d; k++) {
247       SCU.Curve(nbP3d+k, TabPole2d);
248       for (j=1; j<=nbpoles; j++) ThePoles2d(j+i2) = TabPole2d(j);
249       i2 += nbpoles;
250     }
251     
252     //  Une iteration rapide de projection est faite par la methode de 
253     //  Rogers & Fog 89, methode equivalente a Hoschek 88 qui ne necessite pas
254     //  le calcul de D2.
255     
256     const math_Matrix& A = MyF.FunctionMatrix();
257     const math_Matrix& DA = MyF.DerivativeFunctionMatrix();
258     const math_IntegerVector& Index = MyF.Index();
259     Standard_Real aa, da, a, b, c, d , e , f, px, py, pz;
260     Standard_Integer indexdeb, indexfin;
261
262     for (j = FirstPoint+1; j <= LastPoint-1; j++) {
263       
264       UF = Parameters(j);
265       if (nbP3d != 0 && nbP2d != 0) ToolLine::Value(SSP, j, TabP, TabP2d);
266       else if (nbP2d != 0)          ToolLine::Value(SSP, j, TabP2d);
267       else                          ToolLine::Value(SSP, j, TabP);
268       
269       FU = 0.0;
270       DFU = 0.0;
271       i2 = 0;
272       
273       indexdeb = Index(j) + 1;
274       indexfin = indexdeb + Deg;
275
276       for (k = 1; k <= nbP3d; k++) {
277         a = b = c = d = e = f = 0.0;
278         for (l = indexdeb; l <= indexfin; l++) {
279           Pt = ThePoles(l+i2); 
280           px = Pt.X(); py = Pt.Y(); pz = Pt.Z();
281           aa = A(j, l); da = DA(j, l);
282           a += aa* px; d += da* px;
283           b += aa* py; e += da* py;
284           c += aa* pz; f += da* pz;
285         }
286         Pt.SetCoord(a, b, c);
287         V1.SetCoord(d, e, f);
288         i2 += nbpoles;
289
290         MyV = gp_Vec(Pt, TabP(k));
291         FU += MyV*V1;
292         DFU += V1.SquareMagnitude();
293       }
294       i2 = 0;
295       for (k = 1; k <= nbP2d; k++) {
296         a = b = d = e = 0.0;
297         for (l = indexdeb; l <= indexfin; l++) {
298           Pt2d = ThePoles2d(l+i2); 
299           px = Pt2d.X(); py = Pt2d.Y();
300           aa = A(j, l); da = DA(j, l);
301           a += aa* px; d += da* px;
302           b += aa* py; e += da* py;
303         }
304         Pt2d.SetCoord(a, b);
305         V12d.SetCoord(d, e);
306         i2 += nbpoles;
307
308         MyV2d = gp_Vec2d(Pt2d, TabP2d(k));
309         FU += MyV2d*V12d;
310         DFU += V12d.SquareMagnitude();
311       }
312       
313       if (DFU >= RealEpsilon()) {
314         DU = FU/DFU;
315         DU = Sign(Min(5.e-02, Abs(DU)), DU);
316         UF += DU;
317         Parameters(j) = UF;
318       }
319     }
320     
321     MyF.Value(Parameters, Fval);
322     MError3d = MyF.MaxError3d();
323     MError2d = MyF.MaxError2d();
324   }
325
326
327   if (MError3d<= Tol3d && MError2d <= Tol2d) {
328     Done = Standard_True;
329   }
330   else if (NbIterations != 0) {
331     // NbIterations de gradient conjugue:
332     // =================================
333     Standard_Real Eps = 1.e-07;
334     AppParCurves_BSpGradient_BFGS FResol(MyF, Parameters, Tol3d, 
335                                          Tol2d, Eps, NbIterations);
336   }
337
338   SCU = MyF.CurveValue();
339
340
341   AvError = 0.;
342   for (j = FirstPoint; j <= LastPoint; j++) {  
343     Parameters(j) = MyF.NewParameters()(j);
344     // Recherche des erreurs maxi et moyenne a un index donne:
345     for (k = 1; k <= nbP; k++) {
346       ParError(j) = Max(ParError(j), MyF.Error(j, k));
347     }
348     AvError += ParError(j);
349   }
350   AvError = AvError/(LastPoint-FirstPoint+1);
351
352
353   MError3d = MyF.MaxError3d();
354   MError2d = MyF.MaxError2d();
355   if (MError3d <= Tol3d && MError2d <= Tol2d) {
356     Done = Standard_True;
357   }
358
359 }
360
361
362
363 AppParCurves_MultiBSpCurve AppParCurves_BSpGradient::Value() const {
364   return SCU;
365 }
366
367
368 Standard_Boolean AppParCurves_BSpGradient::IsDone() const {
369   return Done;
370 }
371
372
373 Standard_Real AppParCurves_BSpGradient::Error(const Standard_Integer Index) const {
374   return ParError(Index);
375 }
376
377 Standard_Real AppParCurves_BSpGradient::AverageError() const {
378   return AvError;
379 }
380
381 Standard_Real AppParCurves_BSpGradient::MaxError3d() const {
382   return MError3d;
383 }
384
385 Standard_Real AppParCurves_BSpGradient::MaxError2d() const {
386   return MError2d;
387 }
388
389