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