rall2d.h
Go to the documentation of this file.
1 
2 /*****************************************************************************
3  * \file
4  * class for automatic differentiation on scalar values and 1st
5  * derivatives and 2nd derivative.
6  *
7  * \author
8  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
9  *
10  * \version
11  * ORO_Geometry V0.2
12  *
13  * \par Note
14  * VC6++ contains a bug, concerning the use of inlined friend functions
15  * in combination with namespaces. So, try to avoid inlined friend
16  * functions !
17  *
18  * \par History
19  * - $log$
20  *
21  * \par Release
22  * $Id: rall2d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
23  * $Name: $
24  ****************************************************************************/
25 
26 #ifndef Rall2D_H
27 #define Rall2D_H
28 
29 #include <math.h>
30 #include <assert.h>
31 #include "utility.h"
32 
33 
34 namespace KDL {
35 
53 template <class T,class V=T,class S=T>
54 class Rall2d
55  {
56  public :
57  T t;
58  V d;
59  V dd;
60  public :
61  // = Constructors
62  INLINE Rall2d():t(),d(),dd() {};
63 
64  explicit INLINE Rall2d(typename TI<T>::Arg c)
65  {t=c;SetToZero(d);SetToZero(dd);}
66 
67  INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
68 
69  INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
70 
71  // = Copy Constructor
72  INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
73  //if one defines this constructor, it's better optimized then the
74  //automatically generated one ( that one set's up a loop to copy
75  // word by word.
76 
77  // = Member functions to access internal structures :
78  INLINE T& Value() {
79  return t;
80  }
81 
82  INLINE V& D() {
83  return d;
84  }
85 
86  INLINE V& DD() {
87  return dd;
88  }
90  Rall2d<T,V,S> tmp;
91  SetToZero(tmp);
92  return tmp;
93  }
95  Rall2d<T,V,S> tmp;
96  SetToIdentity(tmp);
97  return tmp;
98  }
99 
100  // = assignment operators
102  {t=c;SetToZero(d);SetToZero(dd);return *this;}
103 
105  {t=r.t;d=r.d;dd=r.dd;return *this;}
106 
108  {
109  t /= rhs.t;
110  d = (d-t*rhs.d)/rhs.t;
111  dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
112  return *this;
113  }
114 
116  {
117  t *= rhs.t;
118  d = (d*rhs.t+t*rhs.d);
119  dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
120  return *this;
121  }
122 
124  {
125  t +=rhs.t;
126  d +=rhs.d;
127  dd+=rhs.dd;
128  return *this;
129  }
130 
132  {
133  t -= rhs.t;
134  d -= rhs.d;
135  dd -= rhs.dd;
136  return *this;
137  }
138 
140  {
141  t /= rhs;
142  d /= rhs;
143  dd /= rhs;
144  return *this;
145  }
146 
148  {
149  t *= rhs;
150  d *= rhs;
151  dd *= rhs;
152  return *this;
153  }
154 
156  {
157  t -= rhs;
158  return *this;
159  }
160 
162  {
163  t += rhs;
164  return *this;
165  }
166 
167  // = Operators between Rall2d objects
168 /*
169  friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
170  friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
171  friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
172  friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
173  friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
174  friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
175  friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
176  friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
177  friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
178  friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
179  friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
180  friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
181  friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
182 
183  // = Mathematical functions that operate on Rall2d objects
184 
185  friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
186  friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
187  friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
188  friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
189  friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
190  friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
191  friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
192  friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
193  friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
194  friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
195  friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
196  friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
197  friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
198  friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
199  friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
200  friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
201  friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
202  // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
203  friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
204  // returns Norm( value.Value() ).
205 
206  // = Some utility functions to improve performance
207  // (should also be declared on primitive types to improve uniformity
208  friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
209  TI<T>::Arg beta,const Rall2d<T,V,S>& b );
210  friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
211  TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
212  // = Setting value of a Rall2d object to 0 or 1
213  friend INLINE void SetToZero(Rall2d<T,V,S>& value);
214  friend INLINE void SetToOne(Rall2d<T,V,S>& value);
215  // = Equality in an eps-interval
216  friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
217  */
218  };
219 
220 
221 
222 
223 
224 // = Operators between Rall2d objects
225 template <class T,class V,class S>
227  {
228  Rall2d<T,V,S> tmp;
229  tmp.t = lhs.t/rhs.t;
230  tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
231  tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
232  return tmp;
233  }
234 
235 template <class T,class V,class S>
237  {
238  Rall2d<T,V,S> tmp;
239  tmp.t = lhs.t*rhs.t;
240  tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d);
241  tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
242  return tmp;
243  }
244 
245 template <class T,class V,class S>
247  {
248  return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
249  }
250 
251 template <class T,class V,class S>
253  {
254  return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
255  }
256 
257 template <class T,class V,class S>
259  {
260  return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
261  }
262 
263 template <class T,class V,class S>
265  {
266  return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
267  }
268 
269 template <class T,class V,class S>
271  {
272  return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
273  }
274 
275 template <class T,class V,class S>
277  {
278  return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
279  }
280 
281 template <class T,class V,class S>
283  {
284  return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
285  }
286 
287 template <class T,class V,class S>
289  {
290  return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
291  }
292 
293 template <class T,class V,class S>
295  {
296  return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
297  }
298 
299 template <class T,class V,class S>
301  {
302  Rall2d<T,V,S> tmp;
303  tmp.t = s/rhs.t;
304  tmp.d = (-tmp.t*rhs.d)/rhs.t;
305  tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
306  return tmp;
307 }
308 
309 
310 template <class T,class V,class S>
312  {
313  return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
314  }
315 
316 
317 template <class T,class V,class S>
319  {
320  Rall2d<T,V,S> tmp;
321  tmp.t = exp(arg.t);
322  tmp.d = tmp.t*arg.d;
323  tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
324  return tmp;
325  }
326 
327 template <class T,class V,class S>
329  {
330  Rall2d<T,V,S> tmp;
331  tmp.t = log(arg.t);
332  tmp.d = arg.d/arg.t;
333  tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
334  return tmp;
335  }
336 
337 template <class T,class V,class S>
339  {
340  T v1 = sin(arg.t);
341  T v2 = cos(arg.t);
342  return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
343  }
344 
345 template <class T,class V,class S>
347  {
348  T v1 = cos(arg.t);
349  T v2 = -sin(arg.t);
350  return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
351  }
352 
353 template <class T,class V,class S>
355  {
356  T v1 = tan(arg.t);
357  T v2 = S(1)+sqr(v1);
358  return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
359  }
360 
361 template <class T,class V,class S>
363  {
364  T v1 = sinh(arg.t);
365  T v2 = cosh(arg.t);
366  return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
367  }
368 
369 template <class T,class V,class S>
371  {
372  T v1 = cosh(arg.t);
373  T v2 = sinh(arg.t);
374  return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
375  }
376 
377 template <class T,class V,class S>
379  {
380  T v1 = tanh(arg.t);
381  T v2 = S(1)-sqr(v1);
382  return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
383  }
384 
385 template <class T,class V,class S>
387  {
388  return Rall2d<T,V,S>(arg.t*arg.t,
389  (S(2)*arg.t)*arg.d,
390  S(2)*(sqr(arg.d)+arg.t*arg.dd)
391  );
392  }
393 
394 template <class T,class V,class S>
395 INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m)
396  {
397  Rall2d<T,V,S> tmp;
398  tmp.t = pow(arg.t,m);
399  T v2 = (m/arg.t)*tmp.t;
400  tmp.d = v2*arg.d;
401  tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
402  return tmp;
403  }
404 
405 template <class T,class V,class S>
407  {
408  /* By inversion of sqr(x) :*/
409  Rall2d<T,V,S> tmp;
410  tmp.t = sqrt(arg.t);
411  tmp.d = (S(0.5)/tmp.t)*arg.d;
412  tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
413  return tmp;
414  }
415 
416 template <class T,class V,class S>
418 {
419  /* By inversion of sin(x) */
420  Rall2d<T,V,S> tmp;
421  tmp.t = asin(arg.t);
422  T v = cos(tmp.t);
423  tmp.d = arg.d/v;
424  tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
425  return tmp;
426 }
427 
428 template <class T,class V,class S>
430 {
431  /* By inversion of cos(x) */
432  Rall2d<T,V,S> tmp;
433  tmp.t = acos(arg.t);
434  T v = -sin(tmp.t);
435  tmp.d = arg.d/v;
436  tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
437  return tmp;
438 
439 }
440 
441 template <class T,class V,class S>
443 {
444  /* By inversion of tan(x) */
445  Rall2d<T,V,S> tmp;
446  tmp.t = atan(x.t);
447  T v = S(1)+sqr(x.t);
448  tmp.d = x.d/v;
449  tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
450  return tmp;
451 }
452 
453 template <class T,class V,class S>
455 {
456  Rall2d<T,V,S> tmp;
457  tmp.t = atan2(y.t,x.t);
458  T v = sqr(y.t)+sqr(x.t);
459  tmp.d = (x.t*y.d-x.d*y.t)/v;
460  tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
461  return tmp;
462 }
463 
464 template <class T,class V,class S>
466 {
467  T v(Sign(x));
468  return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
469 }
470 
471 template <class T,class V,class S>
473 {
474  Rall2d<T,V,S> tmp;
475  tmp.t = hypot(y.t,x.t);
476  tmp.d = (x.t*x.d+y.t*y.d)/tmp.t;
477  tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
478  return tmp;
479 }
480 // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
481 
482 template <class T,class V,class S>
483 INLINE S Norm(const Rall2d<T,V,S>& value)
484 {
485  return Norm(value.t);
486 }
487 // returns Norm( value.Value() ).
488 
489 
490 // (should also be declared on primitive types to improve uniformity
491 template <class T,class V,class S>
493  const T& beta,const Rall2d<T,V,S>& b ) {
494  return Rall2d<T,V,S>(
495  LinComb(alfa,a.t,beta,b.t),
496  LinComb(alfa,a.d,beta,b.d),
497  LinComb(alfa,a.dd,beta,b.dd)
498  );
499 }
500 
501 template <class T,class V,class S>
502 INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
503  const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
504  LinCombR(alfa, a.t, beta, b.t, result.t);
505  LinCombR(alfa, a.d, beta, b.d, result.d);
506  LinCombR(alfa, a.dd, beta, b.dd, result.dd);
507 }
508 
509 template <class T,class V,class S>
511  {
512  SetToZero(value.t);
513  SetToZero(value.d);
514  SetToZero(value.dd);
515  }
516 
517 template <class T,class V,class S>
519  {
520  SetToZero(value.d);
521  SetToIdentity(value.t);
522  SetToZero(value.dd);
523  }
524 
525 template <class T,class V,class S>
526 INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
527 {
528  return (Equal(x.t,y.t,eps)&&
529  Equal(x.d,y.d,eps)&&
530  Equal(x.dd,y.dd,eps)
531  );
532 }
533 
534 template <class T,class V,class S>
536 {
537 #ifdef KDL_USE_EQUAL
538  return Equal(y, x);
539 #else
540  return (x.t == y.t &&
541  x.d == y.d &&
542  x.dd == y.dd);
543 #endif
544 
545 }
546 
547 template <class T,class V,class S>
549 {
550  return !operator==(y, x);
551 }
552 
553 }
554 
555 
556 #endif
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.hpp:181
INLINE Rall2d< T, V, S > & operator=(S c)
Definition: rall2d.h:101
INLINE Rall2d(typename TI< T >::Arg tn, const V &afg, const V &afg2)
Definition: rall2d.h:69
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
INLINE void LinCombR(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b, Rall1d< T, V, S > &result)
Definition: rall1d.h:448
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:305
static INLINE Rall2d< T, V, S > Identity()
Definition: rall2d.h:94
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:345
INLINE Rall2d< T, V, S > & operator/=(const Rall2d< T, V, S > &rhs)
Definition: rall2d.h:107
#define INLINE
Definition: utility.h:227
INLINE V & D()
Definition: rall2d.h:82
INLINE V & DD()
Definition: rall2d.h:86
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:385
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:353
const T & Arg
Arg is used for passing the element to a function.
Definition: utility.h:146
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:337
static INLINE Rall2d< T, V, S > Zero()
Definition: rall2d.h:89
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition: frames.hpp:1285
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:424
IMETHOD void SetToZero(Vector &v)
Definition: frames.hpp:1069
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
Definition: rall1d.h:377
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Definition: rall1d.h:393
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall2d(const Rall2d< T, V, S > &r)
Definition: rall2d.h:72
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:297
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
Definition: rall1d.h:462
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:361
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
Definition: rall1d.h:401
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:409
INLINE Rall2d< T, V, S > & operator+=(const Rall2d< T, V, S > &rhs)
Definition: rall2d.h:123
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
V d
1st derivative
Definition: rall2d.h:58
INLINE Rall2d< T, V, S > & operator*=(const Rall2d< T, V, S > &rhs)
Definition: rall2d.h:115
INLINE Rall2d(typename TI< T >::Arg tn, const V &afg)
Definition: rall2d.h:67
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:321
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:329
INLINE Rall2d()
Definition: rall2d.h:62
INLINE Rall2d(typename TI< T >::Arg c)
Definition: rall2d.h:64
T t
value
Definition: rall2d.h:57
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
INLINE Rall2d< T, V, S > & operator-=(const Rall2d< T, V, S > &rhs)
Definition: rall2d.h:131
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:430
INLINE Rall1d< T, V, S > LinComb(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b)
Definition: rall1d.h:439
INLINE T & Value()
Definition: rall2d.h:78
V dd
2nd derivative
Definition: rall2d.h:59


orocos_kdl
Author(s):
autogenerated on Sun Nov 22 2020 03:16:43