$search
00001 00002 /***************************************************************************** 00003 * \file 00004 * class for automatic differentiation on scalar values and 1st 00005 * derivatives and 2nd derivative. 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: rall2d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 00023 * $Name: $ 00024 ****************************************************************************/ 00025 00026 #ifndef Rall2D_H 00027 #define Rall2D_H 00028 00029 #include <math.h> 00030 #include <assert.h> 00031 #include "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 // = Constructors 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 // = Copy Constructor 00072 INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {} 00073 //if one defines this constructor, it's better optimized then the 00074 //automatically generated one ( that one set's up a loop to copy 00075 // word by word. 00076 00077 // = Member functions to access internal structures : 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 // = assignment operators 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 // = Operators between Rall2d objects 00168 /* 00169 friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); 00170 friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); 00171 friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); 00172 friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs); 00173 friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg); 00174 friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v); 00175 friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s); 00176 friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v); 00177 friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s); 00178 friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v); 00179 friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s); 00180 friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v); 00181 friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s); 00182 00183 // = Mathematical functions that operate on Rall2d objects 00184 00185 friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg); 00186 friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg); 00187 friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg); 00188 friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg); 00189 friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg); 00190 friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg); 00191 friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg); 00192 friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg); 00193 friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg); 00194 friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ; 00195 friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg); 00196 friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg); 00197 friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg); 00198 friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x); 00199 friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x); 00200 friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x); 00201 friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x); 00202 // returns sqrt(y*y+x*x), but is optimized for accuracy and speed. 00203 friend INLINE S Norm(const Rall2d<T,V,S>& value) ; 00204 // returns Norm( value.Value() ). 00205 00206 // = Some utility functions to improve performance 00207 // (should also be declared on primitive types to improve uniformity 00208 friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a, 00209 TI<T>::Arg beta,const Rall2d<T,V,S>& b ); 00210 friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a, 00211 TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ); 00212 // = Setting value of a Rall2d object to 0 or 1 00213 friend INLINE void SetToZero(Rall2d<T,V,S>& value); 00214 friend INLINE void SetToOne(Rall2d<T,V,S>& value); 00215 // = Equality in an eps-interval 00216 friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps); 00217 */ 00218 }; 00219 00220 00221 00222 00223 00224 // = Operators between Rall2d objects 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 /* By inversion of sqr(x) :*/ 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 /* By inversion of sin(x) */ 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 /* By inversion of cos(x) */ 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 /* By inversion of tan(x) */ 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 // returns sqrt(y*y+x*x), but is optimized for accuracy and speed. 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 // returns Norm( value.Value() ). 00488 00489 00490 // (should also be declared on primitive types to improve uniformity 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