[occt.git] / src / math / math_BFGS.cxx

// Copyright (c) 1997-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

//#ifndef OCCT_DEBUG
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#define No_Standard_DimensionError

//#endif

#include <math_BFGS.hxx>
#include <math_BracketMinimum.hxx>
#include <math_BrentMinimum.hxx>
#include <math_FunctionWithDerivative.hxx>
#include <math_Matrix.hxx>
#include <math_MultipleVarFunctionWithGradient.hxx>
#include <Standard_DimensionError.hxx>
#include <StdFail_NotDone.hxx>

#define R 0.61803399
#define C (1.0-R)
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
#define SIGN(a, b) ((b) > 0.0 ? fabs(a) : -fabs(a))
#define MOV3(a,b,c, d, e, f) (a)=(d); (b)= (e); (c)=(f);


// l'utilisation de math_BrentMinumim pur trouver un minimum dans une direction
// donnee n'est pas du tout optimale. voir peut etre interpolation cubique
// classique et aussi essayer "recherche unidimensionnelle economique"
// PROGRAMMATION MATHEMATIQUE (theorie et algorithmes) tome1 page 82.

class DirFunction : public math_FunctionWithDerivative {

     math_Vector *P0;
     math_Vector *Dir;
     math_Vector *P;
     math_Vector *G;
     math_MultipleVarFunctionWithGradient *F;

public :

     DirFunction(math_Vector& V1, 
                 math_Vector& V2,
                 math_Vector& V3,
		 math_Vector& V4,
                 math_MultipleVarFunctionWithGradient& f) ;

     void Initialize(const math_Vector& p0, const math_Vector& dir) const;
     void TheGradient(math_Vector& Grad);
     virtual Standard_Boolean Value(const Standard_Real x, Standard_Real& fval) ;
     virtual Standard_Boolean Values(const Standard_Real x, Standard_Real& fval, Standard_Real& D) ;
     virtual Standard_Boolean Derivative(const Standard_Real x, Standard_Real& D) ;

     
};

     DirFunction::DirFunction(math_Vector& V1, 
                              math_Vector& V2,
                              math_Vector& V3,
			      math_Vector& V4,
                              math_MultipleVarFunctionWithGradient& f) {
        
        P0  = &V1;
        Dir = &V2;
        P   = &V3;
        F   = &f;
	G =   &V4;
     }

     void DirFunction::Initialize(const math_Vector& p0, 
                                  const math_Vector& dir)  const{

        *P0 = p0;
        *Dir = dir;
     }

     void DirFunction::TheGradient(math_Vector& Grad) {
       Grad = *G;
     }


     Standard_Boolean DirFunction::Value(const Standard_Real x, 
					 Standard_Real& fval) 
     {
        *P = *Dir;
        P->Multiply(x);
        P->Add(*P0);        
        F->Value(*P, fval);
        return Standard_True;
     }

     Standard_Boolean DirFunction::Values(const Standard_Real x, 
					  Standard_Real& fval, 
					  Standard_Real& D) 
     {
        *P = *Dir;
        P->Multiply(x);
        P->Add(*P0);  
        F->Values(*P, fval, *G);
	D = (*G).Multiplied(*Dir);
        return Standard_True;
     }
     Standard_Boolean DirFunction::Derivative(const Standard_Real x, 
					      Standard_Real& D) 
     {
        *P = *Dir;
        P->Multiply(x);
        P->Add(*P0);        
	Standard_Real fval;
        F->Values(*P, fval, *G);
	D = (*G).Multiplied(*Dir);
        return Standard_True;
     }


