Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/intern/itasc/kdl/utilities
diff options
context:
space:
mode:
Diffstat (limited to 'intern/itasc/kdl/utilities')
-rw-r--r--intern/itasc/kdl/utilities/Makefile37
-rw-r--r--intern/itasc/kdl/utilities/error.h245
-rw-r--r--intern/itasc/kdl/utilities/error_stack.cpp59
-rw-r--r--intern/itasc/kdl/utilities/error_stack.h70
-rw-r--r--intern/itasc/kdl/utilities/kdl-config.h33
-rw-r--r--intern/itasc/kdl/utilities/rall1d.h478
-rw-r--r--intern/itasc/kdl/utilities/rall2d.h538
-rw-r--r--intern/itasc/kdl/utilities/svd_eigen_HH.hpp309
-rw-r--r--intern/itasc/kdl/utilities/traits.h111
-rw-r--r--intern/itasc/kdl/utilities/utility.cpp21
-rw-r--r--intern/itasc/kdl/utilities/utility.h298
-rw-r--r--intern/itasc/kdl/utilities/utility_io.cpp208
-rw-r--r--intern/itasc/kdl/utilities/utility_io.h79
13 files changed, 2486 insertions, 0 deletions
diff --git a/intern/itasc/kdl/utilities/Makefile b/intern/itasc/kdl/utilities/Makefile
new file mode 100644
index 00000000000..26bd7cfb470
--- /dev/null
+++ b/intern/itasc/kdl/utilities/Makefile
@@ -0,0 +1,37 @@
+#
+# $Id: Makefile 19905 2009-04-23 13:29:54Z ben2610 $
+#
+# ***** BEGIN GPL LICENSE BLOCK *****
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+# All rights reserved.
+#
+# The Original Code is: all of this file.
+#
+# Contributor(s): Hans Lambermont
+#
+# ***** END GPL LICENSE BLOCK *****
+# iksolver main makefile.
+#
+
+LIBNAME = itasc
+DIR = $(OCGDIR)/intern/$(LIBNAME)
+
+include nan_compile.mk
+
+CPPFLAGS += -I.
+CPPFLAGS += -I../../../../extern/Eigen2
diff --git a/intern/itasc/kdl/utilities/error.h b/intern/itasc/kdl/utilities/error.h
new file mode 100644
index 00000000000..c6cf916c151
--- /dev/null
+++ b/intern/itasc/kdl/utilities/error.h
@@ -0,0 +1,245 @@
+/***************************************************************************
+ tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 error.h
+
+ error.h - description
+ -------------------
+ begin : Mon January 10 2005
+ copyright : (C) 2005 Erwin Aertbelien
+ email : erwin.aertbelien@mech.kuleuven.ac.be
+
+ ***************************************************************************
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Lesser General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2.1 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
+ * Lesser General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Lesser General Public *
+ * License along with this library; if not, write to the Free Software *
+ * Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307 USA *
+ * *
+ ***************************************************************************/
+
+
+/*****************************************************************************
+ * \file
+ * Defines the exception classes that can be thrown
+ * \author
+ * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par History
+ * - $log$
+ *
+ * \par Release
+ * $Id: error.h 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ ****************************************************************************/
+#ifndef ERROR_H_84822 // to make it unique, a random number
+#define ERROR_H_84822
+
+#include "utility.h"
+#include <string>
+
+namespace KDL {
+
+/**
+ * Base class for errors generated by ORO_Geometry
+ */
+class Error {
+public:
+ /** Returns a description string describing the error.
+ * the returned pointer only garanteed to exists as long as
+ * the Error object exists.
+ */
+ virtual ~Error() {}
+ virtual const char* Description() const {return "Unspecified Error\n";}
+
+ virtual int GetType() const {return 0;}
+};
+
+
+class Error_IO : public Error {
+ std::string msg;
+ int typenr;
+public:
+ Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {}
+ virtual const char* Description() const {return msg.c_str();}
+ virtual int GetType() const {return typenr;}
+};
+class Error_BasicIO : public Error_IO {};
+class Error_BasicIO_File : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "Error while reading stream";}
+ virtual int GetType() const {return 1;}
+};
+class Error_BasicIO_Exp_Delim : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "Expected Delimiter not encountered";}
+ virtual int GetType() const {return 2;}
+};
+class Error_BasicIO_Not_A_Space : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "Expected space,tab or newline not encountered";}
+ virtual int GetType() const {return 3;}
+};
+class Error_BasicIO_Unexpected : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "Unexpected character";}
+ virtual int GetType() const {return 4;}
+};
+
+class Error_BasicIO_ToBig : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "Word that is read out of stream is bigger than maxsize";}
+ virtual int GetType() const {return 5;}
+};
+
+class Error_BasicIO_Not_Opened : public Error_BasicIO {
+public:
+ virtual const char* Description() const {return "File cannot be opened";}
+ virtual int GetType() const {return 6;}
+};
+class Error_FrameIO : public Error_IO {};
+class Error_Frame_Vector_Unexpected_id : public Error_FrameIO {
+public:
+ virtual const char* Description() const {return "Unexpected identifier, expecting a vector (explicit or ZERO)";}
+ virtual int GetType() const {return 101;}
+};
+class Error_Frame_Frame_Unexpected_id : public Error_FrameIO {
+public:
+ virtual const char* Description() const {return "Unexpected identifier, expecting a Frame (explicit or DH)";}
+ virtual int GetType() const {return 102;}
+};
+class Error_Frame_Rotation_Unexpected_id : public Error_FrameIO {
+public:
+ virtual const char* Description() const {return "Unexpected identifier, expecting a Rotation (explicit or EULERZYX, EULERZYZ, RPY,ROT,IDENTITY)";}
+ virtual int GetType() const {return 103;}
+};
+class Error_ChainIO : public Error {};
+class Error_Chain_Unexpected_id : public Error_ChainIO {
+public:
+ virtual const char* Description() const {return "Unexpected identifier, expecting TRANS or ROT";}
+ virtual int GetType() const {return 201;}
+};
+//! Error_Redundancy indicates an error that occured during solving for redundancy.
+class Error_RedundancyIO:public Error_IO {};
+class Error_Redundancy_Illegal_Resolutiontype : public Error_RedundancyIO {
+public:
+ virtual const char* Description() const {return "Illegal Resolutiontype is used in I/O with ResolutionTask";}
+ virtual int GetType() const {return 301;}
+};
+class Error_Redundancy:public Error {};
+class Error_Redundancy_Unavoidable : public Error_Redundancy {
+public:
+ virtual const char* Description() const {return "Joint limits cannot be avoided";}
+ virtual int GetType() const {return 1002;}
+};
+class Error_Redundancy_Low_Manip: public Error_Redundancy {
+public:
+ virtual const char* Description() const {return "Manipulability is very low";}
+ virtual int GetType() const {return 1003;}
+};
+class Error_MotionIO : public Error {};
+class Error_MotionIO_Unexpected_MotProf : public Error_MotionIO {
+public:
+ virtual const char* Description() const { return "Wrong keyword while reading motion profile";}
+ virtual int GetType() const {return 2001;}
+};
+class Error_MotionIO_Unexpected_Traj : public Error_MotionIO {
+public:
+ virtual const char* Description() const { return "Trajectory type keyword not known";}
+ virtual int GetType() const {return 2002;}
+};
+
+class Error_MotionPlanning : public Error {};
+
+class Error_MotionPlanning_Circle_ToSmall : public Error_MotionPlanning {
+public:
+ virtual const char* Description() const { return "Circle : radius is to small";}
+ virtual int GetType() const {return 3001;}
+};
+
+class Error_MotionPlanning_Circle_No_Plane : public Error_MotionPlanning {
+public:
+ virtual const char* Description() const { return "Circle : Plane for motion is not properly defined";}
+ virtual int GetType() const {return 3002;}
+};
+
+class Error_MotionPlanning_Incompatible: public Error_MotionPlanning {
+public:
+ virtual const char* Description() const { return "Acceleration of a rectangular velocityprofile cannot be used";}
+ virtual int GetType() const {return 3003;}
+};
+
+class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning {
+public:
+ virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";}
+ virtual int GetType() const {return 3004;}
+};
+
+class Error_MotionPlanning_Not_Applicable: public Error_MotionPlanning {
+public:
+ virtual const char* Description() const { return "Method is not applicable for this derived object";}
+ virtual int GetType() const {return 3004;}
+};
+//! Abstract subclass of all errors that can be thrown by Adaptive_Integrator
+class Error_Integrator : public Error {};
+
+//! Error_Stepsize_Underflow is thrown if the stepsize becomes to small
+class Error_Stepsize_Underflow : public Error_Integrator {
+public:
+ virtual const char* Description() const { return "Stepsize Underflow";}
+ virtual int GetType() const {return 4001;}
+};
+
+//! Error_To_Many_Steps is thrown if the number of steps needed to
+//! integrate to the desired accuracy becomes to big.
+class Error_To_Many_Steps : public Error_Integrator {
+public:
+ virtual const char* Description() const { return "To many steps"; }
+ virtual int GetType() const {return 4002;}
+};
+
+//! Error_Stepsize_To_Small is thrown if the stepsize becomes to small
+class Error_Stepsize_To_Small : public Error_Integrator {
+public:
+ virtual const char* Description() const { return "Stepsize to small"; }
+ virtual int GetType() const {return 4003;}
+};
+
+class Error_Criterium : public Error {};
+
+class Error_Criterium_Unexpected_id: public Error_Criterium {
+public:
+ virtual const char* Description() const { return "Unexpected identifier while reading a criterium"; }
+ virtual int GetType() const {return 5001;}
+};
+
+class Error_Limits : public Error {};
+
+class Error_Limits_Unexpected_id: public Error_Limits {
+public:
+ virtual const char* Description() const { return "Unexpected identifier while reading a jointlimits"; }
+ virtual int GetType() const {return 6001;}
+};
+
+
+class Error_Not_Implemented: public Error {
+public:
+ virtual const char* Description() const { return "The requested object/method/function is not implemented"; }
+ virtual int GetType() const {return 7000;}
+};
+
+
+
+}
+
+#endif
diff --git a/intern/itasc/kdl/utilities/error_stack.cpp b/intern/itasc/kdl/utilities/error_stack.cpp
new file mode 100644
index 00000000000..aabf47ad191
--- /dev/null
+++ b/intern/itasc/kdl/utilities/error_stack.cpp
@@ -0,0 +1,59 @@
+/*****************************************************************************
+ * \author
+ * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par History
+ * - $log$
+ *
+ * \par Release
+ * $Id: error_stack.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ ****************************************************************************/
+
+
+#include "error_stack.h"
+#include <stack>
+#include <vector>
+#include <string>
+#include <cstring>
+
+namespace KDL {
+
+// Trace of the call stack of the I/O routines to help user
+// interprete error messages from I/O
+typedef std::stack<std::string> ErrorStack;
+
+ErrorStack errorstack;
+// should be in Thread Local Storage if this gets multithreaded one day...
+
+
+void IOTrace(const std::string& description) {
+ errorstack.push(description);
+}
+
+
+void IOTracePop() {
+ errorstack.pop();
+}
+
+void IOTraceOutput(std::ostream& os) {
+ while (!errorstack.empty()) {
+ os << errorstack.top().c_str() << std::endl;
+ errorstack.pop();
+ }
+}
+
+
+void IOTracePopStr(char* buffer,int size) {
+ if (errorstack.empty()) {
+ *buffer = 0;
+ return;
+ }
+ strncpy(buffer,errorstack.top().c_str(),size);
+ errorstack.pop();
+}
+
+}
diff --git a/intern/itasc/kdl/utilities/error_stack.h b/intern/itasc/kdl/utilities/error_stack.h
new file mode 100644
index 00000000000..918bc0786a6
--- /dev/null
+++ b/intern/itasc/kdl/utilities/error_stack.h
@@ -0,0 +1,70 @@
+/***************************************************************************
+ tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 error_stack.h
+
+ error_stack.h - description
+ -------------------
+ begin : Mon January 10 2005
+ copyright : (C) 2005 Erwin Aertbelien
+ email : erwin.aertbelien@mech.kuleuven.ac.be
+
+ ***************************************************************************
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of the GNU Lesser General Public *
+ * License as published by the Free Software Foundation; either *
+ * version 2.1 of the License, or (at your option) any later version. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
+ * Lesser General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU Lesser General Public *
+ * License along with this library; if not, write to the Free Software *
+ * Foundation, Inc., 59 Temple Place, *
+ * Suite 330, Boston, MA 02111-1307 USA *
+ * *
+ ***************************************************************************/
+
+
+/**
+ * \file
+ * \author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par history
+ * - changed layout of the comments to accomodate doxygen
+ */
+#ifndef ERROR_STACK_H
+#define ERROR_STACK_H
+
+#include "utility.h"
+#include "utility_io.h"
+#include <string>
+
+
+namespace KDL {
+
+/*
+ * \todo
+ * IOTrace-routines store in static memory, should be in thread-local memory.
+ * pushes a description of the current routine on the IO-stack trace
+ */
+void IOTrace(const std::string& description);
+
+//! pops a description of the IO-stack
+void IOTracePop();
+
+
+//! outputs the IO-stack to a stream to provide a better errormessage.
+void IOTraceOutput(std::ostream& os);
+
+//! outputs one element of the IO-stack to the buffer (maximally size chars)
+//! returns empty string if no elements on the stack.
+void IOTracePopStr(char* buffer,int size);
+
+
+}
+
+#endif
+
diff --git a/intern/itasc/kdl/utilities/kdl-config.h b/intern/itasc/kdl/utilities/kdl-config.h
new file mode 100644
index 00000000000..4d2df2df6c5
--- /dev/null
+++ b/intern/itasc/kdl/utilities/kdl-config.h
@@ -0,0 +1,33 @@
+/* Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+
+/* Version: 1.0 */
+/* Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+/* Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
+/* URL: http://www.orocos.org/kdl */
+
+/* This library is free software; you can redistribute it and/or */
+/* modify it under the terms of the GNU Lesser General Public */
+/* License as published by the Free Software Foundation; either */
+/* version 2.1 of the License, or (at your option) any later version. */
+
+/* This library is distributed in the hope that it will be useful, */
+/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
+/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU */
+/* Lesser General Public License for more details. */
+
+/* You should have received a copy of the GNU Lesser General Public */
+/* License along with this library; if not, write to the Free Software */
+/* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */
+
+
+/* Methods are inlined */
+#define KDL_INLINE 1
+
+/* Column width that is used form printing frames */
+#define KDL_FRAME_WIDTH 12
+
+/* Indices are checked when accessing members of the objects */
+#define KDL_INDEX_CHECK 1
+
+/* use KDL implementation for == operator */
+#define KDL_USE_EQUAL 1
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
diff --git a/intern/itasc/kdl/utilities/rall2d.h b/intern/itasc/kdl/utilities/rall2d.h
new file mode 100644
index 00000000000..ca4c67319f5
--- /dev/null
+++ b/intern/itasc/kdl/utilities/rall2d.h
@@ -0,0 +1,538 @@
+
+/*****************************************************************************
+ * \file
+ * class for automatic differentiation on scalar values and 1st
+ * derivatives and 2nd derivative.
+ *
+ * \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: rall2d.h 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ ****************************************************************************/
+
+#ifndef Rall2D_H
+#define Rall2D_H
+
+#include <math.h>
+#include <assert.h>
+#include "utility.h"
+
+
+namespace KDL {
+
+/**
+ * Rall2d contains a value, and its gradient and its 2nd derivative, 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 <class T,class V=T,class S=T>
+class Rall2d
+ {
+ public :
+ T t; //!< value
+ V d; //!< 1st derivative
+ V dd; //!< 2nd derivative
+ public :
+ // = Constructors
+ INLINE Rall2d() {}
+
+ explicit INLINE Rall2d(typename TI<T>::Arg c)
+ {t=c;SetToZero(d);SetToZero(dd);}
+
+ INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
+
+ INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
+
+ // = Copy Constructor
+ INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
+ //if one defines this constructor, it's better optimized then the
+ //automatically generated one ( that one set's up a loop to copy
+ // word by word.
+
+ // = Member functions to access internal structures :
+ INLINE T& Value() {
+ return t;
+ }
+
+ INLINE V& D() {
+ return d;
+ }
+
+ INLINE V& DD() {
+ return dd;
+ }
+ INLINE static Rall2d<T,V,S> Zero() {
+ Rall2d<T,V,S> tmp;
+ SetToZero(tmp);
+ return tmp;
+ }
+ INLINE static Rall2d<T,V,S> Identity() {
+ Rall2d<T,V,S> tmp;
+ SetToIdentity(tmp);
+ return tmp;
+ }
+
+ // = assignment operators
+ INLINE Rall2d<T,V,S>& operator =(S c)
+ {t=c;SetToZero(d);SetToZero(dd);return *this;}
+
+ INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
+ {t=r.t;d=r.d;dd=r.dd;return *this;}
+
+ INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
+ {
+ t /= rhs.t;
+ d = (d-t*rhs.d)/rhs.t;
+ dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
+ {
+ t *= rhs.t;
+ d = (d*rhs.t+t*rhs.d);
+ dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
+ {
+ t +=rhs.t;
+ d +=rhs.d;
+ dd+=rhs.dd;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
+ {
+ t -= rhs.t;
+ d -= rhs.d;
+ dd -= rhs.dd;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator /=(S rhs)
+ {
+ t /= rhs;
+ d /= rhs;
+ dd /= rhs;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator *=(S rhs)
+ {
+ t *= rhs;
+ d *= rhs;
+ dd *= rhs;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator -=(S rhs)
+ {
+ t -= rhs;
+ return *this;
+ }
+
+ INLINE Rall2d<T,V,S>& operator +=(S rhs)
+ {
+ t += rhs;
+ return *this;
+ }
+
+ // = Operators between Rall2d objects
+/*
+ friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+ friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+ friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+ friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
+ friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
+ friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
+ friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
+ friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
+ friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
+ friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
+ friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
+ friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
+
+ // = Mathematical functions that operate on Rall2d objects
+
+ friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
+ friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
+ friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
+ friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
+ friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
+ friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
+ // returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
+ friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
+ // returns Norm( value.Value() ).
+
+ // = Some utility functions to improve performance
+ // (should also be declared on primitive types to improve uniformity
+ friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
+ TI<T>::Arg beta,const Rall2d<T,V,S>& b );
+ friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
+ TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
+ // = Setting value of a Rall2d object to 0 or 1
+ friend INLINE void SetToZero(Rall2d<T,V,S>& value);
+ friend INLINE void SetToOne(Rall2d<T,V,S>& value);
+ // = Equality in an eps-interval
+ friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
+ */
+ };
+
+
+
+
+
+// = Operators between Rall2d objects
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = lhs.t/rhs.t;
+ tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
+ tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = lhs.t*rhs.t;
+ tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d);
+ tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+ {
+ return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
+ {
+ return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
+ {
+ return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
+ {
+ return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
+ {
+ return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
+ {
+ return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
+ {
+ return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
+ {
+ return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
+ {
+ return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = s/rhs.t;
+ tmp.d = (-tmp.t*rhs.d)/rhs.t;
+ tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
+ return tmp;
+}
+
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
+ {
+ return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
+ }
+
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = exp(arg.t);
+ tmp.d = tmp.t*arg.d;
+ tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = log(arg.t);
+ tmp.d = arg.d/arg.t;
+ tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = sin(arg.t);
+ T v2 = cos(arg.t);
+ return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = cos(arg.t);
+ T v2 = -sin(arg.t);
+ return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = tan(arg.t);
+ T v2 = S(1)+sqr(v1);
+ return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = sinh(arg.t);
+ T v2 = cosh(arg.t);
+ return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = cosh(arg.t);
+ T v2 = sinh(arg.t);
+ return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
+ {
+ T v1 = tanh(arg.t);
+ T v2 = S(1)-sqr(v1);
+ return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
+ {
+ return Rall2d<T,V,S>(arg.t*arg.t,
+ (S(2)*arg.t)*arg.d,
+ S(2)*(sqr(arg.d)+arg.t*arg.dd)
+ );
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m)
+ {
+ Rall2d<T,V,S> tmp;
+ tmp.t = pow(arg.t,m);
+ T v2 = (m/arg.t)*tmp.t;
+ tmp.d = v2*arg.d;
+ tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
+ {
+ /* By inversion of sqr(x) :*/
+ Rall2d<T,V,S> tmp;
+ tmp.t = sqrt(arg.t);
+ tmp.d = (S(0.5)/tmp.t)*arg.d;
+ tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
+ return tmp;
+ }
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
+{
+ /* By inversion of sin(x) */
+ Rall2d<T,V,S> tmp;
+ tmp.t = asin(arg.t);
+ T v = cos(tmp.t);
+ tmp.d = arg.d/v;
+ tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
+ return tmp;
+}
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
+{
+ /* By inversion of cos(x) */
+ Rall2d<T,V,S> tmp;
+ tmp.t = acos(arg.t);
+ T v = -sin(tmp.t);
+ tmp.d = arg.d/v;
+ tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
+ return tmp;
+
+}
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
+{
+ /* By inversion of tan(x) */
+ Rall2d<T,V,S> tmp;
+ tmp.t = atan(x.t);
+ T v = S(1)+sqr(x.t);
+ tmp.d = x.d/v;
+ tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
+ return tmp;
+}
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+ Rall2d<T,V,S> tmp;
+ tmp.t = atan2(y.t,x.t);
+ T v = sqr(y.t)+sqr(x.t);
+ tmp.d = (x.t*y.d-x.d*y.t)/v;
+ tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
+ return tmp;
+}
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
+{
+ T v(Sign(x));
+ return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
+}
+
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
+{
+ Rall2d<T,V,S> tmp;
+ tmp.t = hypot(y.t,x.t);
+ tmp.d = (x.t*x.d+y.t*y.d)/tmp.t;
+ tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
+ return tmp;
+}
+// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
+
+template <class T,class V,class S>
+INLINE S Norm(const Rall2d<T,V,S>& value)
+{
+ return Norm(value.t);
+}
+// returns Norm( value.Value() ).
+
+
+// (should also be declared on primitive types to improve uniformity
+template <class T,class V,class S>
+INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
+ const T& beta,const Rall2d<T,V,S>& b ) {
+ return Rall2d<T,V,S>(
+ LinComb(alfa,a.t,beta,b.t),
+ LinComb(alfa,a.d,beta,b.d),
+ LinComb(alfa,a.dd,beta,b.dd)
+ );
+}
+
+template <class T,class V,class S>
+INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
+ const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
+ LinCombR(alfa, a.t, beta, b.t, result.t);
+ LinCombR(alfa, a.d, beta, b.d, result.d);
+ LinCombR(alfa, a.dd, beta, b.dd, result.dd);
+}
+
+template <class T,class V,class S>
+INLINE void SetToZero(Rall2d<T,V,S>& value)
+ {
+ SetToZero(value.t);
+ SetToZero(value.d);
+ SetToZero(value.dd);
+ }
+
+template <class T,class V,class S>
+INLINE void SetToIdentity(Rall2d<T,V,S>& value)
+ {
+ SetToZero(value.d);
+ SetToIdentity(value.t);
+ SetToZero(value.dd);
+ }
+
+template <class T,class V,class S>
+INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
+{
+ return (Equal(x.t,y.t,eps)&&
+ Equal(x.d,y.d,eps)&&
+ Equal(x.dd,y.dd,eps)
+ );
+}
+
+
+}
+
+
+#endif
diff --git a/intern/itasc/kdl/utilities/svd_eigen_HH.hpp b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp
new file mode 100644
index 00000000000..2bbb8df521f
--- /dev/null
+++ b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp
@@ -0,0 +1,309 @@
+// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+
+//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
+#ifndef SVD_EIGEN_HH_HPP
+#define SVD_EIGEN_HH_HPP
+
+
+#include <Eigen/Array>
+#include <algorithm>
+
+namespace KDL
+{
+ template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) {
+ double at,bt,ct;
+ at = fabs(a);
+ bt = fabs(b);
+ if (at > bt ) {
+ ct=bt/at;
+ return Scalar(at*sqrt(1.0+ct*ct));
+ } else {
+ if (bt==0)
+ return Scalar(0.0);
+ else {
+ ct=at/bt;
+ return Scalar(bt*sqrt(1.0+ct*ct));
+ }
+ }
+ }
+
+
+ template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) {
+ return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a));
+ }
+
+ /**
+ * svd calculation of boost ublas matrices
+ *
+ * @param A matrix<double>(mxn)
+ * @param U matrix<double>(mxn)
+ * @param S vector<double> n
+ * @param V matrix<double>(nxn)
+ * @param tmp vector<double> n
+ * @param maxiter defaults to 150
+ *
+ * @return -2 if maxiter exceeded, 0 otherwise
+ */
+ template<typename MatrixA, typename MatrixUV, typename VectorS>
+ int svd_eigen_HH(
+ const Eigen::MatrixBase<MatrixA>& A,
+ Eigen::MatrixBase<MatrixUV>& U,
+ Eigen::MatrixBase<VectorS>& S,
+ Eigen::MatrixBase<MatrixUV>& V,
+ Eigen::MatrixBase<VectorS>& tmp,
+ int maxiter=150)
+ {
+ //get the rows/columns of the matrix
+ const int rows = A.rows();
+ const int cols = A.cols();
+
+ U = A;
+
+ int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
+ int ppi(0);
+ bool flag;
+ e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
+
+ g=scale=anorm=e_scalar(0.0);
+
+ /* Householder reduction to bidiagonal form. */
+ for (i=0;i<cols;i++) {
+ ppi=i+1;
+ tmp(i)=scale*g;
+ g=s=scale=e_scalar(0.0);
+ if (i<rows) {
+ // compute the sum of the i-th column, starting from the i-th row
+ for (k=i;k<rows;k++) scale += fabs(U(k,i));
+ if (scale!=0) {
+ // multiply the i-th column by 1.0/scale, start from the i-th element
+ // sum of squares of column i, start from the i-th element
+ for (k=i;k<rows;k++) {
+ U(k,i) /= scale;
+ s += U(k,i)*U(k,i);
+ }
+ f=U(i,i); // f is the diag elem
+ g = -SIGN(e_scalar(sqrt(s)),f);
+ h=f*g-s;
+ U(i,i)=f-g;
+ for (j=ppi;j<cols;j++) {
+ // dot product of columns i and j, starting from the i-th row
+ for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
+ f=s/h;
+ // copy the scaled i-th column into the j-th column
+ for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
+ }
+ for (k=i;k<rows;k++) U(k,i) *= scale;
+ }
+ }
+ // save singular value
+ S(i)=scale*g;
+ g=s=scale=e_scalar(0.0);
+ if ((i <rows) && (i+1 != cols)) {
+ // sum of row i, start from columns i+1
+ for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
+ if (scale!=0) {
+ for (k=ppi;k<cols;k++) {
+ U(i,k) /= scale;
+ s += U(i,k)*U(i,k);
+ }
+ f=U(i,ppi);
+ g = -SIGN(e_scalar(sqrt(s)),f);
+ h=f*g-s;
+ U(i,ppi)=f-g;
+ for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
+ for (j=ppi;j<rows;j++) {
+ for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
+ for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
+ }
+ for (k=ppi;k<cols;k++) U(i,k) *= scale;
+ }
+ }
+ maxarg1=anorm;
+ maxarg2=(fabs(S(i))+fabs(tmp(i)));
+ anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
+ }
+ /* Accumulation of right-hand transformations. */
+ for (i=cols-1;i>=0;i--) {
+ if (i<cols-1) {
+ if (g) {
+ for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
+ for (j=ppi;j<cols;j++) {
+ for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
+ for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
+ }
+ }
+ for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
+ }
+ V(i,i)=1.0;
+ g=tmp(i);
+ ppi=i;
+ }
+ /* Accumulation of left-hand transformations. */
+ for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
+ ppi=i+1;
+ g=S(i);
+ for (j=ppi;j<cols;j++) U(i,j)=0.0;
+ if (g) {
+ g=e_scalar(1.0)/g;
+ for (j=ppi;j<cols;j++) {
+ for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
+ f=(s/U(i,i))*g;
+ for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
+ }
+ for (j=i;j<rows;j++) U(j,i) *= g;
+ } else {
+ for (j=i;j<rows;j++) U(j,i)=0.0;
+ }
+ ++U(i,i);
+ }
+
+ /* Diagonalization of the bidiagonal form. */
+ for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
+ for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */
+ flag=true;
+ for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */
+ nm=ppi-1; /* Note that tmp(1) is always zero. */
+ if ((fabs(tmp(ppi))+anorm) == anorm) {
+ flag=false;
+ break;
+ }
+ if ((fabs(S(nm)+anorm) == anorm)) break;
+ }
+ if (flag) {
+ c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */
+ s=e_scalar(1.);
+ for (i=ppi;i<=k;i++) {
+ f=s*tmp(i);
+ tmp(i)=c*tmp(i);
+ if ((fabs(f)+anorm) == anorm) break;
+ g=S(i);
+ h=PYTHAG(f,g);
+ S(i)=h;
+ h=e_scalar(1.0)/h;
+ c=g*h;
+ s=(-f*h);
+ for (j=0;j<rows;j++) {
+ y=U(j,nm);
+ z=U(j,i);
+ U(j,nm)=y*c+z*s;
+ U(j,i)=z*c-y*s;
+ }
+ }
+ }
+ z=S(k);
+
+ if (ppi == k) { /* Convergence. */
+ if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */
+ S(k) = -z;
+ for (j=0;j<cols;j++) V(j,k)=-V(j,k);
+ }
+ break;
+ }
+
+ x=S(ppi); /* Shift from bottom 2-by-2 minor: */
+ nm=k-1;
+ y=S(nm);
+ g=tmp(nm);
+ h=tmp(k);
+ f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y);
+
+ g=PYTHAG(f,e_scalar(1.0));
+ f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
+
+ /* Next QR transformation: */
+ c=s=1.0;
+ for (j=ppi;j<=nm;j++) {
+ i=j+1;
+ g=tmp(i);
+ y=S(i);
+ h=s*g;
+ g=c*g;
+ z=PYTHAG(f,h);
+ tmp(j)=z;
+ c=f/z;
+ s=h/z;
+ f=x*c+g*s;
+ g=g*c-x*s;
+ h=y*s;
+ y=y*c;
+ for (jj=0;jj<cols;jj++) {
+ x=V(jj,j);
+ z=V(jj,i);
+ V(jj,j)=x*c+z*s;
+ V(jj,i)=z*c-x*s;
+ }
+ z=PYTHAG(f,h);
+ S(j)=z;
+ if (z) {
+ z=e_scalar(1.0)/z;
+ c=f*z;
+ s=h*z;
+ }
+ f=(c*g)+(s*y);
+ x=(c*y)-(s*g);
+ for (jj=0;jj<rows;jj++) {
+ y=U(jj,j);
+ z=U(jj,i);
+ U(jj,j)=y*c+z*s;
+ U(jj,i)=z*c-y*s;
+ }
+ }
+ tmp(ppi)=0.0;
+ tmp(k)=f;
+ S(k)=x;
+ }
+ }
+
+ //Sort eigen values:
+ for (i=0; i<cols; i++){
+
+ double S_max = S(i);
+ int i_max = i;
+ for (j=i+1; j<cols; j++){
+ double Sj = S(j);
+ if (Sj > S_max){
+ S_max = Sj;
+ i_max = j;
+ }
+ }
+ if (i_max != i){
+ /* swap eigenvalues */
+ e_scalar tmp = S(i);
+ S(i)=S(i_max);
+ S(i_max)=tmp;
+
+ /* swap eigenvectors */
+ U.col(i).swap(U.col(i_max));
+ V.col(i).swap(V.col(i_max));
+ }
+ }
+
+
+ if (its == maxiter)
+ return (-2);
+ else
+ return (0);
+ }
+
+}
+#endif
diff --git a/intern/itasc/kdl/utilities/traits.h b/intern/itasc/kdl/utilities/traits.h
new file mode 100644
index 00000000000..2656d633653
--- /dev/null
+++ b/intern/itasc/kdl/utilities/traits.h
@@ -0,0 +1,111 @@
+#ifndef KDLPV_TRAITS_H
+#define KDLPV_TRAITS_H
+
+#include "utility.h"
+
+
+// forwards declarations :
+namespace KDL {
+ class Frame;
+ class Rotation;
+ class Vector;
+ class Twist;
+ class Wrench;
+ class FrameVel;
+ class RotationVel;
+ class VectorVel;
+ class TwistVel;
+}
+
+
+/**
+ * @brief Traits are traits classes to determine the type of a derivative of another type.
+ *
+ * For geometric objects the "geometric" derivative is chosen. For example the derivative of a Rotation
+ * matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix. The derivative
+ * of the rotation matrix is a Vector corresponding the rotational velocity. Mostly used in template classes
+ * and routines to derive a correct type when needed.
+ *
+ * You can see this as a compile-time lookuptable to find the type of the derivative.
+ *
+ * Example
+ * \verbatim
+ Rotation R;
+ Traits<Rotation> dR;
+ \endverbatim
+ */
+template <typename T>
+struct Traits {
+ typedef T valueType;
+ typedef T derivType;
+};
+
+template <>
+struct Traits<KDL::Frame> {
+ typedef KDL::Frame valueType;
+ typedef KDL::Twist derivType;
+};
+template <>
+struct Traits<KDL::Twist> {
+ typedef KDL::Twist valueType;
+ typedef KDL::Twist derivType;
+};
+template <>
+struct Traits<KDL::Wrench> {
+ typedef KDL::Wrench valueType;
+ typedef KDL::Wrench derivType;
+};
+
+template <>
+struct Traits<KDL::Rotation> {
+ typedef KDL::Rotation valueType;
+ typedef KDL::Vector derivType;
+};
+
+template <>
+struct Traits<KDL::Vector> {
+ typedef KDL::Vector valueType;
+ typedef KDL::Vector derivType;
+};
+
+template <>
+struct Traits<double> {
+ typedef double valueType;
+ typedef double derivType;
+};
+
+template <>
+struct Traits<float> {
+ typedef float valueType;
+ typedef float derivType;
+};
+
+template <>
+struct Traits<KDL::FrameVel> {
+ typedef KDL::Frame valueType;
+ typedef KDL::TwistVel derivType;
+};
+template <>
+struct Traits<KDL::TwistVel> {
+ typedef KDL::Twist valueType;
+ typedef KDL::TwistVel derivType;
+};
+
+template <>
+struct Traits<KDL::RotationVel> {
+ typedef KDL::Rotation valueType;
+ typedef KDL::VectorVel derivType;
+};
+
+template <>
+struct Traits<KDL::VectorVel> {
+ typedef KDL::Vector valueType;
+ typedef KDL::VectorVel derivType;
+};
+
+
+
+#endif
+
+
+
diff --git a/intern/itasc/kdl/utilities/utility.cpp b/intern/itasc/kdl/utilities/utility.cpp
new file mode 100644
index 00000000000..1ab9cb6f83d
--- /dev/null
+++ b/intern/itasc/kdl/utilities/utility.cpp
@@ -0,0 +1,21 @@
+/** @file utility.cpp
+ * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ * @version
+ * ORO_Geometry V0.2
+ *
+ * @par history
+ * - changed layout of the comments to accomodate doxygen
+ */
+
+#include "utility.h"
+
+namespace KDL {
+
+int STREAMBUFFERSIZE=10000;
+int MAXLENFILENAME = 255;
+const double PI= 3.1415926535897932384626433832795;
+const double deg2rad = 0.01745329251994329576923690768488;
+const double rad2deg = 57.2957795130823208767981548141052;
+double epsilon = 0.000001;
+double epsilon2 = 0.000001*0.000001;
+}
diff --git a/intern/itasc/kdl/utilities/utility.h b/intern/itasc/kdl/utilities/utility.h
new file mode 100644
index 00000000000..e7dcc55d8a8
--- /dev/null
+++ b/intern/itasc/kdl/utilities/utility.h
@@ -0,0 +1,298 @@
+/*****************************************************************************
+ * \author
+ * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par History
+ * - $log$
+ *
+ * \par Release
+ * $Id: utility.h 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ * \file
+ * Included by most lrl-files to provide some general
+ * functions and macro definitions.
+ *
+ * \par history
+ * - changed layout of the comments to accomodate doxygen
+ */
+
+
+#ifndef KDL_UTILITY_H
+#define KDL_UTILITY_H
+
+#include "kdl-config.h"
+#include <cstdlib>
+#include <cassert>
+#include <cmath>
+
+
+/////////////////////////////////////////////////////////////
+// configurable options for the frames library.
+
+#ifdef KDL_INLINE
+ #ifdef _MSC_VER
+ // Microsoft Visual C
+ #define IMETHOD __forceinline
+ #else
+ // Some other compiler, e.g. gcc
+ #define IMETHOD inline
+ #endif
+#else
+ #define IMETHOD
+#endif
+
+
+
+//! turn on or off frames bounds checking. If turned on, assert() can still
+//! be turned off with -DNDEBUG.
+#ifdef KDL_INDEX_CHECK
+ #define FRAMES_CHECKI(a) assert(a)
+#else
+ #define FRAMES_CHECKI(a)
+#endif
+
+
+namespace KDL {
+
+#ifdef __GNUC__
+ // so that sin,cos can be overloaded and complete
+ // resolution of overloaded functions work.
+ using ::sin;
+ using ::cos;
+ using ::exp;
+ using ::log;
+ using ::sin;
+ using ::cos;
+ using ::tan;
+ using ::sinh;
+ using ::cosh;
+ using ::pow;
+ using ::sqrt;
+ using ::atan;
+ using ::hypot;
+ using ::asin;
+ using ::acos;
+ using ::tanh;
+ using ::atan2;
+#endif
+#ifndef __GNUC__
+ //only real solution : get Rall1d and varia out of namespaces.
+ #pragma warning (disable:4786)
+
+ inline double sin(double a) {
+ return ::sin(a);
+ }
+
+ inline double cos(double a) {
+ return ::cos(a);
+ }
+ inline double exp(double a) {
+ return ::exp(a);
+ }
+ inline double log(double a) {
+ return ::log(a);
+ }
+ inline double tan(double a) {
+ return ::tan(a);
+ }
+ inline double cosh(double a) {
+ return ::cosh(a);
+ }
+ inline double sinh(double a) {
+ return ::sinh(a);
+ }
+ inline double sqrt(double a) {
+ return ::sqrt(a);
+ }
+ inline double atan(double a) {
+ return ::atan(a);
+ }
+ inline double acos(double a) {
+ return ::acos(a);
+ }
+ inline double asin(double a) {
+ return ::asin(a);
+ }
+ inline double tanh(double a) {
+ return ::tanh(a);
+ }
+ inline double pow(double a,double b) {
+ return ::pow(a,b);
+ }
+ inline double atan2(double a,double b) {
+ return ::atan2(a,b);
+ }
+#endif
+
+
+
+
+
+/**
+ * Auxiliary class for argument types (Trait-template class )
+ *
+ * Is used to pass doubles by value, and arbitrary objects by const reference.
+ * This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning
+ * the assignment of the result of intrinsic functions to const double&-typed variables,
+ * and optimization on.
+ */
+template <class T>
+class TI
+{
+ public:
+ typedef const T& Arg; //!< Arg is used for passing the element to a function.
+};
+
+template <>
+class TI<double> {
+public:
+ typedef double Arg;
+};
+
+template <>
+class TI<int> {
+public:
+ typedef int Arg;
+};
+
+
+
+
+
+/**
+ * /note linkage
+ * Something fishy about the difference between C++ and C
+ * in C++ const values default to INTERNAL linkage, in C they default
+ * to EXTERNAL linkage. Here the constants should have EXTERNAL linkage
+ * because they, for at least some of them, can be changed by the user.
+ * If you want to explicitly declare internal linkage, use "static".
+ */
+//!
+extern int STREAMBUFFERSIZE;
+
+//! maximal length of a file name
+extern int MAXLENFILENAME;
+
+//! the value of pi
+extern const double PI;
+
+//! the value pi/180
+extern const double deg2rad;
+
+//! the value 180/pi
+extern const double rad2deg;
+
+//! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
+extern double epsilon;
+
+//! power or 2 of epsilon
+extern double epsilon2;
+
+//! the number of derivatives used in the RN-... objects.
+extern int VSIZE;
+
+
+
+#ifndef _MFC_VER
+#undef max
+inline double max(double a,double b) {
+ if (b<a)
+ return a;
+ else
+ return b;
+}
+
+#undef min
+inline double min(double a,double b) {
+ if (b<a)
+ return b;
+ else
+ return a;
+}
+#endif
+
+
+#ifdef _MSC_VER
+ //#pragma inline_depth( 255 )
+ //#pragma inline_recursion( on )
+ #define INLINE __forceinline
+ //#define INLINE inline
+#else
+ #define INLINE inline
+#endif
+
+
+inline double LinComb(double alfa,double a,
+ double beta,double b ) {
+ return alfa*a+beta*b;
+}
+
+inline void LinCombR(double alfa,double a,
+ double beta,double b,double& result ) {
+ result=alfa*a+beta*b;
+ }
+
+//! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes
+inline void SetToZero(double& arg) {
+ arg=0;
+}
+
+//! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes
+inline void SetToIdentity(double& arg) {
+ arg=1;
+}
+
+inline double sign(double arg) {
+ return (arg<0)?(-1):(1);
+}
+
+inline double sqr(double arg) { return arg*arg;}
+inline double Norm(double arg) {
+ return fabs( (double)arg );
+}
+
+#ifdef __WIN32__
+inline double hypot(double y,double x) { return ::_hypot(y,x);}
+inline double abs(double x) { return ::fabs(x);}
+#endif
+
+// compares whether 2 doubles are equal in an eps-interval.
+// Does not check whether a or b represents numbers
+// On VC6, if a/b is -INF, it returns false;
+inline bool Equal(double a,double b,double eps=epsilon)
+{
+ double tmp=(a-b);
+ return ((eps>tmp)&& (tmp>-eps) );
+}
+
+inline void random(double& a) {
+ a = 1.98*rand()/(double)RAND_MAX -0.99;
+}
+
+inline void posrandom(double& a) {
+ a = 0.001+0.99*rand()/(double)RAND_MAX;
+}
+
+inline double diff(double a,double b,double dt) {
+ return (b-a)/dt;
+}
+//inline float diff(float a,float b,double dt) {
+//return (b-a)/dt;
+//}
+inline double addDelta(double a,double da,double dt) {
+ return a+da*dt;
+}
+
+//inline float addDelta(float a,float da,double dt) {
+// return a+da*dt;
+//}
+
+
+}
+
+
+
+#endif
diff --git a/intern/itasc/kdl/utilities/utility_io.cpp b/intern/itasc/kdl/utilities/utility_io.cpp
new file mode 100644
index 00000000000..ae65047fdbc
--- /dev/null
+++ b/intern/itasc/kdl/utilities/utility_io.cpp
@@ -0,0 +1,208 @@
+/*****************************************************************************
+ * \author
+ * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par History
+ * - $log$
+ *
+ * \par Release
+ * $Id: utility_io.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ * \todo
+ * make IO routines more robust against the differences between DOS/UNIX end-of-line style.
+ ****************************************************************************/
+
+
+#include "utility_io.h"
+#include "error.h"
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <string.h>
+
+namespace KDL {
+
+//
+// _functions are private functions
+//
+
+ void _check_istream(std::istream& is)
+ {
+ if ((!is.good())&&(is.eof()) )
+ {
+ throw Error_BasicIO_File();
+ }
+ }
+// Eats until the end of the line
+ int _EatUntilEndOfLine( std::istream& is, int* countp=NULL) {
+ int ch;
+ int count;
+ count = 0;
+ do {
+ ch = is.get();
+ count++;
+ _check_istream(is);
+ } while (ch!='\n');
+ if (countp!=NULL) *countp = count;
+ return ch;
+}
+
+// Eats until the end of the comment
+ int _EatUntilEndOfComment( std::istream& is, int* countp=NULL) {
+ int ch;
+ int count;
+ count = 0;
+ int prevch;
+ ch = 0;
+ do {
+ prevch = ch;
+ ch = is.get();
+ count++;
+ _check_istream(is);
+ if ((prevch=='*')&&(ch=='/')) {
+ break;
+ }
+ } while (true);
+ if (countp!=NULL) *countp = count;
+ ch = is.get();
+ return ch;
+}
+
+// Eats space-like characters and comments
+// possibly returns the number of space-like characters eaten.
+int _EatSpace( std::istream& is,int* countp=NULL) {
+ int ch;
+ int count;
+ count=-1;
+ do {
+ _check_istream(is);
+
+ ch = is.get();
+ count++;
+ if (ch == '#') {
+ ch = _EatUntilEndOfLine(is,&count);
+ }
+ if (ch == '/') {
+ ch = is.get();
+ if (ch == '/') {
+ ch = _EatUntilEndOfLine(is,&count);
+ } else if (ch == '*') {
+ ch = _EatUntilEndOfComment(is,&count);
+ } else {
+ is.putback(ch);
+ ch = '/';
+ }
+ }
+ } while ((ch==' ')||(ch=='\n')||(ch=='\t'));
+ if (countp!=NULL) *countp = count;
+ return ch;
+}
+
+
+
+// Eats whites, returns, tabs and the delim character
+// Checks wether delim char. is encountered.
+void Eat( std::istream& is, int delim )
+{
+ int ch;
+ ch=_EatSpace(is);
+ if (ch != delim) {
+ throw Error_BasicIO_Exp_Delim();
+ }
+ ch=_EatSpace(is);
+ is.putback(ch);
+}
+
+// Eats whites, returns, tabs and the delim character
+// Checks wether delim char. is encountered.
+// EatEnd does not eat all space-like char's at the end.
+void EatEnd( std::istream& is, int delim )
+{
+ int ch;
+ ch=_EatSpace(is);
+ if (ch != delim) {
+ throw Error_BasicIO_Exp_Delim();
+ }
+}
+
+
+
+// For each space in descript, this routine eats whites,tabs, and newlines (at least one)
+// There should be no consecutive spaces in the description.
+// for each letter in descript, its reads the corresponding letter in the output
+// the routine is case insensitive.
+
+
+// Simple routine, enough for our purposes.
+// works with ASCII chars
+inline char Upper(char ch)
+{
+ /*if (('a'<=ch)&&(ch<='z'))
+ return (ch-'a'+'A');
+ else
+ return ch;
+ */
+ return toupper(ch);
+}
+
+void Eat(std::istream& is,const char* descript)
+{
+ // eats whites before word
+ char ch;
+ char chdescr;
+ ch=_EatSpace(is);
+ is.putback(ch);
+ const char* p;
+ p = descript;
+ while ((*p)!=0) {
+ chdescr = (char)Upper(*p);
+ if (chdescr==' ') {
+ int count=0;
+ ch=_EatSpace(is,&count);
+ is.putback(ch);
+ if (count==0) {
+ throw Error_BasicIO_Not_A_Space();
+ }
+ } else {
+ ch=(char)is.get();
+ if (chdescr!=Upper(ch)) {
+ throw Error_BasicIO_Unexpected();
+ }
+ }
+ p++;
+ }
+
+}
+
+
+
+void EatWord(std::istream& is,const char* delim,char* storage,int maxsize)
+{
+ int ch;
+ char* p;
+ int size;
+ // eat white before word
+ ch=_EatSpace(is);
+ p = storage;
+ size=0;
+ int count = 0;
+ while ((count==0)&&(strchr(delim,ch)==NULL)) {
+ *p = (char) toupper(ch);
+ ++p;
+ if (size==maxsize) {
+ throw Error_BasicIO_ToBig();
+ }
+ _check_istream(is);
+ ++size;
+ //ch = is.get();
+ ch =_EatSpace(is,&count);
+ }
+ *p=0;
+ is.putback(ch);
+}
+
+
+}
diff --git a/intern/itasc/kdl/utilities/utility_io.h b/intern/itasc/kdl/utilities/utility_io.h
new file mode 100644
index 00000000000..7c3d4f91296
--- /dev/null
+++ b/intern/itasc/kdl/utilities/utility_io.h
@@ -0,0 +1,79 @@
+/*****************************************************************************
+ * \author
+ * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ * \version
+ * ORO_Geometry V0.2
+ *
+ * \par History
+ * - $log$
+ *
+ * \par Release
+ * $Id: utility_io.h 19905 2009-04-23 13:29:54Z ben2610 $
+ * $Name: $
+ *
+ * \file utility_io.h
+ * Included by most lrl-files to provide some general
+ * functions and macro definitions related to file/stream I/O.
+ */
+
+#ifndef KDL_UTILITY_IO_H_84822
+#define KDL_UTILITY_IO_H_84822
+
+//#include <kdl/kdl-config.h>
+
+
+// Standard includes
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+
+
+namespace KDL {
+
+
+/**
+ * checks validity of basic io of is
+ */
+void _check_istream(std::istream& is);
+
+
+/**
+ * Eats characters of the stream until the character delim is encountered
+ * @param is a stream
+ * @param delim eat until this character is encountered
+ */
+void Eat(std::istream& is, int delim );
+
+/**
+ * Eats characters of the stream as long as they satisfy the description in descript
+ * @param is a stream
+ * @param descript description string. A sequence of spaces, tabs,
+ * new-lines and comments is regarded as 1 space in the description string.
+ */
+void Eat(std::istream& is,const char* descript);
+
+/**
+ * Eats a word of the stream delimited by the letters in delim or space(tabs...)
+ * @param is a stream
+ * @param delim a string containing the delimmiting characters
+ * @param storage for returning the word
+ * @param maxsize a word can be maximally maxsize-1 long.
+ */
+void EatWord(std::istream& is,const char* delim,char* storage,int maxsize);
+
+/**
+ * Eats characters of the stream until the character delim is encountered
+ * similar to Eat(is,delim) but spaces at the end are not read.
+ * @param is a stream
+ * @param delim eat until this character is encountered
+ */
+void EatEnd( std::istream& is, int delim );
+
+
+
+
+}
+
+
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-11 07:43:49 +0300