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

//static const char* sccsid = "@(#)math_BFGS.cxx	3.2 95/01/10"; // Do not delete this line. Used by sccs.

//#ifndef DEB
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#define No_Standard_DimensionError
//#endif

#include <math_BFGS.ixx>

#include <math_BracketMinimum.hxx>
#include <math_BrentMinimum.hxx>
#include <math_FunctionWithDerivative.hxx>
#include <math_MultipleVarFunctionWithGradient.hxx>
#include <math_Matrix.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;
#ifdef DEB
     Standard_Integer n = 
#endif
       Dir.Length();
     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(math_MultipleVarFunctionWithGradient& F,
                         const math_Vector& StartingPoint, 
                         const Standard_Real        Tolerance,
                         const Standard_Integer     NbIterations,
                         const Standard_Real        ZEPS) 
                         : TheLocation(1, StartingPoint.Length()),
                           TheGradient(1, StartingPoint.Length()) {

       XTol = Tolerance;
       EPSZ = ZEPS;
       Itermax = NbIterations;
       Perform(F, StartingPoint);
    }
                             
    math_BFGS::math_BFGS(math_MultipleVarFunctionWithGradient& F,
                         const Standard_Real        Tolerance,
                         const Standard_Integer     NbIterations,
                         const Standard_Real        ZEPS) 
                         : TheLocation(1, F.NbVariables()),
                           TheGradient(1, F.NbVariables()) {

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


    void math_BFGS::Delete()
    {}

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