static Standard_Boolean MinimizeDirection(math_Vector&   P,
					  Standard_Real  F0,
					  math_Vector&   Gr,
					  math_Vector&   Dir,
					  Standard_Real& Result,
					  DirFunction&   F) {


     Standard_Real ax, xx, bx, Fax, Fxx, Fbx, F1;
     F.Initialize(P, Dir);

     Standard_Real dy1, Hnr1, lambda, alfa=0;
     dy1 = Gr*Dir;
     if (dy1 != 0) {
       Hnr1 = Dir.Norm2();
       alfa = 0.7*(-F0)/dy1;
       lambda = 0.015/Sqrt(Hnr1);
     }
     else lambda = 1.0;
     if (lambda > alfa) lambda = alfa;
     F.Value(lambda, F1);
     math_BracketMinimum Bracket(F, 0.0, lambda, F0, F1);
     if(Bracket.IsDone()) {
       Bracket.Values(ax, xx, bx);
       Bracket.FunctionValues(Fax, Fxx, Fbx);

       Standard_Integer niter = 100;
       Standard_Real tol = 1.e-03;
       math_BrentMinimum Sol(tol, Fxx, niter, 1.e-08);
       Sol.Perform(F, ax, xx, bx);
       if(Sol.IsDone()) {
	 Standard_Real Scale = Sol.Location();
	 Result = Sol.Minimum();
	 Dir.Multiply(Scale);
	 P.Add(Dir);
	 return Standard_True;
       }
     }
     return Standard_False;
   }


void  math_BFGS::Perform(math_MultipleVarFunctionWithGradient& F,
                         const math_Vector& StartingPoint) {
  
       Standard_Boolean Good;
       Standard_Integer n = TheLocation.Length();
       Standard_Integer j, i;
       Standard_Real fae, fad, fac;

       math_Vector xi(1, n), dg(1, n), hdg(1, n);
       math_Matrix hessin(1, n, 1, n);
       hessin.Init(0.0);

       math_Vector Temp1(1, n);
       math_Vector Temp2(1, n);
       math_Vector Temp3(1, n);
       math_Vector Temp4(1, n);
       DirFunction F_Dir(Temp1, Temp2, Temp3, Temp4, F);

       TheLocation = StartingPoint;
       Good = F.Values(TheLocation, PreviousMinimum, TheGradient);
       if(!Good) { 
         Done = Standard_False;
         TheStatus = math_FunctionError;
         return;
       }
       for(i = 1; i <= n; i++) {
         hessin(i, i) = 1.0;
	 xi(i) = -TheGradient(i);
       }


       for(nbiter = 1; nbiter <= Itermax; nbiter++) {
	 TheMinimum = PreviousMinimum;
         Standard_Boolean IsGood = MinimizeDirection(TheLocation, TheMinimum,
						     TheGradient,
                                                     xi, TheMinimum, F_Dir);
         if(!IsGood) {
           Done = Standard_False;
           TheStatus = math_DirectionSearchError;
           return;
         }
         if(IsSolutionReached(F)) {
           Done = Standard_True;
           TheStatus = math_OK;
           return;
         }
	 if (nbiter == Itermax) {
	   Done = Standard_False;
	   TheStatus = math_TooManyIterations;
	   return;
	 }
         PreviousMinimum = TheMinimum;

	 dg = TheGradient;

         Good = F.Values(TheLocation, TheMinimum, TheGradient);
         if(!Good) { 
           Done = Standard_False;
           TheStatus = math_FunctionError;
           return;
         }

         for(i = 1; i <= n; i++) {
	   dg(i) = TheGradient(i) - dg(i);
	 }
	 for(i = 1; i <= n; i++) {
	   hdg(i) = 0.0;
	   for (j = 1; j <= n; j++) 
	     hdg(i) += hessin(i, j) * dg(j);
	 }
	 fac = fae = 0.0;
	 for(i = 1; i <= n; i++) {
	   fac += dg(i) * xi(i);
	   fae += dg(i) * hdg(i);
	 }
         fac = 1.0 / fac;
         fad = 1.0 / fae;

	 for(i = 1; i <= n; i++) 
	   dg(i) = fac * xi(i) - fad * hdg(i);
	 
         for(i = 1; i <= n; i++) 
           for(j = 1; j <= n; j++)
             hessin(i, j) += fac * xi(i) * xi(j)
               - fad * hdg(i) * hdg(j) + fae * dg(i) * dg(j);

	 for(i = 1; i <= n; i++) {
	   xi(i) = 0.0;
	   for (j = 1; j <= n; j++) xi(i) -= hessin(i, j) * TheGradient(j);
	 }  
       }
       Done = Standard_False;
       TheStatus = math_TooManyIterations;
       return;
   }

    Standard_Boolean math_BFGS::IsSolutionReached(
                           math_MultipleVarFunctionWithGradient&) const {

       return 2.0 * fabs(TheMinimum - PreviousMinimum) <= 
              XTol * (fabs(TheMinimum) + fabs(PreviousMinimum) + EPSZ);
    }
                             
    math_BFGS::math_BFGS(const Standard_Integer     NbVariables,
                         const Standard_Real        Tolerance,
                         const Standard_Integer     NbIterations,
                         const Standard_Real        ZEPS) 
                         : TheLocation(1, NbVariables),
                           TheGradient(1, NbVariables) {

       XTol = Tolerance;
       EPSZ = ZEPS;
       Itermax = NbIterations;
    }


    math_BFGS::~math_BFGS()
    {
    }

    void math_BFGS::Dump(Standard_OStream& o) const {

       o<< "math_BFGS resolution: ";
       if(Done) {
         o << " Status = Done \n";
	 o <<" Location Vector = " << Location() << "\n";
	 o <<" Minimum value = "<< Minimum()<<"\n";
	 o <<" Number of iterations = "<<NbIterations() <<"\n";;
       }
       else {
         o<< " Status = not Done because " << (Standard_Integer)TheStatus << "\n";
       }
    }
