0031007: Coding - eliminate warnings issued while compiling with -pedantic flag
[occt.git] / src / math / math_BFGS.cxx
1 // Copyright (c) 1997-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 under
7 // the terms of the GNU Lesser General Public License 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 //#ifndef OCCT_DEBUG
16 #define No_Standard_RangeError
17 #define No_Standard_OutOfRange
18 #define No_Standard_DimensionError
19
20 //#endif
21
22 #include <math_BFGS.hxx>
23 #include <math_BracketMinimum.hxx>
24 #include <math_BrentMinimum.hxx>
25 #include <math_FunctionWithDerivative.hxx>
26 #include <math_Matrix.hxx>
27 #include <math_MultipleVarFunctionWithGradient.hxx>
28 #include <Standard_DimensionError.hxx>
29 #include <StdFail_NotDone.hxx>
30 #include <Precision.hxx>
31
32 #define R 0.61803399
33 #define C (1.0-R)
34 #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
35 #define SIGN(a, b) ((b) > 0.0 ? fabs(a) : -fabs(a))
36 #define MOV3(a,b,c, d, e, f) (a)=(d); (b)= (e); (c)=(f);
37
38
39 // l'utilisation de math_BrentMinumim pur trouver un minimum dans une direction
40 // donnee n'est pas du tout optimale. voir peut etre interpolation cubique
41 // classique et aussi essayer "recherche unidimensionnelle economique"
42 // PROGRAMMATION MATHEMATIQUE (theorie et algorithmes) tome1 page 82.
43
44 class DirFunction : public math_FunctionWithDerivative {
45
46      math_Vector *P0;
47      math_Vector *Dir;
48      math_Vector *P;
49      math_Vector *G;
50      math_MultipleVarFunctionWithGradient *F;
51
52 public :
53
54      DirFunction(math_Vector& V1, 
55                  math_Vector& V2,
56                  math_Vector& V3,
57                  math_Vector& V4,
58                  math_MultipleVarFunctionWithGradient& f) ;
59
60      void Initialize(const math_Vector& p0, const math_Vector& dir) const;
61      void TheGradient(math_Vector& Grad);
62      virtual Standard_Boolean Value(const Standard_Real x, Standard_Real& fval) ;
63      virtual Standard_Boolean Values(const Standard_Real x, Standard_Real& fval, Standard_Real& D) ;
64      virtual Standard_Boolean Derivative(const Standard_Real x, Standard_Real& D) ;
65
66      
67 };
68
69      DirFunction::DirFunction(math_Vector& V1, 
70                               math_Vector& V2,
71                               math_Vector& V3,
72                               math_Vector& V4,
73                               math_MultipleVarFunctionWithGradient& f) {
74         
75         P0  = &V1;
76         Dir = &V2;
77         P   = &V3;
78         F   = &f;
79         G =   &V4;
80      }
81
82      void DirFunction::Initialize(const math_Vector& p0, 
83                                   const math_Vector& dir)  const{
84
85         *P0 = p0;
86         *Dir = dir;
87      }
88
89      void DirFunction::TheGradient(math_Vector& Grad) {
90        Grad = *G;
91      }
92
93
94      Standard_Boolean DirFunction::Value(const Standard_Real x, 
95                                          Standard_Real& fval) 
96      {
97         *P = *Dir;
98         P->Multiply(x);
99         P->Add(*P0); 
100         fval = 0.;
101         return F->Value(*P, fval);
102      }
103
104      Standard_Boolean DirFunction::Values(const Standard_Real x, 
105                                           Standard_Real& fval, 
106                                           Standard_Real& D) 
107      {
108         *P = *Dir;
109         P->Multiply(x);
110         P->Add(*P0);
111         fval = D = 0.;
112         if (F->Values(*P, fval, *G))
113         {
114           D = (*G).Multiplied(*Dir);
115           return Standard_True;
116         }
117         return Standard_False;
118      }
119      Standard_Boolean DirFunction::Derivative(const Standard_Real x, 
120                                               Standard_Real& D) 
121      {
122         *P = *Dir;
123         P->Multiply(x);
124         P->Add(*P0);        
125         Standard_Real fval;
126         D = 0.;
127         if (F->Values(*P, fval, *G))
128         {
129           D = (*G).Multiplied(*Dir);
130           return Standard_True;
131         }
132         return Standard_False;
133      }
134
135 //=======================================================================
136 //function : ComputeInitScale
137 //purpose  : Compute the appropriate initial value of scale factor to apply
138 //           to the direction to approach to the minimum of the function
139 //=======================================================================
140 static Standard_Boolean ComputeInitScale(const Standard_Real theF0,
141                                          const math_Vector& theDir,
142                                          const math_Vector& theGr,
143                                          Standard_Real& theScale)
144 {
145   Standard_Real dy1 = theGr * theDir;
146   if (Abs(dy1) < RealSmall())
147     return Standard_False;
148   Standard_Real aHnr1 = theDir.Norm2();
149   Standard_Real alfa = 0.7*(-theF0) / dy1;
150   theScale = 0.015 / Sqrt(aHnr1);
151   if (theScale > alfa)
152     theScale = alfa;
153   return Standard_True;
154 }
155
156 //=======================================================================
157 //function : ComputeMinMaxScale
158 //purpose  : For a given point and direction, and bounding box,
159 //           find min and max scale factors with which the point reaches borders
160 //           if we apply translation Point+Dir*Scale.
161 //return   : True if found, False if point is out of bounds.
162 //=======================================================================
163 static Standard_Boolean ComputeMinMaxScale(const math_Vector& thePoint,
164                                            const math_Vector& theDir,
165                                            const math_Vector& theLeft,
166                                            const math_Vector& theRight,
167                                            Standard_Real& theMinScale,
168                                            Standard_Real& theMaxScale)
169 {
170   Standard_Integer anIdx;
171   for (anIdx = 1; anIdx <= theLeft.Upper(); anIdx++)
172   {
173     Standard_Real aLeft = theLeft(anIdx) - thePoint(anIdx);
174     Standard_Real aRight = theRight(anIdx) - thePoint(anIdx);
175     if (Abs(theDir(anIdx)) > RealSmall())
176     {
177       // use PConfusion to get off a little from the bounds to prevent
178       // possible refuse in Value function.
179       Standard_Real aLScale = (aLeft + Precision::PConfusion()) / theDir(anIdx);
180       Standard_Real aRScale = (aRight - Precision::PConfusion()) / theDir(anIdx);
181       if (Abs(aLeft) < Precision::PConfusion())
182       {
183         // point is on the left border
184         theMaxScale = Min(theMaxScale, Max(0., aRScale));
185         theMinScale = Max(theMinScale, Min(0., aRScale));
186       }
187       else if (Abs(aRight) < Precision::PConfusion())
188       {
189         // point is on the right border
190         theMaxScale = Min(theMaxScale, Max(0., aLScale));
191         theMinScale = Max(theMinScale, Min(0., aLScale));
192       }
193       else if (aLeft * aRight < 0)
194       {
195         // point is inside allowed range
196         theMaxScale = Min(theMaxScale, Max(aLScale, aRScale));
197         theMinScale = Max(theMinScale, Min(aLScale, aRScale));
198       }
199       else
200         // point is out of bounds
201         return Standard_False;
202     }
203     else
204     {
205       // Direction is parallel to the border.
206       // Check that the point is not out of bounds
207       if (aLeft > Precision::PConfusion() ||
208         aRight < -Precision::PConfusion())
209       {
210         return Standard_False;
211       }
212     }
213   }
214   return Standard_True;
215 }
216
217 static Standard_Boolean MinimizeDirection(math_Vector&   P,
218                                           Standard_Real  F0,
219                                           math_Vector&   Gr,
220                                           math_Vector&   Dir,
221                                           Standard_Real& Result,
222                                           DirFunction&   F,
223                                           Standard_Boolean isBounds,
224                                           const math_Vector& theLeft,
225                                           const math_Vector& theRight)
226 {
227   Standard_Real lambda;
228   if (!ComputeInitScale(F0, Dir, Gr, lambda))
229     return Standard_False;
230
231   // by default the scaling range is unlimited
232   Standard_Real aMinLambda = -Precision::Infinite();
233   Standard_Real aMaxLambda = Precision::Infinite();
234   if (isBounds)
235   {
236     // limit the scaling range taking into account the bounds
237     if (!ComputeMinMaxScale(P, Dir, theLeft, theRight, aMinLambda, aMaxLambda))
238       return Standard_False;
239
240     if (aMinLambda > -Precision::PConfusion() && aMaxLambda < Precision::PConfusion())
241     {
242       // Point is on the border and the direction shows outside.
243       // Make direction to go along the border
244       for (Standard_Integer anIdx = 1; anIdx <= theLeft.Upper(); anIdx++)
245       {
246         if ((Abs(P(anIdx) - theRight(anIdx)) < Precision::PConfusion() && Dir(anIdx) > 0.0) ||
247             (Abs(P(anIdx) - theLeft(anIdx))  < Precision::PConfusion() && Dir(anIdx) < 0.0))
248         {
249           Dir(anIdx) = 0.0;
250         }
251       }
252
253       // re-compute scale values with new direction
254       if (!ComputeInitScale(F0, Dir, Gr, lambda))
255         return Standard_False;
256       if (!ComputeMinMaxScale(P, Dir, theLeft, theRight, aMinLambda, aMaxLambda))
257         return Standard_False;
258     }
259     lambda = Min(lambda, aMaxLambda);
260     lambda = Max(lambda, aMinLambda);
261   }
262
263   F.Initialize(P, Dir);
264   Standard_Real F1;
265   if (!F.Value(lambda, F1))
266     return Standard_False;
267   math_BracketMinimum Bracket(0.0, lambda);
268   if (isBounds)
269     Bracket.SetLimits(aMinLambda, aMaxLambda);
270   Bracket.SetFA(F0);
271   Bracket.SetFB(F1);
272   Bracket.Perform(F);
273   if (Bracket.IsDone()) {
274     // find minimum inside the bracket
275     Standard_Real ax, xx, bx, Fax, Fxx, Fbx;
276     Bracket.Values(ax, xx, bx);
277     Bracket.FunctionValues(Fax, Fxx, Fbx);
278
279     Standard_Integer niter = 100;
280     Standard_Real tol = 1.e-03;
281     math_BrentMinimum Sol(tol, Fxx, niter, 1.e-08);
282     Sol.Perform(F, ax, xx, bx);
283     if (Sol.IsDone()) {
284       Standard_Real Scale = Sol.Location();
285       Result = Sol.Minimum();
286       Dir.Multiply(Scale);
287       P.Add(Dir);
288       return Standard_True;
289     }
290   }
291   else if (isBounds)
292   {
293     // Bracket definition is failure. If the bounds are defined then
294     // set current point to intersection with bounds
295     Standard_Real aFMin, aFMax;
296     if (!F.Value(aMinLambda, aFMin))
297       return Standard_False;
298     if (!F.Value(aMaxLambda, aFMax))
299       return Standard_False;
300     Standard_Real aBestLambda;
301     if (aFMin < aFMax)
302     {
303       aBestLambda = aMinLambda;
304       Result = aFMin;
305     }
306     else
307     {
308       aBestLambda = aMaxLambda;
309       Result = aFMax;
310     }
311     Dir.Multiply(aBestLambda);
312     P.Add(Dir);
313     return Standard_True;
314   }
315   return Standard_False;
316 }
317
318
319 void  math_BFGS::Perform(math_MultipleVarFunctionWithGradient& F,
320                          const math_Vector& StartingPoint) {
321   
322        Standard_Boolean Good;
323        Standard_Integer n = TheLocation.Length();
324        Standard_Integer j, i;
325        Standard_Real fae, fad, fac;
326
327        math_Vector xi(1, n), dg(1, n), hdg(1, n);
328        math_Matrix hessin(1, n, 1, n);
329        hessin.Init(0.0);
330
331        math_Vector Temp1(1, n);
332        math_Vector Temp2(1, n);
333        math_Vector Temp3(1, n);
334        math_Vector Temp4(1, n);
335        DirFunction F_Dir(Temp1, Temp2, Temp3, Temp4, F);
336
337        TheLocation = StartingPoint;
338        Good = F.Values(TheLocation, PreviousMinimum, TheGradient);
339        if(!Good) { 
340          Done = Standard_False;
341          TheStatus = math_FunctionError;
342          return;
343        }
344        for(i = 1; i <= n; i++) {
345          hessin(i, i) = 1.0;
346          xi(i) = -TheGradient(i);
347        }
348
349
350        for(nbiter = 1; nbiter <= Itermax; nbiter++) {
351          TheMinimum = PreviousMinimum;
352          Standard_Boolean IsGood = MinimizeDirection(TheLocation, TheMinimum,
353                                                      TheGradient,
354                                                      xi, TheMinimum, F_Dir,
355                                                      myIsBoundsDefined, myLeft, myRight);
356          if(!IsGood) {
357            Done = Standard_False;
358            TheStatus = math_DirectionSearchError;
359            return;
360          }
361          if(IsSolutionReached(F)) {
362            Done = Standard_True;
363            TheStatus = math_OK;
364            return;
365          }
366          if (nbiter == Itermax) {
367            Done = Standard_False;
368            TheStatus = math_TooManyIterations;
369            return;
370          }
371          PreviousMinimum = TheMinimum;
372
373          dg = TheGradient;
374
375          Good = F.Values(TheLocation, TheMinimum, TheGradient);
376          if(!Good) { 
377            Done = Standard_False;
378            TheStatus = math_FunctionError;
379            return;
380          }
381
382          for(i = 1; i <= n; i++) {
383            dg(i) = TheGradient(i) - dg(i);
384          }
385          for(i = 1; i <= n; i++) {
386            hdg(i) = 0.0;
387            for (j = 1; j <= n; j++) 
388              hdg(i) += hessin(i, j) * dg(j);
389          }
390          fac = fae = 0.0;
391          for(i = 1; i <= n; i++) {
392            fac += dg(i) * xi(i);
393            fae += dg(i) * hdg(i);
394          }
395          fac = 1.0 / fac;
396          fad = 1.0 / fae;
397
398          for(i = 1; i <= n; i++) 
399            dg(i) = fac * xi(i) - fad * hdg(i);
400          
401          for(i = 1; i <= n; i++) 
402            for(j = 1; j <= n; j++)
403              hessin(i, j) += fac * xi(i) * xi(j)
404                - fad * hdg(i) * hdg(j) + fae * dg(i) * dg(j);
405
406          for(i = 1; i <= n; i++) {
407            xi(i) = 0.0;
408            for (j = 1; j <= n; j++) xi(i) -= hessin(i, j) * TheGradient(j);
409          }  
410        }
411        Done = Standard_False;
412        TheStatus = math_TooManyIterations;
413        return;
414    }
415
416     Standard_Boolean math_BFGS::IsSolutionReached(
417                            math_MultipleVarFunctionWithGradient&) const {
418
419        return 2.0 * fabs(TheMinimum - PreviousMinimum) <= 
420               XTol * (fabs(TheMinimum) + fabs(PreviousMinimum) + EPSZ);
421     }
422                              
423     math_BFGS::math_BFGS(const Standard_Integer     NbVariables,
424                          const Standard_Real        Tolerance,
425                          const Standard_Integer     NbIterations,
426                          const Standard_Real        ZEPS) 
427     : TheStatus(math_OK),
428       TheLocation(1, NbVariables),
429       TheGradient(1, NbVariables),
430       PreviousMinimum(0.),
431       TheMinimum(0.),
432       XTol(Tolerance),
433       EPSZ(ZEPS),
434       nbiter(0),
435       myIsBoundsDefined(Standard_False),
436       myLeft(1, NbVariables, 0.0),
437       myRight(1, NbVariables, 0.0),
438       Done(Standard_False),
439       Itermax(NbIterations)
440     {
441     }
442
443
444     math_BFGS::~math_BFGS()
445     {
446     }
447
448     void math_BFGS::Dump(Standard_OStream& o) const {
449
450        o<< "math_BFGS resolution: ";
451        if(Done) {
452          o << " Status = Done \n";
453          o <<" Location Vector = " << Location() << "\n";
454          o <<" Minimum value = "<< Minimum()<<"\n";
455          o <<" Number of iterations = "<<NbIterations() <<"\n";
456        }
457        else {
458          o<< " Status = not Done because " << (Standard_Integer)TheStatus << "\n";
459        }
460     }
461
462 //=======================================================================
463 //function : SetBoundary
464 //purpose  : Set boundaries for conditional optimization
465 //=======================================================================
466 void math_BFGS::SetBoundary(const math_Vector& theLeftBorder,
467                             const math_Vector& theRightBorder)
468 {
469   myLeft = theLeftBorder;
470   myRight = theRightBorder;
471   myIsBoundsDefined = Standard_True;
472 }