Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / utilities / rall2d.h
1
2 /*****************************************************************************
3  * \file  
4  *      class for automatic differentiation on scalar values and 1st 
5  *      derivatives and 2nd derivative.
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: rall2d.h 19905 2009-04-23 13:29:54Z ben2610 $
23  *      $Name:  $ 
24  ****************************************************************************/
25
26 #ifndef Rall2D_H
27 #define Rall2D_H
28
29 #include <math.h>
30 #include <assert.h>
31 #include "utility.h"
32
33
34 namespace KDL {
35
36 /**
37  * Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic 
38  * structure on this pair.
39  *  This template class has 3 template parameters :
40  *  -   T contains the type of the value.
41  *  -   V contains the type of the gradient (can be a vector-like type).  
42  *  -   S defines a scalar type that can operate on Rall1d.  This is the type that 
43  *      is used to give back values of Norm() etc. 
44  *
45  * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. 
46  * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
47  *
48  * S is always passed by value. 
49  *
50  * \par Class Type
51  * Concrete implementation
52  */
53 template <class T,class V=T,class S=T>
54 class Rall2d                      
55     {
56     public :
57         T t;    //!< value
58         V d;    //!< 1st derivative
59         V dd;   //!< 2nd derivative
60     public :
61         // = Constructors
62         INLINE Rall2d() {}
63
64         explicit INLINE  Rall2d(typename TI<T>::Arg c)
65             {t=c;SetToZero(d);SetToZero(dd);}
66
67         INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
68
69         INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
70
71         // = Copy Constructor
72         INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
73             //if one defines this constructor, it's better optimized then the
74             //automatically generated one ( that one set's up a loop to copy
75             // word by word.
76         
77         // = Member functions to access internal structures :
78         INLINE T& Value() {
79             return t;
80         }
81
82         INLINE V& D() {
83             return d;
84         }
85
86         INLINE V& DD() {
87             return dd;
88         }
89         INLINE static Rall2d<T,V,S> Zero() {
90             Rall2d<T,V,S> tmp;
91             SetToZero(tmp);
92             return tmp;
93         }
94         INLINE static Rall2d<T,V,S> Identity() {
95             Rall2d<T,V,S> tmp;
96             SetToIdentity(tmp);
97             return tmp;
98         }
99
100         // = assignment operators
101         INLINE Rall2d<T,V,S>& operator =(S c)
102             {t=c;SetToZero(d);SetToZero(dd);return *this;}
103
104         INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
105             {t=r.t;d=r.d;dd=r.dd;return *this;}
106
107         INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
108             {
109             t /= rhs.t;
110             d = (d-t*rhs.d)/rhs.t;
111             dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
112             return *this;
113             }
114
115         INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
116             {
117             t *= rhs.t;
118             d  = (d*rhs.t+t*rhs.d);
119             dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
120             return *this;
121             }
122
123         INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
124             {
125             t    +=rhs.t;
126             d +=rhs.d;
127             dd+=rhs.dd;
128             return *this;
129             }
130
131         INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
132             {
133             t     -= rhs.t;
134             d     -= rhs.d;
135             dd    -= rhs.dd;
136             return *this;
137             }
138
139         INLINE Rall2d<T,V,S>& operator /=(S rhs)
140             {
141             t    /= rhs;
142             d    /= rhs;
143             dd   /= rhs;
144             return *this;
145             }
146
147         INLINE Rall2d<T,V,S>& operator *=(S rhs)
148             {
149             t    *= rhs;
150             d    *= rhs;
151             dd   *= rhs;
152             return *this;
153             }
154
155         INLINE Rall2d<T,V,S>& operator -=(S rhs)
156             {
157             t    -= rhs;
158             return *this;
159             }
160
161         INLINE Rall2d<T,V,S>& operator +=(S rhs)
162             {
163             t    += rhs;
164             return *this;
165             }
166
167         // = Operators between Rall2d objects
168 /*
169          friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
170          friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
171          friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
172          friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
173          friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
174          friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
175          friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
176          friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
177          friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
178          friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
179          friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
180          friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
181          friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
182
183         // = Mathematical functions that operate on Rall2d objects
184
185          friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
186          friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
187          friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
188          friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
189          friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
190          friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
191          friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
192          friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
193          friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
194          friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
195          friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
196          friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
197          friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
198          friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
199          friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
200          friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
201          friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
202         // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
203          friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
204         // returns Norm( value.Value() ).
205
206         // = Some utility functions to improve performance
207         // (should also be declared on primitive types to improve uniformity
208          friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
209             TI<T>::Arg beta,const Rall2d<T,V,S>& b );
210          friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
211             TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
212         // = Setting value of a Rall2d object to 0 or 1
213          friend INLINE void SetToZero(Rall2d<T,V,S>& value);
214          friend INLINE void SetToOne(Rall2d<T,V,S>& value);
215         // = Equality in an eps-interval
216          friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
217          */
218     };
219
220
221
222
223
224 // = Operators between Rall2d objects
225 template <class T,class V,class S>
226 INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
227     {
228     Rall2d<T,V,S> tmp;
229     tmp.t = lhs.t/rhs.t;
230     tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
231     tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
232     return tmp;
233     }
234
235 template <class T,class V,class S>
236 INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
237     {
238     Rall2d<T,V,S> tmp;
239     tmp.t  = lhs.t*rhs.t;
240     tmp.d  = (lhs.d*rhs.t+lhs.t*rhs.d);
241     tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
242     return tmp;
243     }
244
245 template <class T,class V,class S>
246 INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
247     {
248     return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
249     }
250
251 template <class T,class V,class S>
252 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
253     {
254     return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
255     }
256
257 template <class T,class V,class S>
258 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
259     {
260     return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
261     }
262
263 template <class T,class V,class S>
264 INLINE  Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
265     {
266     return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
267     }
268
269 template <class T,class V,class S>
270 INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
271     {
272     return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
273     }
274
275 template <class T,class V,class S>
276 INLINE  Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
277     {
278     return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
279     }
280
281 template <class T,class V,class S>
282 INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
283     {
284     return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
285     }
286
287 template <class T,class V,class S>
288 INLINE  Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
289     {
290     return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
291     }
292
293 template <class T,class V,class S>
294 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
295     {
296     return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
297     }
298
299 template <class T,class V,class S>
300 INLINE  Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
301     {
302     Rall2d<T,V,S> tmp;
303     tmp.t = s/rhs.t;
304     tmp.d = (-tmp.t*rhs.d)/rhs.t;
305     tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
306     return tmp;
307 }
308
309
310 template <class T,class V,class S>
311 INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
312     {
313     return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
314     }
315
316
317 template <class T,class V,class S>
318 INLINE  Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
319     {
320     Rall2d<T,V,S> tmp;
321     tmp.t  = exp(arg.t);
322     tmp.d  = tmp.t*arg.d;
323     tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
324     return tmp;
325     }
326
327 template <class T,class V,class S>
328 INLINE  Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
329     {
330     Rall2d<T,V,S> tmp;
331     tmp.t  = log(arg.t);
332     tmp.d  = arg.d/arg.t;
333     tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
334     return tmp;
335     }
336
337 template <class T,class V,class S>
338 INLINE  Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
339     {
340     T v1 = sin(arg.t);
341     T v2 = cos(arg.t);
342     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
343     }
344
345 template <class T,class V,class S>
346 INLINE  Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
347     {
348     T v1 = cos(arg.t);
349     T v2 = -sin(arg.t);
350     return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
351     }
352
353 template <class T,class V,class S>
354 INLINE  Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
355     {
356     T v1 = tan(arg.t);
357     T v2 = S(1)+sqr(v1);
358     return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
359     }
360
361 template <class T,class V,class S>
362 INLINE  Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
363     {
364     T v1 = sinh(arg.t);
365     T v2 = cosh(arg.t);
366     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
367     }
368
369 template <class T,class V,class S>
370 INLINE  Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
371     {
372     T v1 = cosh(arg.t);
373     T v2 = sinh(arg.t);
374     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
375     }
376
377 template <class T,class V,class S>
378 INLINE  Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
379     {       
380     T v1 = tanh(arg.t);
381     T v2 = S(1)-sqr(v1);
382     return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
383     }
384
385 template <class T,class V,class S>
386 INLINE  Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
387     {
388     return Rall2d<T,V,S>(arg.t*arg.t,
389                         (S(2)*arg.t)*arg.d,
390                         S(2)*(sqr(arg.d)+arg.t*arg.dd)
391                         );
392     }
393
394 template <class T,class V,class S>
395 INLINE  Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) 
396     {
397     Rall2d<T,V,S> tmp;
398     tmp.t = pow(arg.t,m);
399     T v2  = (m/arg.t)*tmp.t;
400     tmp.d  = v2*arg.d;
401     tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
402     return tmp;
403     }
404
405 template <class T,class V,class S>
406 INLINE  Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
407     {
408     /* By inversion of sqr(x) :*/
409     Rall2d<T,V,S> tmp;
410     tmp.t  = sqrt(arg.t);
411     tmp.d  = (S(0.5)/tmp.t)*arg.d;
412     tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
413     return tmp;
414     }
415
416 template <class T,class V,class S>
417 INLINE  Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
418 {
419     /* By inversion of sin(x) */
420     Rall2d<T,V,S> tmp;
421     tmp.t  = asin(arg.t);
422     T v = cos(tmp.t);
423     tmp.d  = arg.d/v;
424     tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
425     return tmp;
426 }
427
428 template <class T,class V,class S>
429 INLINE  Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
430 {
431     /* By inversion of cos(x) */
432     Rall2d<T,V,S> tmp;
433     tmp.t  = acos(arg.t);
434     T v = -sin(tmp.t);
435     tmp.d  = arg.d/v;
436     tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
437     return tmp;
438
439 }
440
441 template <class T,class V,class S>
442 INLINE  Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
443 {
444     /* By inversion of tan(x) */
445     Rall2d<T,V,S> tmp;
446     tmp.t  = atan(x.t);
447     T v    = S(1)+sqr(x.t);
448     tmp.d  = x.d/v;
449     tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
450     return tmp;
451 }
452
453 template <class T,class V,class S>
454 INLINE  Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
455 {
456     Rall2d<T,V,S> tmp;
457     tmp.t  = atan2(y.t,x.t);
458     T v    = sqr(y.t)+sqr(x.t);
459     tmp.d  = (x.t*y.d-x.d*y.t)/v;
460     tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
461     return tmp;
462 }
463
464 template <class T,class V,class S>
465 INLINE  Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
466 {
467     T v(Sign(x));
468     return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
469 }
470
471 template <class T,class V,class S>
472 INLINE  Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
473 {
474     Rall2d<T,V,S> tmp;
475     tmp.t  = hypot(y.t,x.t);
476     tmp.d  = (x.t*x.d+y.t*y.d)/tmp.t;
477     tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
478     return tmp;
479 }
480 // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
481
482 template <class T,class V,class S>
483 INLINE  S Norm(const Rall2d<T,V,S>& value) 
484 {
485     return Norm(value.t);
486 }
487 // returns Norm( value.Value() ).
488
489
490 // (should also be declared on primitive types to improve uniformity
491 template <class T,class V,class S>
492 INLINE  Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
493     const T& beta,const Rall2d<T,V,S>& b ) {
494         return Rall2d<T,V,S>(
495             LinComb(alfa,a.t,beta,b.t),
496             LinComb(alfa,a.d,beta,b.d),
497             LinComb(alfa,a.dd,beta,b.dd) 
498         );
499 }
500
501 template <class T,class V,class S>
502 INLINE  void LinCombR(S alfa,const Rall2d<T,V,S>& a,
503     const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
504             LinCombR(alfa, a.t,       beta, b.t,      result.t);
505             LinCombR(alfa, a.d,    beta, b.d,   result.d);
506             LinCombR(alfa, a.dd,    beta, b.dd,   result.dd);
507 }
508
509 template <class T,class V,class S>
510 INLINE  void SetToZero(Rall2d<T,V,S>& value)
511     {
512     SetToZero(value.t);
513     SetToZero(value.d);
514     SetToZero(value.dd);
515     }
516
517 template <class T,class V,class S>
518 INLINE  void SetToIdentity(Rall2d<T,V,S>& value)
519     {
520     SetToZero(value.d);
521     SetToIdentity(value.t);
522     SetToZero(value.dd);
523     }
524
525 template <class T,class V,class S>
526 INLINE  bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
527 {
528     return (Equal(x.t,y.t,eps)&&
529             Equal(x.d,y.d,eps)&&
530             Equal(x.dd,y.dd,eps)
531             );
532 }
533
534
535 }
536
537
538 #endif