Commit	Line	Data
b311480e	1	// Copyright (c) 1997-1999 Matra Datavision
973c2be1	2	// Copyright (c) 1999-2014 OPEN CASCADE SAS
b311480e	3	//
973c2be1	4	// This file is part of Open CASCADE Technology software library.
b311480e	5	//
d5f74e42	6	// This library is free software; you can redistribute it and/or modify it under
d5f74e42	7	// the terms of the GNU Lesser General Public License version 2.1 as published
973c2be1	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.
b311480e	11	//
973c2be1	12	// Alternatively, this file may be used under the terms of Open CASCADE
973c2be1	13	// commercial license or contractual agreement.
b311480e	14
0797d9d3	15	//#ifndef OCCT_DEBUG
7fd59977	16	#define No_Standard_RangeError
	17	#define No_Standard_OutOfRange
	18	#define No_Standard_DimensionError
7fd59977	19
42cf5bc1	20	//#endif
7fd59977	21
42cf5bc1	22	#include <math_BFGS.hxx>
7fd59977	23	#include <math_BracketMinimum.hxx>
	24	#include <math_BrentMinimum.hxx>
	25	#include <math_FunctionWithDerivative.hxx>
7fd59977	26	#include <math_Matrix.hxx>
42cf5bc1	27	#include <math_MultipleVarFunctionWithGradient.hxx>
	28	#include <Standard_DimensionError.hxx>
	29	#include <StdFail_NotDone.hxx>
