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