rall2d.h
Go to the documentation of this file.
00001 
00002 /*****************************************************************************
00003  * \file  
00004  *      class for automatic differentiation on scalar values and 1st 
00005  *      derivatives and 2nd derivative.
00006  *       
00007  *  \author 
00008  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00009  *
00010  *  \version 
00011  *      ORO_Geometry V0.2
00012  *
00013  *  \par Note
00014  *      VC6++ contains a bug, concerning the use of inlined friend functions 
00015  *      in combination with namespaces.  So, try to avoid inlined friend 
00016  *      functions !  
00017  *
00018  *  \par History
00019  *      - $log$ 
00020  *
00021  *  \par Release
00022  *      $Id: rall2d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
00023  *      $Name:  $ 
00024  ****************************************************************************/
00025 
00026 #ifndef Rall2D_H
00027 #define Rall2D_H
00028 
00029 #include <math.h>
00030 #include <assert.h>
00031 #include "utility.h"
00032 
00033 
00034 namespace KDL {
00035 
00053 template <class T,class V=T,class S=T>
00054 class Rall2d                      
00055     {
00056     public :
00057         T t;    
00058         V d;    
00059         V dd;   
00060     public :
00061         // = Constructors
00062         INLINE Rall2d() {}
00063 
00064         explicit INLINE  Rall2d(typename TI<T>::Arg c)
00065             {t=c;SetToZero(d);SetToZero(dd);}
00066 
00067         INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
00068 
00069         INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
00070 
00071         // = Copy Constructor
00072         INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
00073             //if one defines this constructor, it's better optimized then the
00074             //automatically generated one ( that one set's up a loop to copy
00075             // word by word.
00076         
00077         // = Member functions to access internal structures :
00078         INLINE T& Value() {
00079             return t;
00080         }
00081 
00082         INLINE V& D() {
00083             return d;
00084         }
00085 
00086         INLINE V& DD() {
00087             return dd;
00088         }
00089         INLINE static Rall2d<T,V,S> Zero() {
00090             Rall2d<T,V,S> tmp;
00091             SetToZero(tmp);
00092             return tmp;
00093         }
00094         INLINE static Rall2d<T,V,S> Identity() {
00095             Rall2d<T,V,S> tmp;
00096             SetToIdentity(tmp);
00097             return tmp;
00098         }
00099 
00100         // = assignment operators
00101         INLINE Rall2d<T,V,S>& operator =(S c)
00102             {t=c;SetToZero(d);SetToZero(dd);return *this;}
00103 
00104         INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
00105             {t=r.t;d=r.d;dd=r.dd;return *this;}
00106 
00107         INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
00108             {
00109             t /= rhs.t;
00110             d = (d-t*rhs.d)/rhs.t;
00111             dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
00112             return *this;
00113             }
00114 
00115         INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
00116             {
00117             t *= rhs.t;
00118             d  = (d*rhs.t+t*rhs.d);
00119             dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
00120             return *this;
00121             }
00122 
00123         INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
00124             {
00125             t    +=rhs.t;
00126             d +=rhs.d;
00127             dd+=rhs.dd;
00128             return *this;
00129             }
00130 
00131         INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
00132             {
00133             t     -= rhs.t;
00134             d     -= rhs.d;
00135             dd    -= rhs.dd;
00136             return *this;
00137             }
00138 
00139         INLINE Rall2d<T,V,S>& operator /=(S rhs)
00140             {
00141             t    /= rhs;
00142             d    /= rhs;
00143             dd   /= rhs;
00144             return *this;
00145             }
00146 
00147         INLINE Rall2d<T,V,S>& operator *=(S rhs)
00148             {
00149             t    *= rhs;
00150             d    *= rhs;
00151             dd   *= rhs;
00152             return *this;
00153             }
00154 
00155         INLINE Rall2d<T,V,S>& operator -=(S rhs)
00156             {
00157             t    -= rhs;
00158             return *this;
00159             }
00160 
00161         INLINE Rall2d<T,V,S>& operator +=(S rhs)
00162             {
00163             t    += rhs;
00164             return *this;
00165             }
00166 
00167         // = Operators between Rall2d objects
00168 /*
00169          friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
00170          friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
00171          friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
00172          friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
00173          friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
00174          friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
00175          friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
00176          friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
00177          friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
00178          friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
00179          friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
00180          friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
00181          friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
00182 
00183         // = Mathematical functions that operate on Rall2d objects
00184 
00185          friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
00186          friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
00187          friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
00188          friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
00189          friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
00190          friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
00191          friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
00192          friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
00193          friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
00194          friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
00195          friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
00196          friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
00197          friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
00198          friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
00199          friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
00200          friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
00201          friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
00202         // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
00203          friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
00204         // returns Norm( value.Value() ).
00205 
00206         // = Some utility functions to improve performance
00207         // (should also be declared on primitive types to improve uniformity
00208          friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
00209             TI<T>::Arg beta,const Rall2d<T,V,S>& b );
00210          friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
00211             TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
00212         // = Setting value of a Rall2d object to 0 or 1
00213          friend INLINE void SetToZero(Rall2d<T,V,S>& value);
00214          friend INLINE void SetToOne(Rall2d<T,V,S>& value);
00215         // = Equality in an eps-interval
00216          friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
00217          */
00218     };
00219 
00220 
00221 
00222 
00223 
00224 // = Operators between Rall2d objects
00225 template <class T,class V,class S>
00226 INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
00227     {
00228     Rall2d<T,V,S> tmp;
00229     tmp.t = lhs.t/rhs.t;
00230     tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
00231     tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
00232     return tmp;
00233     }
00234 
00235 template <class T,class V,class S>
00236 INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
00237     {
00238     Rall2d<T,V,S> tmp;
00239     tmp.t  = lhs.t*rhs.t;
00240     tmp.d  = (lhs.d*rhs.t+lhs.t*rhs.d);
00241     tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
00242     return tmp;
00243     }
00244 
00245 template <class T,class V,class S>
00246 INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
00247     {
00248     return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
00249     }
00250 
00251 template <class T,class V,class S>
00252 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
00253     {
00254     return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
00255     }
00256 
00257 template <class T,class V,class S>
00258 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
00259     {
00260     return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
00261     }
00262 
00263 template <class T,class V,class S>
00264 INLINE  Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
00265     {
00266     return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
00267     }
00268 
00269 template <class T,class V,class S>
00270 INLINE  Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
00271     {
00272     return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
00273     }
00274 
00275 template <class T,class V,class S>
00276 INLINE  Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
00277     {
00278     return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
00279     }
00280 
00281 template <class T,class V,class S>
00282 INLINE  Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
00283     {
00284     return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
00285     }
00286 
00287 template <class T,class V,class S>
00288 INLINE  Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
00289     {
00290     return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
00291     }
00292 
00293 template <class T,class V,class S>
00294 INLINE  Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
00295     {
00296     return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
00297     }
00298 
00299 template <class T,class V,class S>
00300 INLINE  Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
00301     {
00302     Rall2d<T,V,S> tmp;
00303     tmp.t = s/rhs.t;
00304     tmp.d = (-tmp.t*rhs.d)/rhs.t;
00305     tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
00306     return tmp;
00307 }
00308 
00309 
00310 template <class T,class V,class S>
00311 INLINE  Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
00312     {
00313     return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
00314     }
00315 
00316 
00317 template <class T,class V,class S>
00318 INLINE  Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
00319     {
00320     Rall2d<T,V,S> tmp;
00321     tmp.t  = exp(arg.t);
00322     tmp.d  = tmp.t*arg.d;
00323     tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
00324     return tmp;
00325     }
00326 
00327 template <class T,class V,class S>
00328 INLINE  Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
00329     {
00330     Rall2d<T,V,S> tmp;
00331     tmp.t  = log(arg.t);
00332     tmp.d  = arg.d/arg.t;
00333     tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
00334     return tmp;
00335     }
00336 
00337 template <class T,class V,class S>
00338 INLINE  Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
00339     {
00340     T v1 = sin(arg.t);
00341     T v2 = cos(arg.t);
00342     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
00343     }
00344 
00345 template <class T,class V,class S>
00346 INLINE  Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
00347     {
00348     T v1 = cos(arg.t);
00349     T v2 = -sin(arg.t);
00350     return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
00351     }
00352 
00353 template <class T,class V,class S>
00354 INLINE  Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
00355     {
00356     T v1 = tan(arg.t);
00357     T v2 = S(1)+sqr(v1);
00358     return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
00359     }
00360 
00361 template <class T,class V,class S>
00362 INLINE  Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
00363     {
00364     T v1 = sinh(arg.t);
00365     T v2 = cosh(arg.t);
00366     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
00367     }
00368 
00369 template <class T,class V,class S>
00370 INLINE  Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
00371     {
00372     T v1 = cosh(arg.t);
00373     T v2 = sinh(arg.t);
00374     return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
00375     }
00376 
00377 template <class T,class V,class S>
00378 INLINE  Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
00379     {       
00380     T v1 = tanh(arg.t);
00381     T v2 = S(1)-sqr(v1);
00382     return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
00383     }
00384 
00385 template <class T,class V,class S>
00386 INLINE  Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
00387     {
00388     return Rall2d<T,V,S>(arg.t*arg.t,
00389                         (S(2)*arg.t)*arg.d,
00390                         S(2)*(sqr(arg.d)+arg.t*arg.dd)
00391                         );
00392     }
00393 
00394 template <class T,class V,class S>
00395 INLINE  Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) 
00396     {
00397     Rall2d<T,V,S> tmp;
00398     tmp.t = pow(arg.t,m);
00399     T v2  = (m/arg.t)*tmp.t;
00400     tmp.d  = v2*arg.d;
00401     tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
00402     return tmp;
00403     }
00404 
00405 template <class T,class V,class S>
00406 INLINE  Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
00407     {
00408     /* By inversion of sqr(x) :*/
00409     Rall2d<T,V,S> tmp;
00410     tmp.t  = sqrt(arg.t);
00411     tmp.d  = (S(0.5)/tmp.t)*arg.d;
00412     tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
00413     return tmp;
00414     }
00415 
00416 template <class T,class V,class S>
00417 INLINE  Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
00418 {
00419     /* By inversion of sin(x) */
00420     Rall2d<T,V,S> tmp;
00421     tmp.t  = asin(arg.t);
00422     T v = cos(tmp.t);
00423     tmp.d  = arg.d/v;
00424     tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
00425     return tmp;
00426 }
00427 
00428 template <class T,class V,class S>
00429 INLINE  Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
00430 {
00431     /* By inversion of cos(x) */
00432     Rall2d<T,V,S> tmp;
00433     tmp.t  = acos(arg.t);
00434     T v = -sin(tmp.t);
00435     tmp.d  = arg.d/v;
00436     tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
00437     return tmp;
00438 
00439 }
00440 
00441 template <class T,class V,class S>
00442 INLINE  Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
00443 {
00444     /* By inversion of tan(x) */
00445     Rall2d<T,V,S> tmp;
00446     tmp.t  = atan(x.t);
00447     T v    = S(1)+sqr(x.t);
00448     tmp.d  = x.d/v;
00449     tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
00450     return tmp;
00451 }
00452 
00453 template <class T,class V,class S>
00454 INLINE  Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
00455 {
00456     Rall2d<T,V,S> tmp;
00457     tmp.t  = atan2(y.t,x.t);
00458     T v    = sqr(y.t)+sqr(x.t);
00459     tmp.d  = (x.t*y.d-x.d*y.t)/v;
00460     tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
00461     return tmp;
00462 }
00463 
00464 template <class T,class V,class S>
00465 INLINE  Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
00466 {
00467     T v(Sign(x));
00468     return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
00469 }
00470 
00471 template <class T,class V,class S>
00472 INLINE  Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
00473 {
00474     Rall2d<T,V,S> tmp;
00475     tmp.t  = hypot(y.t,x.t);
00476     tmp.d  = (x.t*x.d+y.t*y.d)/tmp.t;
00477     tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
00478     return tmp;
00479 }
00480 // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
00481 
00482 template <class T,class V,class S>
00483 INLINE  S Norm(const Rall2d<T,V,S>& value) 
00484 {
00485     return Norm(value.t);
00486 }
00487 // returns Norm( value.Value() ).
00488 
00489 
00490 // (should also be declared on primitive types to improve uniformity
00491 template <class T,class V,class S>
00492 INLINE  Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
00493     const T& beta,const Rall2d<T,V,S>& b ) {
00494         return Rall2d<T,V,S>(
00495             LinComb(alfa,a.t,beta,b.t),
00496             LinComb(alfa,a.d,beta,b.d),
00497             LinComb(alfa,a.dd,beta,b.dd) 
00498         );
00499 }
00500 
00501 template <class T,class V,class S>
00502 INLINE  void LinCombR(S alfa,const Rall2d<T,V,S>& a,
00503     const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
00504             LinCombR(alfa, a.t,       beta, b.t,      result.t);
00505             LinCombR(alfa, a.d,    beta, b.d,   result.d);
00506             LinCombR(alfa, a.dd,    beta, b.dd,   result.dd);
00507 }
00508 
00509 template <class T,class V,class S>
00510 INLINE  void SetToZero(Rall2d<T,V,S>& value)
00511     {
00512     SetToZero(value.t);
00513     SetToZero(value.d);
00514     SetToZero(value.dd);
00515     }
00516 
00517 template <class T,class V,class S>
00518 INLINE  void SetToIdentity(Rall2d<T,V,S>& value)
00519     {
00520     SetToZero(value.d);
00521     SetToIdentity(value.t);
00522     SetToZero(value.dd);
00523     }
00524 
00525 template <class T,class V,class S>
00526 INLINE  bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
00527 {
00528     return (Equal(x.t,y.t,eps)&&
00529             Equal(x.d,y.d,eps)&&
00530             Equal(x.dd,y.dd,eps)
00531             );
00532 }
00533 
00534 
00535 }
00536 
00537 
00538 #endif


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14