00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
00075
00076
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
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
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
