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 Rall2D_H
00027 #define Rall2D_H
00028
00029 #include <math.h>
00030 #include <assert.h>
00031 #include <kdl/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
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
00072 INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
00073
00074
00075
00076
00077
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
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
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
00217
00218 };
00219
00220
00221
00222
00223
00224
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
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
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
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
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
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
00488
00489
00490
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