7fd59977	30
	31	#define R 0.61803399
	32	#define C (1.0-R)
	33	#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
	34	#define SIGN(a, b) ((b) > 0.0 ? fabs(a) : -fabs(a))
	35	#define MOV3(a,b,c, d, e, f) (a)=(d); (b)= (e); (c)=(f);
	36
	37
	38	// l'utilisation de math_BrentMinumim pur trouver un minimum dans une direction
	39	// donnee n'est pas du tout optimale. voir peut etre interpolation cubique
	40	// classique et aussi essayer "recherche unidimensionnelle economique"
	41	// PROGRAMMATION MATHEMATIQUE (theorie et algorithmes) tome1 page 82.
	42
	43	class DirFunction : public math_FunctionWithDerivative {
	44
	45	math_Vector *P0;
	46	math_Vector *Dir;
	47	math_Vector *P;
	48	math_Vector *G;
	49	math_MultipleVarFunctionWithGradient *F;
	50
	51	public :
	52
	53	DirFunction(math_Vector& V1,
	54	math_Vector& V2,
	55	math_Vector& V3,
	56	math_Vector& V4,
	57	math_MultipleVarFunctionWithGradient& f) ;
	58
	59	void Initialize(const math_Vector& p0, const math_Vector& dir) const;
	60	void TheGradient(math_Vector& Grad);
	61	virtual Standard_Boolean Value(const Standard_Real x, Standard_Real& fval) ;
	62	virtual Standard_Boolean Values(const Standard_Real x, Standard_Real& fval, Standard_Real& D) ;
	63	virtual Standard_Boolean Derivative(const Standard_Real x, Standard_Real& D) ;
	64
	65
	66	};
	67
	68	DirFunction::DirFunction(math_Vector& V1,
	69	math_Vector& V2,
	70	math_Vector& V3,
	71	math_Vector& V4,
	72	math_MultipleVarFunctionWithGradient& f) {
	73
	74	P0 = &V1;
	75	Dir = &V2;
	76	P = &V3;
	77	F = &f;
	78	G = &V4;
	79	}
	80
	81	void DirFunction::Initialize(const math_Vector& p0,
	82	const math_Vector& dir) const{
	83
	84	*P0 = p0;
	85	*Dir = dir;
	86	}
	87
	88	void DirFunction::TheGradient(math_Vector& Grad) {
	89	Grad = *G;
	90	}
	91
	92
	93	Standard_Boolean DirFunction::Value(const Standard_Real x,
94	Standard_Real& fval)
95	{
96	P = Dir;
97	P->Multiply(x);
98	P->Add(*P0);
99	F->Value(*P, fval);
100	return Standard_True;
101	}
102
103	Standard_Boolean DirFunction::Values(const Standard_Real x,
104	Standard_Real& fval,
105	Standard_Real& D)
106	{
107	P = Dir;
108	P->Multiply(x);
109	P->Add(*P0);
110	F->Values(P, fval, G);
111	D = (G).Multiplied(Dir);
112	return Standard_True;
113	}
114	Standard_Boolean DirFunction::Derivative(const Standard_Real x,
115	Standard_Real& D)
116	{
117	P = Dir;
118	P->Multiply(x);
119	P->Add(*P0);
120	Standard_Real fval;
121	F->Values(P, fval, G);
122	D = (G).Multiplied(Dir);
123	return Standard_True;
124	}
125
126
127	static Standard_Boolean MinimizeDirection(math_Vector& P,
128	Standard_Real F0,
129	math_Vector& Gr,
130	math_Vector& Dir,
131	Standard_Real& Result,
132	DirFunction& F) {
133
134
135
136	Standard_Real ax, xx, bx, Fax, Fxx, Fbx, F1;
137	F.Initialize(P, Dir);
138
139	Standard_Real dy1, Hnr1, lambda, alfa=0;
7fd59977	140	dy1 = Gr*Dir;
	141	if (dy1 != 0) {
	142	Hnr1 = Dir.Norm2();
	143	alfa = 0.7*(-F0)/dy1;
	144	lambda = 0.015/Sqrt(Hnr1);
	145	}
	146	else lambda = 1.0;
	147	if (lambda > alfa) lambda = alfa;
	148	F.Value(lambda, F1);
	149	math_BracketMinimum Bracket(F, 0.0, lambda, F0, F1);
	150	if(Bracket.IsDone()) {
	151	Bracket.Values(ax, xx, bx);
	152	Bracket.FunctionValues(Fax, Fxx, Fbx);
	153
	154	Standard_Integer niter = 100;
	155	Standard_Real tol = 1.e-03;
	156	math_BrentMinimum Sol(tol, Fxx, niter, 1.e-08);
	157	Sol.Perform(F, ax, xx, bx);
	158	if(Sol.IsDone()) {
	159	Standard_Real Scale = Sol.Location();
	160	Result = Sol.Minimum();
	161	Dir.Multiply(Scale);
	162	P.Add(Dir);
	163	return Standard_True;
	164	}
	165	}
	166	return Standard_False;
	167	}
	168
	169
	170	void math_BFGS::Perform(math_MultipleVarFunctionWithGradient& F,
	171	const math_Vector& StartingPoint) {
	172
	173	Standard_Boolean Good;
	174	Standard_Integer n = TheLocation.Length();
	175	Standard_Integer j, i;
	176	Standard_Real fae, fad, fac;
	177
	178	math_Vector xi(1, n), dg(1, n), hdg(1, n);
	179	math_Matrix hessin(1, n, 1, n);
	180	hessin.Init(0.0);
	181
	182	math_Vector Temp1(1, n);
	183	math_Vector Temp2(1, n);
	184	math_Vector Temp3(1, n);
	185	math_Vector Temp4(1, n);
	186	DirFunction F_Dir(Temp1, Temp2, Temp3, Temp4, F);
	187
	188	TheLocation = StartingPoint;
	189	Good = F.Values(TheLocation, PreviousMinimum, TheGradient);
	190	if(!Good) {
	191	Done = Standard_False;
	192	TheStatus = math_FunctionError;
	193	return;
	194	}
	195	for(i = 1; i <= n; i++) {
	196	hessin(i, i) = 1.0;
	197	xi(i) = -TheGradient(i);
	198	}
	199
	200
	201	for(nbiter = 1; nbiter <= Itermax; nbiter++) {
	202	TheMinimum = PreviousMinimum;
	203	Standard_Boolean IsGood = MinimizeDirection(TheLocation, TheMinimum,
204	TheGradient,
205	xi, TheMinimum, F_Dir);
206	if(!IsGood) {
207	Done = Standard_False;
208	TheStatus = math_DirectionSearchError;
209	return;
210	}
211	if(IsSolutionReached(F)) {
212	Done = Standard_True;
213	TheStatus = math_OK;
214	return;
215	}
216	if (nbiter == Itermax) {
217	Done = Standard_False;
218	TheStatus = math_TooManyIterations;
219	return;
220	}
221	PreviousMinimum = TheMinimum;
222
223	dg = TheGradient;
224
225	Good = F.Values(TheLocation, TheMinimum, TheGradient);
226	if(!Good) {
227	Done = Standard_False;
228	TheStatus = math_FunctionError;
229	return;
230	}
231
232	for(i = 1; i <= n; i++) {
233	dg(i) = TheGradient(i) - dg(i);
234	}
235	for(i = 1; i <= n; i++) {
236	hdg(i) = 0.0;
237	for (j = 1; j <= n; j++)
238	hdg(i) += hessin(i, j) * dg(j);
239	}
240	fac = fae = 0.0;
241	for(i = 1; i <= n; i++) {
242	fac += dg(i) * xi(i);
243	fae += dg(i) * hdg(i);
244	}
245	fac = 1.0 / fac;
246	fad = 1.0 / fae;
247
248	for(i = 1; i <= n; i++)
249	dg(i) = fac * xi(i) - fad * hdg(i);
250
251	for(i = 1; i <= n; i++)
252	for(j = 1; j <= n; j++)
253	hessin(i, j) += fac * xi(i) * xi(j)
254	- fad * hdg(i) * hdg(j) + fae * dg(i) * dg(j);
255
256	for(i = 1; i <= n; i++) {
257	xi(i) = 0.0;
258	for (j = 1; j <= n; j++) xi(i) -= hessin(i, j) * TheGradient(j);
259	}
260	}
261	Done = Standard_False;
262	TheStatus = math_TooManyIterations;
263	return;
264	}
265
266	Standard_Boolean math_BFGS::IsSolutionReached(
267	math_MultipleVarFunctionWithGradient&) const {
268
269	return 2.0 * fabs(TheMinimum - PreviousMinimum) <=
270	XTol * (fabs(TheMinimum) + fabs(PreviousMinimum) + EPSZ);
271	}
7fd59977	272
07f1a2e6	273	math_BFGS::math_BFGS(const Standard_Integer NbVariables,
7fd59977	274	const Standard_Real Tolerance,
	275	const Standard_Integer NbIterations,
	276	const Standard_Real ZEPS)
07f1a2e6	277	: TheLocation(1, NbVariables),
07f1a2e6	278	TheGradient(1, NbVariables) {
7fd59977	279
	280	XTol = Tolerance;
	281	EPSZ = ZEPS;
	282	Itermax = NbIterations;
	283	}
	284
	285
6da30ff1	286	math_BFGS::~math_BFGS()
	287	{
	288	}
7fd59977	289
	290	void math_BFGS::Dump(Standard_OStream& o) const {
	291
	292	o<< "math_BFGS resolution: ";
	293	if(Done) {
	294	o << " Status = Done \n";
	295	o <<" Location Vector = " << Location() << "\n";
	296	o <<" Minimum value = "<< Minimum()<<"\n";
	297	o <<" Number of iterations = "<<NbIterations() <<"\n";;
	298	}
	299	else {
	300	o<< " Status = not Done because " << (Standard_Integer)TheStatus << "\n";
	301	}
	302	}
	303
	304
	305