0025720: Incorrect code of math classes can lead to unpredicted behavior of algorithms
[occt.git] / src / math / math_NewtonFunctionSetRoot.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 //#endif
20
21 #include <math_NewtonFunctionSetRoot.ixx>
22 #include <math_Recipes.hxx>
23 #include <math_FunctionSetWithDerivatives.hxx>
24
25
26 //=======================================================================
27 //function : math_NewtonFunctionSetRoot
28 //purpose  : Constructor
29 //=======================================================================
30 math_NewtonFunctionSetRoot::math_NewtonFunctionSetRoot(
31   math_FunctionSetWithDerivatives& theFunction,
32   const math_Vector&               theXTolerance,
33   const Standard_Real              theFTolerance,
34   const Standard_Integer           theNbIterations)
35
36 : TolX    (1, theFunction.NbVariables()),
37   TolF    (theFTolerance),
38   Indx    (1, theFunction.NbVariables()),
39   Scratch (1, theFunction.NbVariables()),
40   Sol     (1, theFunction.NbVariables()),
41   DeltaX  (1, theFunction.NbVariables()),
42   FValues (1, theFunction.NbVariables()),
43   Jacobian(1, theFunction.NbVariables(), 1, theFunction.NbVariables()),
44   Done    (Standard_False),
45   State   (0),
46   Iter    (0),
47   Itermax (theNbIterations)
48 {
49   SetTolerance(theXTolerance);
50 }
51
52 //=======================================================================
53 //function : math_NewtonFunctionSetRoot
54 //purpose  : Constructor
55 //=======================================================================
56 math_NewtonFunctionSetRoot::math_NewtonFunctionSetRoot(
57   math_FunctionSetWithDerivatives& theFunction,
58   const Standard_Real              theFTolerance,
59   const Standard_Integer           theNbIterations)
60
61 : TolX    (1, theFunction.NbVariables()),
62   TolF    (theFTolerance),
63   Indx    (1, theFunction.NbVariables()),
64   Scratch (1, theFunction.NbVariables()),
65   Sol     (1, theFunction.NbVariables()),
66   DeltaX  (1, theFunction.NbVariables()),
67   FValues (1, theFunction.NbVariables()),
68   Jacobian(1, theFunction.NbVariables(), 1, theFunction.NbVariables()),
69   Done    (Standard_False),
70   State   (0),
71   Iter    (0),
72   Itermax (theNbIterations)
73 {
74 }
75
76 //=======================================================================
77 //function : ~math_NewtonFunctionSetRoot
78 //purpose  : Destructor
79 //=======================================================================
80 math_NewtonFunctionSetRoot::~math_NewtonFunctionSetRoot()
81 {
82 }
83
84 //=======================================================================
85 //function : SetTolerance
86 //purpose  : 
87 //=======================================================================
88 void math_NewtonFunctionSetRoot::SetTolerance(const math_Vector& theXTolerance)
89 {
90   for (Standard_Integer i = 1; i <= TolX.Length(); ++i)
91     TolX(i) = theXTolerance(i);
92 }
93
94 //=======================================================================
95 //function : Perform
96 //purpose  : 
97 //=======================================================================
98 void math_NewtonFunctionSetRoot::Perform(
99   math_FunctionSetWithDerivatives& theFunction,
100   const math_Vector&               theStartingPoint)
101 {
102   const math_Vector anInf(1, theFunction.NbVariables(), RealFirst());
103   const math_Vector aSup (1, theFunction.NbVariables(), RealLast ());
104
105   Perform(theFunction, theStartingPoint, anInf, aSup);
106 }
107
108 //=======================================================================
109 //function : Perform
110 //purpose  : 
111 //=======================================================================
112 void math_NewtonFunctionSetRoot::Perform(
113                            math_FunctionSetWithDerivatives& F,
114                            const math_Vector& StartingPoint,
115                            const math_Vector& InfBound,
116                            const math_Vector& SupBound) 
117 {
118
119   Standard_Real d;
120   Standard_Boolean OK;
121   Standard_Integer Error;
122   
123   Done = Standard_False;
124   Sol = StartingPoint;
125   OK = F.Values(Sol, FValues, Jacobian);
126   if(!OK) return;
127   for(Iter = 1; Iter <= Itermax; Iter++) {
128     for(Standard_Integer k = 1; k <= DeltaX.Length(); k++) {
129       DeltaX(k) = -FValues(k);
130     }
131     Error = LU_Decompose(Jacobian, Indx, d, Scratch, 1.0e-30);
132     if(Error) return; 
133     LU_Solve(Jacobian, Indx, DeltaX);
134     for(Standard_Integer i = 1; i <= Sol.Length(); i++) { 
135       Sol(i) += DeltaX(i);
136       
137       // Limitation de Sol dans les bornes [InfBound, SupBound] :
138       if (Sol(i) <= InfBound(i)) Sol(i) = InfBound(i);
139       if (Sol(i) >= SupBound(i)) Sol(i) = SupBound(i);
140       
141     }
142     OK = F.Values(Sol, FValues, Jacobian);
143     if(!OK) return;
144     if(IsSolutionReached(F)) { 
145       State = F.GetStateNumber();
146       Done = Standard_True; 
147       return;
148     }
149   }               
150 }
151
152 //=======================================================================
153 //function : Dump
154 //purpose  : 
155 //=======================================================================
156 void math_NewtonFunctionSetRoot::Dump(Standard_OStream& o) const 
157 {
158   o <<"math_NewtonFunctionSetRoot ";
159   if (Done) {
160     o << " Status = Done \n";
161     o << " Vector solution = " << Sol <<"\n";
162     o << " Value of the function at this solution = \n";
163     o << FValues <<"\n";
164     o << " Number of iterations = " << Iter <<"\n";
165   }
166   else {
167     o << "Status = not Done \n";
168   }
169 }