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