Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.

1 files changed, 478 insertions, 0 deletions

diff --git a/intern/itasc/kdl/utilities/rall1d.h b/intern/itasc/kdl/utilities/rall1d.h
new file mode 100644
index 00000000000..617683ffce6
--- /dev/null
+++ b/intern/itasc/kdl/utilities/rall1d.h
@@ -0,0 +1,478 @@
+ 
+/*****************************************************************************
+ * \file  
+ *      class for automatic differentiation on scalar values and 1st 
+ *      derivatives .
+ *       
+ *  \author 
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version 
+ *      ORO_Geometry V0.2
+ *
+ *  \par Note
+ *      VC6++ contains a bug, concerning the use of inlined friend functions 
+ *      in combination with namespaces.  So, try to avoid inlined friend 
+ *      functions !  
+ *
+ *  \par History
+ *      - $log$ 
+ *
+ *  \par Release
+ *      $Id: rall1d.h 19905 2009-04-23 13:29:54Z ben2610 $
+ *      $Name:  $ 
+ ****************************************************************************/
+ 
+#ifndef Rall1D_H
+#define Rall1D_H
+#include <assert.h>
+#include "utility.h"
+
+namespace KDL {
+/**
+ * Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair.
+ *  This template class has 3 template parameters :
+ *  -   T contains the type of the value.
+ *  -   V contains the type of the gradient (can be a vector-like type).  
+ *  -   S defines a scalar type that can operate on Rall1d.  This is the type that 
+ *      is used to give back values of Norm() etc. 
+ *
+ * S is usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 4th,.. 
+ * derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
+ *
+ * S is always passed by value. 
+ *
+ * \par Class Type
+ * Concrete implementation
+ */
+template <typename T,typename V=T,typename S=T>
+class Rall1d
+    {
+    public:
+        typedef T valuetype;
+        typedef V gradienttype;
+        typedef S scalartype;
+    public :
+        T t;        //!< value
+        V grad;     //!< gradient
+    public :
+        INLINE Rall1d() {}
+
+        T value() const {
+            return t;
+        }
+        V deriv() const {
+            return grad;
+        }
+
+        explicit INLINE  Rall1d(typename TI<T>::Arg c)
+            {t=T(c);SetToZero(grad);}
+
+        INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
+
+        INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
+        //if one defines this constructor, it's better optimized then the
+        //automatically generated one ( this one set's up a loop to copy
+        // word by word.
+        
+        INLINE T& Value() {
+            return t;
+        }
+
+        INLINE V& Gradient() {
+            return grad;
+        }
+
+        INLINE static Rall1d<T,V,S> Zero() {
+            Rall1d<T,V,S> tmp;
+            SetToZero(tmp);
+            return tmp;
+        }
+        INLINE static Rall1d<T,V,S> Identity() {
+            Rall1d<T,V,S> tmp;
+            SetToIdentity(tmp);
+            return tmp;
+        }
+
+        INLINE Rall1d<T,V,S>& operator =(S c)
+            {t=c;SetToZero(grad);return *this;}
+
+        INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
+            {t=r.t;grad=r.grad;return *this;}
+
+        INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
+            {
+            grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
+            t     /= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
+            {
+            LinCombR(rhs.t,grad,t,rhs.grad,grad);
+            t *= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
+            {
+            grad +=rhs.grad;
+            t    +=rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
+            {
+            grad -= rhs.grad;
+            t     -= rhs.t;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator /=(S rhs)
+            {
+            grad /= rhs;
+            t    /= rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator *=(S rhs)
+            {
+            grad *= rhs;
+            t    *= rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator +=(S rhs)
+            {
+            t    += rhs;
+            return *this;
+            }
+
+        INLINE Rall1d<T,V,S>& operator -=(S rhs)
+            {
+            t    -= rhs;
+            return *this;
+            }
+
+
+
+        // = operators
+        /* gives warnings on cygwin 
+        
+         template <class T2,class V2,class S2>
+         friend INLINE  Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
+         
+         friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
+         friend INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
+         friend INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
+
+        // = Mathematical functions that operate on Rall1d objects
+         friend INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
+         friend INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
+         friend INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
+         friend INLINE  S Norm(const Rall1d<T,V,S>& value) ;
+         friend INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
+         friend INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
+         
+        // = Utility functions to improve performance
+
+         friend INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
+            const T& beta,const Rall1d<T,V,S>& b );
+        
+         friend 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 );
+        
+        // = Setting value of a Rall1d object to 0 or 1
+
+         friend INLINE  void SetToZero(Rall1d<T,V,S>& value);
+         friend INLINE  void SetToOne(Rall1d<T,V,S>& value);
+        // = Equality in an eps-interval
+         friend INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
+         */
+    };
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
+    }
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
+    {
+    return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
+    {
+    return Rall1d<T,V,S>(-arg.t,-arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s*v.t,s*v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t*s,v.grad*s);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s+v.t,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t+s,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s-v.t,-v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t-s,v.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
+    {
+    return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
+    {
+    return Rall1d<T,V,S>(v.t/s,v.grad/s);
+    }
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v= (exp(arg.t));
+    return Rall1d<T,V,S>(v,v*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(log(arg.t));
+    return Rall1d<T,V,S>(v,arg.grad/arg.t);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(sin(arg.t));
+    return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(cos(arg.t));
+    return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(tan(arg.t));
+    return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(sinh(arg.t));
+    return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(cosh(arg.t));
+    return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=(arg.t*arg.t);
+    return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) 
+    {
+    T v;
+    v=(pow(arg.t,m));
+    return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
+    {
+    T v;
+    v=sqrt(arg.t);
+    return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
+    }   
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(atan(x.t));
+    return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(hypot(y.t,x.t));
+    return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(asin(x.t));
+    return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(acos(x.t));
+    return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
+{
+    T v;
+    v=(Sign(x));
+    return Rall1d<T,V,S>(v*x,v*x.grad);
+}
+
+
+template <class T,class V,class S>
+INLINE  S Norm(const Rall1d<T,V,S>& value) 
+{
+    return Norm(value.t);
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
+{       
+    T v(tanh(arg.t));       
+    return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
+}
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
+{
+    T v(x.t*x.t+y.t*y.t);
+    return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
+}
+
+
+template <class T,class V,class S>
+INLINE  Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
+    const T& beta,const Rall1d<T,V,S>& b ) {
+        return Rall1d<T,V,S>(
+            LinComb(alfa,a.t,beta,b.t),
+            LinComb(alfa,a.grad,beta,b.grad)
+        );
+}
+
+template <class T,class V,class S>
+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 ) {
+            LinCombR(alfa, a.t,       beta, b.t,      result.t);
+            LinCombR(alfa, a.grad,    beta, b.grad,   result.grad);
+}
+
+
+template <class T,class V,class S>
+INLINE  void SetToZero(Rall1d<T,V,S>& value)
+    {
+    SetToZero(value.grad);
+    SetToZero(value.t);
+    }
+template <class T,class V,class S>
+INLINE  void SetToIdentity(Rall1d<T,V,S>& value)
+    {
+    SetToIdentity(value.t);
+    SetToZero(value.grad);
+    }
+
+template <class T,class V,class S>
+INLINE  bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
+{
+    return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
+}
+
+}
+
+
+
+#endif