rall1d.h
Go to the documentation of this file.
1 
2 /*****************************************************************************
3  * \file
4  * class for automatic differentiation on scalar values and 1st
5  * derivatives .
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: rall1d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
23  * $Name: $
24  ****************************************************************************/
25 
26 #ifndef Rall1D_H
27 #define Rall1D_H
28 #include <assert.h>
29 #include "utility.h"
30 
31 namespace KDL {
48 template <typename T,typename V=T,typename S=T>
49 class Rall1d
50  {
51  public:
52  typedef T valuetype;
53  typedef V gradienttype;
54  typedef S scalartype;
55  public :
56  T t;
57  V grad;
58  public :
59  INLINE Rall1d():t(),grad() {};
60 
61  T value() const {
62  return t;
63  }
64  V deriv() const {
65  return grad;
66  }
67 
68  explicit INLINE Rall1d(typename TI<T>::Arg c)
69  {t=T(c);SetToZero(grad);}
70 
71  INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
72 
73  INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
74  //if one defines this constructor, it's better optimized then the
75  //automatically generated one ( this one set's up a loop to copy
76  // word by word.
77 
78  INLINE T& Value() {
79  return t;
80  }
81 
83  return grad;
84  }
85 
87  Rall1d<T,V,S> tmp;
88  SetToZero(tmp);
89  return tmp;
90  }
92  Rall1d<T,V,S> tmp;
93  SetToIdentity(tmp);
94  return tmp;
95  }
96 
98  {t=c;SetToZero(grad);return *this;}
99 
101  {t=r.t;grad=r.grad;return *this;}
102 
104  {
105  grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
106  t /= rhs.t;
107  return *this;
108  }
109 
111  {
112  LinCombR(rhs.t,grad,t,rhs.grad,grad);
113  t *= rhs.t;
114  return *this;
115  }
116 
118  {
119  grad +=rhs.grad;
120  t +=rhs.t;
121  return *this;
122  }
123 
125  {
126  grad -= rhs.grad;
127  t -= rhs.t;
128  return *this;
129  }
130 
132  {
133  grad /= rhs;
134  t /= rhs;
135  return *this;
136  }
137 
139  {
140  grad *= rhs;
141  t *= rhs;
142  return *this;
143  }
144 
146  {
147  t += rhs;
148  return *this;
149  }
150 
152  {
153  t -= rhs;
154  return *this;
155  }
156 
157 
158 
159  // = operators
160  /* gives warnings on cygwin
161 
162  template <class T2,class V2,class S2>
163  friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
164 
165  friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
166  friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
167  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
168  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
169  friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
170  friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
171  friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
172  friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
173  friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
174  friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
175  friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
176  friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
177 
178  // = Mathematical functions that operate on Rall1d objects
179  friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
180  friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
181  friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
182  friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
183  friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
184  friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
185  friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
186  friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
187  friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
188  friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
189  friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
190  friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
191  friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
192  friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
193  friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
194  friend INLINE S Norm(const Rall1d<T,V,S>& value) ;
195  friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
196  friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
197 
198  // = Utility functions to improve performance
199 
200  friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
201  const T& beta,const Rall1d<T,V,S>& b );
202 
203  friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
204  const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
205 
206  // = Setting value of a Rall1d object to 0 or 1
207 
208  friend INLINE void SetToZero(Rall1d<T,V,S>& value);
209  friend INLINE void SetToOne(Rall1d<T,V,S>& value);
210  // = Equality in an eps-interval
211  friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
212  */
213  };
214 
215 
216 template <class T,class V,class S>
218  {
219  return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
220  }
221 
222 template <class T,class V,class S>
224  {
225  return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
226  }
227 
228 template <class T,class V,class S>
230  {
231  return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
232  }
233 
234 
235 template <class T,class V,class S>
237  {
238  return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
239  }
240 
241 template <class T,class V,class S>
243  {
244  return Rall1d<T,V,S>(-arg.t,-arg.grad);
245  }
246 
247 template <class T,class V,class S>
249  {
250  return Rall1d<T,V,S>(s*v.t,s*v.grad);
251  }
252 
253 template <class T,class V,class S>
255  {
256  return Rall1d<T,V,S>(v.t*s,v.grad*s);
257  }
258 
259 template <class T,class V,class S>
261  {
262  return Rall1d<T,V,S>(s+v.t,v.grad);
263  }
264 
265 template <class T,class V,class S>
267  {
268  return Rall1d<T,V,S>(v.t+s,v.grad);
269  }
270 
271 template <class T,class V,class S>
273  {
274  return Rall1d<T,V,S>(s-v.t,-v.grad);
275  }
276 
277 template <class T,class V,class S>
279  {
280  return Rall1d<T,V,S>(v.t-s,v.grad);
281  }
282 
283 template <class T,class V,class S>
285  {
286  return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
287  }
288 
289 template <class T,class V,class S>
291  {
292  return Rall1d<T,V,S>(v.t/s,v.grad/s);
293  }
294 
295 
296 template <class T,class V,class S>
298  {
299  T v;
300  v= (exp(arg.t));
301  return Rall1d<T,V,S>(v,v*arg.grad);
302  }
303 
304 template <class T,class V,class S>
306  {
307  T v;
308  v=(log(arg.t));
309  return Rall1d<T,V,S>(v,arg.grad/arg.t);
310  }
311 
312 template <class T,class V,class S>
314  {
315  T v;
316  v=(sin(arg.t));
317  return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
318  }
319 
320 template <class T,class V,class S>
322  {
323  T v;
324  v=(cos(arg.t));
325  return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
326  }
327 
328 template <class T,class V,class S>
330  {
331  T v;
332  v=(tan(arg.t));
333  return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
334  }
335 
336 template <class T,class V,class S>
338  {
339  T v;
340  v=(sinh(arg.t));
341  return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
342  }
343 
344 template <class T,class V,class S>
346  {
347  T v;
348  v=(cosh(arg.t));
349  return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
350  }
351 
352 template <class T,class V,class S>
354  {
355  T v;
356  v=(arg.t*arg.t);
357  return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
358  }
359 
360 template <class T,class V,class S>
361 INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m)
362  {
363  T v;
364  v=(pow(arg.t,m));
365  return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
366  }
367 
368 template <class T,class V,class S>
370  {
371  T v;
372  v=sqrt(arg.t);
373  return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
374  }
375 
376 template <class T,class V,class S>
378 {
379  T v;
380  v=(atan(x.t));
381  return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
382 }
383 
384 template <class T,class V,class S>
386 {
387  T v;
388  v=(hypot(y.t,x.t));
389  return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
390 }
391 
392 template <class T,class V,class S>
394 {
395  T v;
396  v=(asin(x.t));
397  return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
398 }
399 
400 template <class T,class V,class S>
402 {
403  T v;
404  v=(acos(x.t));
405  return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
406 }
407 
408 template <class T,class V,class S>
410 {
411  T v;
412  v=(Sign(x));
413  return Rall1d<T,V,S>(v*x,v*x.grad);
414 }
415 
416 
417 template <class T,class V,class S>
419 {
420  return Norm(value.t);
421 }
422 
423 template <class T,class V,class S>
425 {
426  T v(tanh(arg.t));
427  return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
428 }
429 
430 template <class T,class V,class S>
432 {
433  T v(x.t*x.t+y.t*y.t);
434  return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
435 }
436 
437 
438 template <class T,class V,class S>
440  const T& beta,const Rall1d<T,V,S>& b ) {
441  return Rall1d<T,V,S>(
442  LinComb(alfa,a.t,beta,b.t),
443  LinComb(alfa,a.grad,beta,b.grad)
444  );
445 }
446 
447 template <class T,class V,class S>
448 INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
449  const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
450  LinCombR(alfa, a.t, beta, b.t, result.t);
451  LinCombR(alfa, a.grad, beta, b.grad, result.grad);
452 }
453 
454 
455 template <class T,class V,class S>
457  {
458  SetToZero(value.grad);
459  SetToZero(value.t);
460  }
461 template <class T,class V,class S>
463  {
464  SetToIdentity(value.t);
465  SetToZero(value.grad);
466  }
467 
468 template <class T,class V,class S>
469 INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
470 {
471  return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
472 }
473 
474 template <class T,class V,class S>
476 {
477 #ifdef KDL_USE_EQUAL
478  return Equal(y, x);
479 #else
480  return (x.t == y.t && x.grad == y.grad);
481 #endif
482 }
483 
484 template <class T,class V,class S>
486 {
487  return !operator==(y, x);
488 }
489 
490 }
491 
492 
493 
494 #endif
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.hpp:181
S scalartype
Definition: rall1d.h:54
INLINE Rall1d(const Rall1d< T, V, S > &r)
Definition: rall1d.h:73
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
T valuetype
Definition: rall1d.h:52
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:345
#define INLINE
Definition: utility.h:227
V gradienttype
Definition: rall1d.h:53
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
INLINE Rall1d< T, V, S > & operator=(S c)
Definition: rall1d.h:97
static INLINE Rall1d< T, V, S > Identity()
Definition: rall1d.h:91
INLINE Rall1d(typename TI< T >::Arg tn, typename TI< V >::Arg afg)
Definition: rall1d.h:71
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 T & Value()
Definition: rall1d.h:78
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.
static INLINE Rall1d< T, V, S > Zero()
Definition: rall1d.h:86
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:369
INLINE V & Gradient()
Definition: rall1d.h:82
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(typename TI< T >::Arg c)
Definition: rall1d.h:68
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Definition: rall1d.h:361
T value() const
Definition: rall1d.h:61
V grad
gradient
Definition: rall1d.h:57
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
T t
value
Definition: rall1d.h:56
INLINE Rall1d< T, V, S > & operator*=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:110
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:431
V deriv() const
Definition: rall1d.h:64
INLINE Rall1d< T, V, S > & operator/=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:103
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 Rall1d< T, V, S > & operator+=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:117
INLINE Rall1d< T, V, S > & operator-=(const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:124
INLINE Rall1d()
Definition: rall1d.h:59
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:313
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


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44