635a5972c02af468d233f9548cbcc533037d0cc4
[blender.git] / intern / iksolver / intern / IK_LineMinimizer.h
1 /**
2  * $Id$
3  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version. The Blender
9  * Foundation also sells licenses for use in proprietary software under
10  * the Blender License.  See http://www.blender.org/BL/ for information
11  * about this.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  *
22  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23  * All rights reserved.
24  *
25  * The Original Code is: all of this file.
26  *
27  * Contributor(s): none yet.
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #ifndef NAN_INCLUDED_IK_LineMinimizer_h
33
34 #define NAN_INCLUDED_IK_LineMinimizer_h
35
36 /**
37  * @author Laurence Bourn
38  * @date 28/6/2001
39  */
40
41 #include "MT_Scalar.h"
42 #include <vector>
43 #include "TNT/tntmath.h"
44 #include "MEM_NonCopyable.h"
45
46
47
48 #define GOLD 1.618034
49 #define GLIMIT 100.0
50 #define TINY 1.0e-20
51 #define ZEPS 1.0e-10
52
53 /** 
54  * Routines for line - minization in n dimensions
55  * these should be templated on the potenial function
56  * p instead of using a virtual class. Please see 
57  * numerical recipes in c for more details. www.nr.com
58  */
59
60 class DifferentiablePotenialFunction1d {
61 public :
62         virtual
63                 MT_Scalar 
64         Evaluate(
65                 const MT_Scalar x
66         ) = 0;
67
68         virtual
69                 MT_Scalar
70         Derivative(
71                 const MT_Scalar x
72         ) = 0;
73 };
74
75
76 /**
77  * TODO get rid of this class and make some
78  * template functions in a seperate namespace
79  */
80
81 class IK_LineMinimizer : public MEM_NonCopyable {
82
83 public :
84
85         /**
86          * Before we proceed with line minimization 
87          * we need to construct an initial bracket
88          * of a minima of the PotenialFunction
89          */
90
91         static
92                 void
93         InitialBracket1d (      
94                 MT_Scalar &ax,
95                 MT_Scalar &bx,
96                 MT_Scalar &cx,
97                 MT_Scalar &fa,
98                 MT_Scalar &fb,
99                 MT_Scalar &fc,
100                 DifferentiablePotenialFunction1d &potenial
101         ) {
102                 MT_Scalar ulim,u,r,q,fu;
103
104                 fa = potenial.Evaluate(ax);
105                 fb = potenial.Evaluate(bx);
106
107                 if (fb > fa) {
108                         std::swap(ax,bx);
109                         std::swap(fa,fb);
110                 }
111                 cx = bx + GOLD*(bx-ax);
112                 fc = potenial.Evaluate(cx);
113                 
114                 while (fb > fc) {
115
116                         r = (bx - ax) * (fb - fc);
117                         q = (bx - cx) * (fb - fa);
118                         u = bx - ((bx - cx)*q - (bx - ax) *r)/ 
119                                 (2 * TNT::sign(TNT::max(TNT::abs(q-r),TINY),q-r));
120                         ulim = bx + GLIMIT*(cx-bx);
121
122                         if ((bx-u)*(u-cx) > 0.0) {
123                                 fu = potenial.Evaluate(u);
124                                 if (fu < fc) {
125                                         ax = bx;
126                                         bx = u;
127                                         fa = fb;
128                                         fb = fu;
129                                         return;
130                                 } else if (fu > fb) {
131                                         cx = u;
132                                         fc = fu;
133                                         return;
134                                 }
135                                 u = cx + GOLD*(cx-bx);
136                                 fu = potenial.Evaluate(u);
137
138                         } else if ((cx - u)*(u - ulim) > 0.0) {
139                                 fu = potenial.Evaluate(u);
140  
141                                 if (fu < fc) {
142                                         bx = cx;
143                                         cx = u;
144                                         u = cx + GOLD*(cx - bx);
145                                         fb = fc;
146                                         fc = fu;
147                                         fu = potenial.Evaluate(u);
148                                 }
149                         } else if ((u-ulim)*(ulim-cx) >=0.0) {
150                                 u = ulim;
151                                 fu = potenial.Evaluate(u);
152                         } else {
153                                 u = cx + GOLD*(cx-bx);
154                                 fu = potenial.Evaluate(u);
155                         }
156                         ax = bx;
157                         bx = cx;
158                         cx = u;
159                         fa = fb;
160                         fb = fc;
161                         fc = fu;
162                 }
163         };
164
165         /**
166          * This is a 1 dimensional brent method for
167          * line-minization with derivatives
168          */
169
170
171         static
172                 MT_Scalar
173         DerivativeBrent1d(
174                 MT_Scalar ax,
175                 MT_Scalar bx,
176                 MT_Scalar cx,
177                 DifferentiablePotenialFunction1d &potenial,
178                 MT_Scalar &x_min,
179                 const MT_Scalar tol,
180                 int max_iter = 100
181         ) {
182                 int iter;
183                 bool ok1,ok2;
184                 MT_Scalar a,b,d,d1,d2,du,dv,dw,dx,e(0);
185                 MT_Scalar fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;
186
187                 a = (ax < cx ? ax : cx);
188                 b = (ax > cx ? ax : cx);
189                 x = w = v = bx;
190                 fw = fv = fx = potenial.Evaluate(x);
191                 dw = dv = dx = potenial.Derivative(x);
192
193                 for (iter = 1; iter <= max_iter; iter++) {
194                         xm = 0.5*(a+b);
195                         tol1 = tol*fabs(x) + ZEPS;
196                         tol2 = 2 * tol1;
197                         if (fabs(x - xm) <= (tol2 - 0.5*(b-a))) {
198                                 x_min = x;
199                                 return fx;
200                         }
201
202                         if (fabs(e) > tol1) {
203                                 d1 = 2*(b-a);
204                                 d2 = d1;
205                                 if (dw != dx) {
206                                         d1 = (w-x)*dx/(dx-dw);
207                                 }
208                                 if (dv != dx) {
209                                         d2 = (v-x)*dx/(dx-dv);
210                                 }
211                                         
212                                 u1 = x+d1;
213                                 u2 = x+d2;
214                                 ok1 = ((a-u1)*(u1-b) > 0.0) && ((dx*d1) <= 0.0);
215                                 ok2 = ((a-u2)*(u2-b) > 0.0) && ((dx*d2) <= 0.0);
216                                 olde = e;
217                                 e = d;
218
219                                 if (ok1 || ok2) {
220                                         if (ok1 && ok2) {
221                                                 d = fabs(d1) < fabs(d2) ? d1 : d2;
222                                         } else if (ok1)  {
223                                                 d = d1;
224                                         } else {
225                                                 d = d2;
226                                         }
227                                         if (fabs(d) <= fabs(0.5*olde)) {                        
228                                                 u = x+d;
229                                                 if ((u-a < tol2) || (b-u < tol2)) {
230                                                         d = TNT::sign(tol1,xm-x);
231                                                 }
232                                         } else {
233                                                 d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
234                                         }
235                                 } else {
236                                         d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
237                                 }
238                         } else {
239                                 d = 0.5*(e = (dx >= 0.0 ? a-x : b-x));
240                         }
241
242                         if (fabs(d) >= tol1) {
243                                 u = x+d;
244                                 fu = potenial.Evaluate(u);
245                         } else {
246                                 u  = x + TNT::sign(tol1,d);
247                                 fu = potenial.Evaluate(u);
248                                 if (fu > fx) {
249                                         x_min = x;
250                                         return fx;
251                                 }
252                         }
253                         du = potenial.Derivative(u);
254                         if (fu <= fx) {
255                                 if (u >= x) {
256                                         a = x;
257                                 } else {
258                                         b = x;
259                                 }
260                                 v = w; fv = fw; dv = dw;
261                                 w = x; fw = fx; dw = dx;
262                                 x = u; fx = fu; dx = du;
263                         } else {
264                                 if (u < x) {
265                                         a = u;
266                                 } else {
267                                         b = u;
268                                 }
269                                 if (fu <= fw || w == x) {
270                                         v = w; fv = fw; dv = dw;
271                                         w = u; fw = fu; dw = du;
272                                 } else if ( fu < fv || v == x || v == w) {
273                                         v = u; fv = fu; dv = du;
274                                 }
275                         }
276                 }
277                 // FIXME throw exception                
278         
279                 assert(false);
280                 return MT_Scalar(0);
281         };
282
283 private :
284
285         /// This class just contains static helper methods so no instantiation
286
287         IK_LineMinimizer();             
288
289 };
290
291 #undef GOLD
292 #undef GLIMIT
293 #undef TINY
294 #undef ZEPS
295
296
297                                 
298 #endif