orocos_kdl: rall1d.h Source File

Go to the documentation of this file.
00001  
00002 /*****************************************************************************
00003  * \file  
00004  *      class for automatic differentiation on scalar values and 1st 
00005  *      derivatives .
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: rall1d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
00023  *      $Name:  $ 
00024  ****************************************************************************/
00025  
00026 #ifndef Rall1D_H
00027 #define Rall1D_H
00028 #include <assert.h>
00029 #include <kdl/utility.h>
00030 
00031 namespace KDL {
00048 template <typename T,typename V=T,typename S=T>
00049 class Rall1d
00050     {
00051     public:
00052         typedef T valuetype;
00053         typedef V gradienttype;
00054         typedef S scalartype;
00055     public :
00056         T t;        
00057         V grad;     
00058     public :
00059         INLINE Rall1d() {}
00060 
00061         T value() const {
00062             return t;
00063         }
00064         V deriv() const {
00065             return grad;
00066         }
00067 
00068         explicit INLINE  Rall1d(typename TI<T>::Arg c)
00069             {t=T(c);SetToZero(grad);}
00070 
00071         INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
00072 
00073         INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
00074         //if one defines this constructor, it's better optimized then the
00075         //automatically generated one ( this one set's up a loop to copy
00076         // word by word.
00077         
00078         INLINE T& Value() {
00079             return t;
00080         }
00081 
00082         INLINE V& Gradient() {
00083             return grad;
00084         }
00085 
00086         INLINE static Rall1d<T,V,S> Zero() {
00087             Rall1d<T,V,S> tmp;
00088             SetToZero(tmp);
00089             return tmp;
00090         }
00091         INLINE static Rall1d<T,V,S> Identity() {
00092             Rall1d<T,V,S> tmp;
00093             SetToIdentity(tmp);
00094             return tmp;
00095         }
00096 
00097         INLINE Rall1d<T,V,S>& operator =(S c)
00098             {t=c;SetToZero(grad);return *this;}
00099 
00100         INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
00101             {t=r.t;grad=r.grad;return *this;}
00102 
00103         INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
00104             {
00105             grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
00106             t     /= rhs.t;
00107             return *this;
00108             }
00109 
00110         INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
00111             {
00112             LinCombR(rhs.t,grad,t,rhs.grad,grad);
00113             t *= rhs.t;
00114             return *this;
00115             }
00116 
00117         INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
00118             {
00119             grad +=rhs.grad;
00120             t    +=rhs.t;
00121             return *this;
00122             }
00123 
00124         INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
00125             {
00126             grad -= rhs.grad;
00127             t     -= rhs.t;
00128             return *this;
00129             }
00130 
00131         INLINE Rall1d<T,V,S>& operator /=(S rhs)
00132             {
00133             grad /= rhs;
00134             t    /= rhs;
00135             return *this;
00136             }
00137 
00138         INLINE Rall1d<T,V,S>& operator *=(S rhs)
00139             {
00140             grad *= rhs;
00141             t    *= rhs;
00142             return *this;
00143             }
00144 
00145         INLINE Rall1d<T,V,S>& operator +=(S rhs)
00146             {
00147             t    += rhs;
00148             return *this;
00149             }
00150 
00151         INLINE Rall1d<T,V,S>& operator -=(S rhs)
00152             {
00153             t    -= rhs;
00154             return *this;
00155             }
00156 
00157 
00158 
00159         // = operators
00160         /* gives warnings on cygwin 
00161         
00162          template <class T2,class V2,class S2>
00163          friend INLINE  Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
00164          
00165          friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
00166          friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
00167          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
00168          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
00169          friend INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
00170          friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
00171          friend INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
00172          friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
00173          friend INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
00174          friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
00175          friend INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
00176          friend INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
00177 
00178         // = Mathematical functions that operate on Rall1d objects
00179          friend INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
00180          friend INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
00181          friend INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
00182          friend INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
00183          friend INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
00184          friend INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
00185          friend INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
00186          friend INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
00187          friend INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
00188          friend INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
00189          friend INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
00190          friend INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
00191          friend INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
00192          friend INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
00193          friend INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
00194          friend INLINE  S Norm(const Rall1d<T,V,S>& value) ;
00195          friend INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
00196          friend INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
00197          
00198         // = Utility functions to improve performance
00199 
00200          friend INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
00201             const T& beta,const Rall1d<T,V,S>& b );
00202         
00203          friend INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
00204             const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
00205         
00206         // = Setting value of a Rall1d object to 0 or 1
00207 
00208          friend INLINE  void SetToZero(Rall1d<T,V,S>& value);
00209          friend INLINE  void SetToOne(Rall1d<T,V,S>& value);
00210         // = Equality in an eps-interval
00211          friend INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
00212          */
00213     };
00214 
00215 
00216 template <class T,class V,class S>
00217 INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
00218     {
00219     return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
00220     }
00221 
00222 template <class T,class V,class S>
00223 INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
00224     {
00225     return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
00226     }
00227 
00228 template <class T,class V,class S>
00229 INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
00230     {
00231     return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
00232     }
00233 
00234 
00235 template <class T,class V,class S>
00236 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
00237     {
00238     return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
00239     }
00240 
00241 template <class T,class V,class S>
00242 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
00243     {
00244     return Rall1d<T,V,S>(-arg.t,-arg.grad);
00245     }
00246 
00247 template <class T,class V,class S>
00248 INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
00249     {
00250     return Rall1d<T,V,S>(s*v.t,s*v.grad);
00251     }
00252 
00253 template <class T,class V,class S>
00254 INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
00255     {
00256     return Rall1d<T,V,S>(v.t*s,v.grad*s);
00257     }
00258 
00259 template <class T,class V,class S>
00260 INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
00261     {
00262     return Rall1d<T,V,S>(s+v.t,v.grad);
00263     }
00264 
00265 template <class T,class V,class S>
00266 INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
00267     {
00268     return Rall1d<T,V,S>(v.t+s,v.grad);
00269     }
00270 
00271 template <class T,class V,class S>
00272 INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
00273     {
00274     return Rall1d<T,V,S>(s-v.t,-v.grad);
00275     }
00276 
00277 template <class T,class V,class S>
00278 INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
00279     {
00280     return Rall1d<T,V,S>(v.t-s,v.grad);
00281     }
00282 
00283 template <class T,class V,class S>
00284 INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
00285     {
00286     return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
00287     }
00288 
00289 template <class T,class V,class S>
00290 INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
00291     {
00292     return Rall1d<T,V,S>(v.t/s,v.grad/s);
00293     }
00294 
00295 
00296 template <class T,class V,class S>
00297 INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
00298     {
00299     T v;
00300     v= (exp(arg.t));
00301     return Rall1d<T,V,S>(v,v*arg.grad);
00302     }
00303 
00304 template <class T,class V,class S>
00305 INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
00306     {
00307     T v;
00308     v=(log(arg.t));
00309     return Rall1d<T,V,S>(v,arg.grad/arg.t);
00310     }
00311 
00312 template <class T,class V,class S>
00313 INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
00314     {
00315     T v;
00316     v=(sin(arg.t));
00317     return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
00318     }
00319 
00320 template <class T,class V,class S>
00321 INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
00322     {
00323     T v;
00324     v=(cos(arg.t));
00325     return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
00326     }
00327 
00328 template <class T,class V,class S>
00329 INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
00330     {
00331     T v;
00332     v=(tan(arg.t));
00333     return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
00334     }
00335 
00336 template <class T,class V,class S>
00337 INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
00338     {
00339     T v;
00340     v=(sinh(arg.t));
00341     return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
00342     }
00343 
00344 template <class T,class V,class S>
00345 INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
00346     {
00347     T v;
00348     v=(cosh(arg.t));
00349     return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
00350     }
00351 
00352 template <class T,class V,class S>
00353 INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
00354     {
00355     T v;
00356     v=(arg.t*arg.t);
00357     return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
00358     }
00359 
00360 template <class T,class V,class S>
00361 INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) 
00362     {
00363     T v;
00364     v=(pow(arg.t,m));
00365     return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
00366     }
00367 
00368 template <class T,class V,class S>
00369 INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
00370     {
00371     T v;
00372     v=sqrt(arg.t);
00373     return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
00374     }   
00375 
00376 template <class T,class V,class S>
00377 INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
00378 {
00379     T v;
00380     v=(atan(x.t));
00381     return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
00382 }
00383 
00384 template <class T,class V,class S>
00385 INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
00386 {
00387     T v;
00388     v=(hypot(y.t,x.t));
00389     return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
00390 }
00391 
00392 template <class T,class V,class S>
00393 INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
00394 {
00395     T v;
00396     v=(asin(x.t));
00397     return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
00398 }
00399 
00400 template <class T,class V,class S>
00401 INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
00402 {
00403     T v;
00404     v=(acos(x.t));
00405     return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
00406 }
00407 
00408 template <class T,class V,class S>
00409 INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
00410 {
00411     T v;
00412     v=(Sign(x));
00413     return Rall1d<T,V,S>(v*x,v*x.grad);
00414 }
00415 
00416 
00417 template <class T,class V,class S>
00418 INLINE  S Norm(const Rall1d<T,V,S>& value) 
00419 {
00420     return Norm(value.t);
00421 }
00422 
00423 template <class T,class V,class S>
00424 INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
00425 {       
00426     T v(tanh(arg.t));       
00427     return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
00428 }
00429 
00430 template <class T,class V,class S>
00431 INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
00432 {
00433     T v(x.t*x.t+y.t*y.t);
00434     return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
00435 }
00436 
00437 
00438 template <class T,class V,class S>
00439 INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
00440     const T& beta,const Rall1d<T,V,S>& b ) {
00441         return Rall1d<T,V,S>(
00442             LinComb(alfa,a.t,beta,b.t),
00443             LinComb(alfa,a.grad,beta,b.grad)
00444         );
00445 }
00446 
00447 template <class T,class V,class S>
00448 INLINE  void LinCombR(S alfa,const Rall1d<T,V,S>& a,
00449     const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
00450             LinCombR(alfa, a.t,       beta, b.t,      result.t);
00451             LinCombR(alfa, a.grad,    beta, b.grad,   result.grad);
00452 }
00453 
00454 
00455 template <class T,class V,class S>
00456 INLINE  void SetToZero(Rall1d<T,V,S>& value)
00457     {
00458     SetToZero(value.grad);
00459     SetToZero(value.t);
00460     }
00461 template <class T,class V,class S>
00462 INLINE  void SetToIdentity(Rall1d<T,V,S>& value)
00463     {
00464     SetToIdentity(value.t);
00465     SetToZero(value.grad);
00466     }
00467 
00468 template <class T,class V,class S>
00469 INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
00470 {
00471     return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
00472 }
00473 
00474 }
00475 
00476 
00477 
00478 #endif