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