UI: remove button to enable auto-save after load factory preferences
[blender.git] / intern / itasc / kdl / utilities / rall1d.h
1  
2 /*****************************************************************************
3  * \file  
4  *      class for automatic differentiation on scalar values and 1st 
5  *      derivatives .
6  *       
7  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
8  *
9  * \version 
10  *      ORO_Geometry V0.2
11  *
12  * \par Note
13  *      VC6++ contains a bug, concerning the use of inlined friend functions 
14  *      in combination with namespaces.  So, try to avoid inlined friend 
15  *      functions !  
16  *
17  * \par History
18  *      - $log$ 
19  *
20  * \par Release
21  *      $Name:  $ 
22  ****************************************************************************/
23  
24 #ifndef Rall1D_H
25 #define Rall1D_H
26 #include <assert.h>
27 #include "utility.h"
28
29 namespace KDL {
30 /**
31  * Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair.
32  *  This template class has 3 template parameters :
33  *  -   T contains the type of the value.
34  *  -   V contains the type of the gradient (can be a vector-like type).  
35  *  -   S defines a scalar type that can operate on Rall1d.  This is the type that 
36  *      is used to give back values of Norm() etc. 
37  *
38  * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. 
39  * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
40  *
41  * S is always passed by value. 
42  *
43  * \par Class Type
44  * Concrete implementation
45  */
46 template <typename T,typename V=T,typename S=T>
47 class Rall1d
48     {
49     public:
50         typedef T valuetype;
51         typedef V gradienttype;
52         typedef S scalartype;
53     public :
54         T t;        //!< value
55         V grad;     //!< gradient
56     public :
57         INLINE Rall1d() {}
58
59         T value() const {
60             return t;
61         }
62         V deriv() const {
63             return grad;
64         }
65
66         explicit INLINE  Rall1d(typename TI<T>::Arg c)
67             {t=T(c);SetToZero(grad);}
68
69         INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
70
71         INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
72         //if one defines this constructor, it's better optimized then the
73         //automatically generated one ( this one set's up a loop to copy
74         // word by word.
75         
76         INLINE T& Value() {
77             return t;
78         }
79
80         INLINE V& Gradient() {
81             return grad;
82         }
83
84         INLINE static Rall1d<T,V,S> Zero() {
85             Rall1d<T,V,S> tmp;
86             SetToZero(tmp);
87             return tmp;
88         }
89         INLINE static Rall1d<T,V,S> Identity() {
90             Rall1d<T,V,S> tmp;
91             SetToIdentity(tmp);
92             return tmp;
93         }
94
95         INLINE Rall1d<T,V,S>& operator =(S c)
96             {t=c;SetToZero(grad);return *this;}
97
98         INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
99             {t=r.t;grad=r.grad;return *this;}
100
101         INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
102             {
103             grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
104             t     /= rhs.t;
105             return *this;
106             }
107
108         INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
109             {
110             LinCombR(rhs.t,grad,t,rhs.grad,grad);
111             t *= rhs.t;
112             return *this;
113             }
114
115         INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
116             {
117             grad +=rhs.grad;
118             t    +=rhs.t;
119             return *this;
120             }
121
122         INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
123             {
124             grad -= rhs.grad;
125             t     -= rhs.t;
126             return *this;
127             }
128
129         INLINE Rall1d<T,V,S>& operator /=(S rhs)
130             {
131             grad /= rhs;
132             t    /= rhs;
133             return *this;
134             }
135
136         INLINE Rall1d<T,V,S>& operator *=(S rhs)
137             {
138             grad *= rhs;
139             t    *= rhs;
140             return *this;
141             }
142
143         INLINE Rall1d<T,V,S>& operator +=(S rhs)
144             {
145             t    += rhs;
146             return *this;
147             }
148
149         INLINE Rall1d<T,V,S>& operator -=(S rhs)
150             {
151             t    -= rhs;
152             return *this;
153             }
154
155
156
157         // = operators
158         /* gives warnings on cygwin 
159         
160          template <class T2,class V2,class S2>
161          friend INLINE  Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
162          
163          friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
164          friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
165          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
166          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
167          friend INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
168          friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
169          friend INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
170          friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
171          friend INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
172          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
173          friend INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
174          friend INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
175
176         // = Mathematical functions that operate on Rall1d objects
177          friend INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
178          friend INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
179          friend INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
180          friend INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
181          friend INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
182          friend INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
183          friend INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
184          friend INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
185          friend INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
186          friend INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
187          friend INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
188          friend INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
189          friend INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
190          friend INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
191          friend INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
192          friend INLINE  S Norm(const Rall1d<T,V,S>& value) ;
193          friend INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
194          friend INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
195          
196         // = Utility functions to improve performance
197
198          friend INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
199             const T& beta,const Rall1d<T,V,S>& b );
200         
201          friend INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
202             const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
203         
204         // = Setting value of a Rall1d object to 0 or 1
205
206          friend INLINE  void SetToZero(Rall1d<T,V,S>& value);
207          friend INLINE  void SetToOne(Rall1d<T,V,S>& value);
208         // = Equality in an eps-interval
209          friend INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
210          */
211     };
212
213
214 template <class T,class V,class S>
215 INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
216     {
217     return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
218     }
219
220 template <class T,class V,class S>
221 INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
222     {
223     return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
224     }
225
226 template <class T,class V,class S>
227 INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
228     {
229     return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
230     }
231
232
233 template <class T,class V,class S>
234 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
235     {
236     return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
237     }
238
239 template <class T,class V,class S>
240 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
241     {
242     return Rall1d<T,V,S>(-arg.t,-arg.grad);
243     }
244
245 template <class T,class V,class S>
246 INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
247     {
248     return Rall1d<T,V,S>(s*v.t,s*v.grad);
249     }
250
251 template <class T,class V,class S>
252 INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
253     {
254     return Rall1d<T,V,S>(v.t*s,v.grad*s);
255     }
256
257 template <class T,class V,class S>
258 INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
259     {
260     return Rall1d<T,V,S>(s+v.t,v.grad);
261     }
262
263 template <class T,class V,class S>
264 INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
265     {
266     return Rall1d<T,V,S>(v.t+s,v.grad);
267     }
268
269 template <class T,class V,class S>
270 INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
271     {
272     return Rall1d<T,V,S>(s-v.t,-v.grad);
273     }
274
275 template <class T,class V,class S>
276 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
277     {
278     return Rall1d<T,V,S>(v.t-s,v.grad);
279     }
280
281 template <class T,class V,class S>
282 INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
283     {
284     return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
285     }
286
287 template <class T,class V,class S>
288 INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
289     {
290     return Rall1d<T,V,S>(v.t/s,v.grad/s);
291     }
292
293
294 template <class T,class V,class S>
295 INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
296     {
297     T v;
298     v= (exp(arg.t));
299     return Rall1d<T,V,S>(v,v*arg.grad);
300     }
301
302 template <class T,class V,class S>
303 INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
304     {
305     T v;
306     v=(log(arg.t));
307     return Rall1d<T,V,S>(v,arg.grad/arg.t);
308     }
309
310 template <class T,class V,class S>
311 INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
312     {
313     T v;
314     v=(sin(arg.t));
315     return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
316     }
317
318 template <class T,class V,class S>
319 INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
320     {
321     T v;
322     v=(cos(arg.t));
323     return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
324     }
325
326 template <class T,class V,class S>
327 INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
328     {
329     T v;
330     v=(tan(arg.t));
331     return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
332     }
333
334 template <class T,class V,class S>
335 INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
336     {
337     T v;
338     v=(sinh(arg.t));
339     return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
340     }
341
342 template <class T,class V,class S>
343 INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
344     {
345     T v;
346     v=(cosh(arg.t));
347     return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
348     }
349
350 template <class T,class V,class S>
351 INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
352     {
353     T v;
354     v=(arg.t*arg.t);
355     return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
356     }
357
358 template <class T,class V,class S>
359 INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) 
360     {
361     T v;
362     v=(pow(arg.t,m));
363     return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
364     }
365
366 template <class T,class V,class S>
367 INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
368     {
369     T v;
370     v=sqrt(arg.t);
371     return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
372     }   
373
374 template <class T,class V,class S>
375 INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
376 {
377     T v;
378     v=(atan(x.t));
379     return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
380 }
381
382 template <class T,class V,class S>
383 INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
384 {
385     T v;
386     v=(hypot(y.t,x.t));
387     return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
388 }
389
390 template <class T,class V,class S>
391 INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
392 {
393     T v;
394     v=(asin(x.t));
395     return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
396 }
397
398 template <class T,class V,class S>
399 INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
400 {
401     T v;
402     v=(acos(x.t));
403     return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
404 }
405
406 template <class T,class V,class S>
407 INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
408 {
409     T v;
410     v=(Sign(x));
411     return Rall1d<T,V,S>(v*x,v*x.grad);
412 }
413
414
415 template <class T,class V,class S>
416 INLINE  S Norm(const Rall1d<T,V,S>& value) 
417 {
418     return Norm(value.t);
419 }
420
421 template <class T,class V,class S>
422 INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
423 {       
424     T v(tanh(arg.t));       
425     return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
426 }
427
428 template <class T,class V,class S>
429 INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
430 {
431     T v(x.t*x.t+y.t*y.t);
432     return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
433 }
434
435
436 template <class T,class V,class S>
437 INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
438     const T& beta,const Rall1d<T,V,S>& b ) {
439         return Rall1d<T,V,S>(
440             LinComb(alfa,a.t,beta,b.t),
441             LinComb(alfa,a.grad,beta,b.grad)
442         );
443 }
444
445 template <class T,class V,class S>
446 INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
447     const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
448             LinCombR(alfa, a.t,       beta, b.t,      result.t);
449             LinCombR(alfa, a.grad,    beta, b.grad,   result.grad);
450 }
451
452
453 template <class T,class V,class S>
454 INLINE  void SetToZero(Rall1d<T,V,S>& value)
455     {
456     SetToZero(value.grad);
457     SetToZero(value.t);
458     }
459 template <class T,class V,class S>
460 INLINE  void SetToIdentity(Rall1d<T,V,S>& value)
461     {
462     SetToIdentity(value.t);
463     SetToZero(value.grad);
464     }
465
466 template <class T,class V,class S>
467 INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
468 {
469     return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
470 }
471
472 }
473
474
475
476 #endif