$search
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 "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