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.

386 files changed, 54369 insertions, 1008 deletions

diff --git a/extern/Eigen2/Eigen/Array b/extern/Eigen2/Eigen/Array
new file mode 100644
index 00000000000..c847f9521fe
--- /dev/null
+++ b/extern/Eigen2/Eigen/Array
@@ -0,0 +1,39 @@
+#ifndef EIGEN_ARRAY_MODULE_H
+#define EIGEN_ARRAY_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+namespace Eigen {
+
+/** \defgroup Array_Module Array module
+  * This module provides several handy features to manipulate matrices as simple array of values.
+  * In addition to listed classes, it defines various methods of the Cwise interface
+  * (accessible from MatrixBase::cwise()), including:
+  *  - matrix-scalar sum,
+  *  - coeff-wise comparison operators,
+  *  - sin, cos, sqrt, pow, exp, log, square, cube, inverse (reciprocal).
+  *
+  * This module also provides various MatrixBase methods, including:
+  *  - \ref MatrixBase::all() "all", \ref MatrixBase::any() "any",
+  *  - \ref MatrixBase::Random() "random matrix initialization"
+  *
+  * \code
+  * #include <Eigen/Array>
+  * \endcode
+  */
+
+#include "src/Array/CwiseOperators.h"
+#include "src/Array/Functors.h"
+#include "src/Array/BooleanRedux.h"
+#include "src/Array/Select.h"
+#include "src/Array/PartialRedux.h"
+#include "src/Array/Random.h"
+#include "src/Array/Norms.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_ARRAY_MODULE_H
diff --git a/extern/Eigen2/Eigen/Cholesky b/extern/Eigen2/Eigen/Cholesky
new file mode 100644
index 00000000000..f1806f726c7
--- /dev/null
+++ b/extern/Eigen2/Eigen/Cholesky
@@ -0,0 +1,65 @@
+#ifndef EIGEN_CHOLESKY_MODULE_H
+#define EIGEN_CHOLESKY_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
+#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
+  #ifndef EIGEN_HIDE_HEAVY_CODE
+  #define EIGEN_HIDE_HEAVY_CODE
+  #endif
+#elif defined EIGEN_HIDE_HEAVY_CODE
+  #undef EIGEN_HIDE_HEAVY_CODE
+#endif
+
+namespace Eigen {
+
+/** \defgroup Cholesky_Module Cholesky module
+  *
+  * \nonstableyet
+  *
+  * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
+  * Those decompositions are accessible via the following MatrixBase methods:
+  *  - MatrixBase::llt(),
+  *  - MatrixBase::ldlt()
+  *
+  * \code
+  * #include <Eigen/Cholesky>
+  * \endcode
+  */
+
+#include "src/Array/CwiseOperators.h"
+#include "src/Array/Functors.h"
+#include "src/Cholesky/LLT.h"
+#include "src/Cholesky/LDLT.h"
+
+} // namespace Eigen
+
+#define EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MATRIXTYPE,PREFIX) \
+  PREFIX template class LLT<MATRIXTYPE>; \
+  PREFIX template class LDLT<MATRIXTYPE>
+
+#define EIGEN_CHOLESKY_MODULE_INSTANTIATE(PREFIX) \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2f,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2d,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3f,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3d,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4f,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4d,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXf,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXd,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcf,PREFIX); \
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcd,PREFIX)
+
+#ifdef EIGEN_EXTERN_INSTANTIATIONS
+
+namespace Eigen {
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE(extern);
+} // namespace Eigen
+#endif
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_CHOLESKY_MODULE_H
diff --git a/extern/Eigen2/Eigen/Core b/extern/Eigen2/Eigen/Core
new file mode 100644
index 00000000000..f5e315a2c9d
--- /dev/null
+++ b/extern/Eigen2/Eigen/Core
@@ -0,0 +1,154 @@
+#ifndef EIGEN_CORE_H
+#define EIGEN_CORE_H
+
+// first thing Eigen does: prevent MSVC from committing suicide
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+#ifdef _MSC_VER
+  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
+  #if (_MSC_VER >= 1500) // 2008 or later
+    // Remember that usage of defined() in a #define is undefined by the standard.
+    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
+    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64)
+      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
+    #endif   
+  #endif
+#endif
+
+#ifdef __GNUC__
+  #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__>=x && __GNUC_MINOR__>=y) || __GNUC__>x)
+#else
+  #define EIGEN_GNUC_AT_LEAST(x,y) 0
+#endif
+
+// Remember that usage of defined() in a #define is undefined by the standard
+#if (defined __SSE2__) && ( (!defined __GNUC__) || EIGEN_GNUC_AT_LEAST(4,2) )
+  #define EIGEN_SSE2_BUT_NOT_OLD_GCC
+#endif
+
+#ifndef EIGEN_DONT_VECTORIZE
+  #if defined (EIGEN_SSE2_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_SSE
+    #include <emmintrin.h>
+    #include <xmmintrin.h>
+    #ifdef __SSE3__
+      #include <pmmintrin.h>
+    #endif
+    #ifdef __SSSE3__
+      #include <tmmintrin.h>
+    #endif
+  #elif defined __ALTIVEC__
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_ALTIVEC
+    #include <altivec.h>
+    // We need to #undef all these ugly tokens defined in <altivec.h>
+    // => use __vector instead of vector
+    #undef bool
+    #undef vector
+    #undef pixel
+  #endif
+#endif
+
+#include <cstdlib>
+#include <cmath>
+#include <complex>
+#include <cassert>
+#include <functional>
+#include <iostream>
+#include <cstring>
+#include <string>
+#include <limits>
+
+#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_NO_EXCEPTIONS)
+  #define EIGEN_EXCEPTIONS
+#endif
+
+#ifdef EIGEN_EXCEPTIONS
+  #include <new>
+#endif
+
+// this needs to be done after all possible windows C header includes and before any Eigen source includes
+// (system C++ includes are supposed to be able to deal with this already):
+// windows.h defines min and max macros which would make Eigen fail to compile.
+#if defined(min) || defined(max)
+#error The preprocessor symbols 'min' or 'max' are defined. If you are compiling on Windows, do #define NOMINMAX to prevent windows.h from defining these symbols.
+#endif
+
+namespace Eigen {
+
+/** \defgroup Core_Module Core module
+  * This is the main module of Eigen providing dense matrix and vector support
+  * (both fixed and dynamic size) with all the features corresponding to a BLAS library
+  * and much more...
+  *
+  * \code
+  * #include <Eigen/Core>
+  * \endcode
+  */
+
+#include "src/Core/util/Macros.h"
+#include "src/Core/util/Constants.h"
+#include "src/Core/util/ForwardDeclarations.h"
+#include "src/Core/util/Meta.h"
+#include "src/Core/util/XprHelper.h"
+#include "src/Core/util/StaticAssert.h"
+#include "src/Core/util/Memory.h"
+
+#include "src/Core/NumTraits.h"
+#include "src/Core/MathFunctions.h"
+#include "src/Core/GenericPacketMath.h"
+
+#if defined EIGEN_VECTORIZE_SSE
+  #include "src/Core/arch/SSE/PacketMath.h"
+#elif defined EIGEN_VECTORIZE_ALTIVEC
+  #include "src/Core/arch/AltiVec/PacketMath.h"
+#endif
+
+#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 16
+#endif
+
+#include "src/Core/Functors.h"
+#include "src/Core/MatrixBase.h"
+#include "src/Core/Coeffs.h"
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
+                                // at least confirmed with Doxygen 1.5.5 and 1.5.6
+  #include "src/Core/Assign.h"
+#endif
+
+#include "src/Core/MatrixStorage.h"
+#include "src/Core/NestByValue.h"
+#include "src/Core/Flagged.h"
+#include "src/Core/Matrix.h"
+#include "src/Core/Cwise.h"
+#include "src/Core/CwiseBinaryOp.h"
+#include "src/Core/CwiseUnaryOp.h"
+#include "src/Core/CwiseNullaryOp.h"
+#include "src/Core/Dot.h"
+#include "src/Core/Product.h"
+#include "src/Core/DiagonalProduct.h"
+#include "src/Core/SolveTriangular.h"
+#include "src/Core/MapBase.h"
+#include "src/Core/Map.h"
+#include "src/Core/Block.h"
+#include "src/Core/Minor.h"
+#include "src/Core/Transpose.h"
+#include "src/Core/DiagonalMatrix.h"
+#include "src/Core/DiagonalCoeffs.h"
+#include "src/Core/Sum.h"
+#include "src/Core/Redux.h"
+#include "src/Core/Visitor.h"
+#include "src/Core/Fuzzy.h"
+#include "src/Core/IO.h"
+#include "src/Core/Swap.h"
+#include "src/Core/CommaInitializer.h"
+#include "src/Core/Part.h"
+#include "src/Core/CacheFriendlyProduct.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_CORE_H
diff --git a/extern/Eigen2/Eigen/Dense b/extern/Eigen2/Eigen/Dense
new file mode 100644
index 00000000000..9655edcd7aa
--- /dev/null
+++ b/extern/Eigen2/Eigen/Dense
@@ -0,0 +1,8 @@
+#include "Core"
+#include "Array"
+#include "LU"
+#include "Cholesky"
+#include "QR"
+#include "SVD"
+#include "Geometry"
+#include "LeastSquares"
diff --git a/extern/Eigen2/Eigen/Eigen b/extern/Eigen2/Eigen/Eigen
new file mode 100644
index 00000000000..654c8dc6380
--- /dev/null
+++ b/extern/Eigen2/Eigen/Eigen
@@ -0,0 +1,2 @@
+#include "Dense"
+#include "Sparse"
diff --git a/extern/Eigen2/Eigen/Geometry b/extern/Eigen2/Eigen/Geometry
new file mode 100644
index 00000000000..617b25eb6f5
--- /dev/null
+++ b/extern/Eigen2/Eigen/Geometry
@@ -0,0 +1,51 @@
+#ifndef EIGEN_GEOMETRY_MODULE_H
+#define EIGEN_GEOMETRY_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+#include "Array"
+#include <limits>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace Eigen {
+
+/** \defgroup Geometry_Module Geometry module
+  *
+  * \nonstableyet
+  *
+  * This module provides support for:
+  *  - fixed-size homogeneous transformations
+  *  - translation, scaling, 2D and 3D rotations
+  *  - quaternions
+  *  - \ref MatrixBase::cross() "cross product"
+  *  - \ref MatrixBase::unitOrthogonal() "orthognal vector generation"
+  *  - some linear components: parametrized-lines and hyperplanes
+  *
+  * \code
+  * #include <Eigen/Geometry>
+  * \endcode
+  */
+
+#include "src/Geometry/OrthoMethods.h"
+#include "src/Geometry/RotationBase.h"
+#include "src/Geometry/Rotation2D.h"
+#include "src/Geometry/Quaternion.h"
+#include "src/Geometry/AngleAxis.h"
+#include "src/Geometry/EulerAngles.h"
+#include "src/Geometry/Transform.h"
+#include "src/Geometry/Translation.h"
+#include "src/Geometry/Scaling.h"
+#include "src/Geometry/Hyperplane.h"
+#include "src/Geometry/ParametrizedLine.h"
+#include "src/Geometry/AlignedBox.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_GEOMETRY_MODULE_H
diff --git a/extern/Eigen2/Eigen/LU b/extern/Eigen2/Eigen/LU
new file mode 100644
index 00000000000..0ce69456598
--- /dev/null
+++ b/extern/Eigen2/Eigen/LU
@@ -0,0 +1,29 @@
+#ifndef EIGEN_LU_MODULE_H
+#define EIGEN_LU_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+namespace Eigen {
+
+/** \defgroup LU_Module LU module
+  * This module includes %LU decomposition and related notions such as matrix inversion and determinant.
+  * This module defines the following MatrixBase methods:
+  *  - MatrixBase::inverse()
+  *  - MatrixBase::determinant()
+  *
+  * \code
+  * #include <Eigen/LU>
+  * \endcode
+  */
+
+#include "src/LU/LU.h"
+#include "src/LU/Determinant.h"
+#include "src/LU/Inverse.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_LU_MODULE_H
diff --git a/extern/Eigen2/Eigen/LeastSquares b/extern/Eigen2/Eigen/LeastSquares
new file mode 100644
index 00000000000..573a13cb42f
--- /dev/null
+++ b/extern/Eigen2/Eigen/LeastSquares
@@ -0,0 +1,27 @@
+#ifndef EIGEN_REGRESSION_MODULE_H
+#define EIGEN_REGRESSION_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+#include "QR"
+#include "Geometry"
+
+namespace Eigen {
+
+/** \defgroup LeastSquares_Module LeastSquares module
+  * This module provides linear regression and related features.
+  *
+  * \code
+  * #include <Eigen/LeastSquares>
+  * \endcode
+  */
+
+#include "src/LeastSquares/LeastSquares.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_REGRESSION_MODULE_H
diff --git a/extern/Eigen2/Eigen/NewStdVector b/extern/Eigen2/Eigen/NewStdVector
new file mode 100644
index 00000000000..f37de5ff673
--- /dev/null
+++ b/extern/Eigen2/Eigen/NewStdVector
@@ -0,0 +1,168 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_STDVECTOR_MODULE_H
+#define EIGEN_STDVECTOR_MODULE_H
+
+#include "Core"
+#include <vector>
+
+namespace Eigen {
+
+// This one is needed to prevent reimplementing the whole std::vector.
+template <class T>
+class aligned_allocator_indirection : public aligned_allocator<T>
+{
+public:
+  typedef size_t    size_type;
+  typedef ptrdiff_t difference_type;
+  typedef T*        pointer;
+  typedef const T*  const_pointer;
+  typedef T&        reference;
+  typedef const T&  const_reference;
+  typedef T         value_type;
+
+  template<class U>
+  struct rebind
+  {
+    typedef aligned_allocator_indirection<U> other;
+  };
+
+  aligned_allocator_indirection() throw() {}
+  aligned_allocator_indirection(const aligned_allocator_indirection& ) throw() : aligned_allocator<T>() {}
+  aligned_allocator_indirection(const aligned_allocator<T>& ) throw() {}
+  template<class U>
+  aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) throw() {}
+  template<class U>
+  aligned_allocator_indirection(const aligned_allocator<U>& ) throw() {}
+  ~aligned_allocator_indirection() throw() {}
+};
+
+#ifdef _MSC_VER
+
+  // sometimes, MSVC detects, at compile time, that the argument x
+  // in std::vector::resize(size_t s,T x) won't be aligned and generate an error
+  // even if this function is never called. Whence this little wrapper.
+  #define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) Eigen::ei_workaround_msvc_std_vector<T>
+  template<typename T> struct ei_workaround_msvc_std_vector : public T
+  {
+    inline ei_workaround_msvc_std_vector() : T() {}
+    inline ei_workaround_msvc_std_vector(const T& other) : T(other) {}
+    inline operator T& () { return *static_cast<T*>(this); }
+    inline operator const T& () const { return *static_cast<const T*>(this); }
+    template<typename OtherT>
+    inline T& operator=(const OtherT& other)
+    { T::operator=(other); return *this; }
+    inline ei_workaround_msvc_std_vector& operator=(const ei_workaround_msvc_std_vector& other)
+    { T::operator=(other); return *this; }
+  };
+
+#else
+
+  #define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) T
+
+#endif
+
+}
+
+namespace std {
+
+#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
+  public:  \
+    typedef T value_type; \
+    typedef typename vector_base::allocator_type allocator_type; \
+    typedef typename vector_base::size_type size_type;  \
+    typedef typename vector_base::iterator iterator;  \
+    typedef typename vector_base::const_iterator const_iterator;  \
+    explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {}  \
+    template<typename InputIterator> \
+    vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
+    : vector_base(first, last, a) {} \
+    vector(const vector& c) : vector_base(c) {}  \
+    explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
+    vector(iterator start, iterator end) : vector_base(start, end) {}  \
+    vector& operator=(const vector& x) {  \
+      vector_base::operator=(x);  \
+      return *this;  \
+    }
+
+template<typename T>
+class vector<T,Eigen::aligned_allocator<T> >
+  : public vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
+                  Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> >
+{
+  typedef vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
+                 Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> > vector_base;
+  EIGEN_STD_VECTOR_SPECIALIZATION_BODY
+
+  void resize(size_type new_size)
+  { resize(new_size, T()); }
+
+#if defined(_VECTOR_)
+  // workaround MSVC std::vector implementation
+  void resize(size_type new_size, const value_type& x)
+  {
+    if (vector_base::size() < new_size)
+      vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
+    else if (new_size < vector_base::size())
+      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
+  }
+  void push_back(const value_type& x)
+  { vector_base::push_back(x); } 
+  using vector_base::insert;  
+  iterator insert(const_iterator position, const value_type& x)
+  { return vector_base::insert(position,x); }
+  void insert(const_iterator position, size_type new_size, const value_type& x)
+  { vector_base::insert(position, new_size, x); }
+#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
+  // workaround GCC std::vector implementation
+  void resize(size_type new_size, const value_type& x)
+  {
+    if (new_size < vector_base::size())
+      vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
+    else
+      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
+  }
+#elif defined(_GLIBCXX_VECTOR) && (!EIGEN_GNUC_AT_LEAST(4,1))
+  // Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&),
+  // no no need to workaround !
+  using vector_base::resize;
+#else
+  // either GCC 4.1 or non-GCC
+  // default implementation which should always work.
+  void resize(size_type new_size, const value_type& x)
+  {
+    if (new_size < vector_base::size())
+      vector_base::erase(vector_base::begin() + new_size, vector_base::end());
+    else if (new_size > vector_base::size())
+      vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
+  }
+#endif
+
+};
+
+}
+
+#endif // EIGEN_STDVECTOR_MODULE_H
diff --git a/extern/Eigen2/Eigen/QR b/extern/Eigen2/Eigen/QR
new file mode 100644
index 00000000000..97907d1e50f
--- /dev/null
+++ b/extern/Eigen2/Eigen/QR
@@ -0,0 +1,73 @@
+#ifndef EIGEN_QR_MODULE_H
+#define EIGEN_QR_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+#include "Cholesky"
+
+// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
+#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
+  #ifndef EIGEN_HIDE_HEAVY_CODE
+  #define EIGEN_HIDE_HEAVY_CODE
+  #endif
+#elif defined EIGEN_HIDE_HEAVY_CODE
+  #undef EIGEN_HIDE_HEAVY_CODE
+#endif
+
+namespace Eigen {
+
+/** \defgroup QR_Module QR module
+  *
+  * \nonstableyet
+  *
+  * This module mainly provides QR decomposition and an eigen value solver.
+  * This module also provides some MatrixBase methods, including:
+  *  - MatrixBase::qr(),
+  *  - MatrixBase::eigenvalues(),
+  *  - MatrixBase::operatorNorm()
+  *
+  * \code
+  * #include <Eigen/QR>
+  * \endcode
+  */
+
+#include "src/QR/QR.h"
+#include "src/QR/Tridiagonalization.h"
+#include "src/QR/EigenSolver.h"
+#include "src/QR/SelfAdjointEigenSolver.h"
+#include "src/QR/HessenbergDecomposition.h"
+
+// declare all classes for a given matrix type
+#define EIGEN_QR_MODULE_INSTANTIATE_TYPE(MATRIXTYPE,PREFIX) \
+  PREFIX template class QR<MATRIXTYPE>; \
+  PREFIX template class Tridiagonalization<MATRIXTYPE>; \
+  PREFIX template class HessenbergDecomposition<MATRIXTYPE>; \
+  PREFIX template class SelfAdjointEigenSolver<MATRIXTYPE>
+
+// removed because it does not support complex yet
+//  PREFIX template class EigenSolver<MATRIXTYPE>
+
+// declare all class for all types
+#define EIGEN_QR_MODULE_INSTANTIATE(PREFIX) \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix2f,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix2d,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix3f,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix3d,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix4f,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix4d,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXf,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXd,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXcf,PREFIX); \
+  EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXcd,PREFIX)
+
+#ifdef EIGEN_EXTERN_INSTANTIATIONS
+  EIGEN_QR_MODULE_INSTANTIATE(extern);
+#endif // EIGEN_EXTERN_INSTANTIATIONS
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_QR_MODULE_H
diff --git a/extern/Eigen2/Eigen/QtAlignedMalloc b/extern/Eigen2/Eigen/QtAlignedMalloc
new file mode 100644
index 00000000000..fde227328fa
--- /dev/null
+++ b/extern/Eigen2/Eigen/QtAlignedMalloc
@@ -0,0 +1,29 @@
+
+#ifndef EIGEN_QTMALLOC_MODULE_H
+#define EIGEN_QTMALLOC_MODULE_H
+
+#include "Core"
+
+#if (!EIGEN_MALLOC_ALREADY_ALIGNED)
+
+inline void *qMalloc(size_t size)
+{
+  return Eigen::ei_aligned_malloc(size);
+}
+
+inline void qFree(void *ptr)
+{
+  Eigen::ei_aligned_free(ptr);
+}
+
+inline void *qRealloc(void *ptr, size_t size)
+{
+  void* newPtr = Eigen::ei_aligned_malloc(size);
+  memcpy(newPtr, ptr, size);
+  Eigen::ei_aligned_free(ptr);
+  return newPtr;
+}
+
+#endif
+
+#endif // EIGEN_QTMALLOC_MODULE_H
diff --git a/extern/Eigen2/Eigen/SVD b/extern/Eigen2/Eigen/SVD
new file mode 100644
index 00000000000..eef05564bde
--- /dev/null
+++ b/extern/Eigen2/Eigen/SVD
@@ -0,0 +1,29 @@
+#ifndef EIGEN_SVD_MODULE_H
+#define EIGEN_SVD_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+namespace Eigen {
+
+/** \defgroup SVD_Module SVD module
+  *
+  * \nonstableyet
+  *
+  * This module provides SVD decomposition for (currently) real matrices.
+  * This decomposition is accessible via the following MatrixBase method:
+  *  - MatrixBase::svd()
+  *
+  * \code
+  * #include <Eigen/SVD>
+  * \endcode
+  */
+
+#include "src/SVD/SVD.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_SVD_MODULE_H
diff --git a/extern/Eigen2/Eigen/Sparse b/extern/Eigen2/Eigen/Sparse
new file mode 100644
index 00000000000..536c284549b
--- /dev/null
+++ b/extern/Eigen2/Eigen/Sparse
@@ -0,0 +1,132 @@
+#ifndef EIGEN_SPARSE_MODULE_H
+#define EIGEN_SPARSE_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <algorithm>
+
+#ifdef EIGEN_GOOGLEHASH_SUPPORT
+  #include <google/dense_hash_map>
+#endif
+
+#ifdef EIGEN_CHOLMOD_SUPPORT
+  extern "C" {
+    #include "cholmod.h"
+  }
+#endif
+
+#ifdef EIGEN_TAUCS_SUPPORT
+  // taucs.h declares a lot of mess
+  #define isnan
+  #define finite
+  #define isinf
+  extern "C" {
+    #include "taucs.h"
+  }
+  #undef isnan
+  #undef finite
+  #undef isinf
+
+  #ifdef min
+    #undef min
+  #endif
+  #ifdef max
+    #undef max
+  #endif
+  #ifdef complex
+    #undef complex
+  #endif
+#endif
+
+#ifdef EIGEN_SUPERLU_SUPPORT
+  typedef int int_t;
+  #include "superlu/slu_Cnames.h"
+  #include "superlu/supermatrix.h"
+  #include "superlu/slu_util.h"
+
+  namespace SuperLU_S {
+  #include "superlu/slu_sdefs.h"
+  }
+  namespace SuperLU_D {
+  #include "superlu/slu_ddefs.h"
+  }
+  namespace SuperLU_C {
+  #include "superlu/slu_cdefs.h"
+  }
+  namespace SuperLU_Z {
+  #include "superlu/slu_zdefs.h"
+  }
+  namespace Eigen { struct SluMatrix; }
+#endif
+
+#ifdef EIGEN_UMFPACK_SUPPORT
+  #include "umfpack.h"
+#endif
+
+namespace Eigen {
+
+/** \defgroup Sparse_Module Sparse module
+  *
+  * \nonstableyet
+  *
+  * See the \ref TutorialSparse "Sparse tutorial"
+  *
+  * \code
+  * #include <Eigen/QR>
+  * \endcode
+  */
+
+#include "src/Sparse/SparseUtil.h"
+#include "src/Sparse/SparseMatrixBase.h"
+#include "src/Sparse/CompressedStorage.h"
+#include "src/Sparse/AmbiVector.h"
+#include "src/Sparse/RandomSetter.h"
+#include "src/Sparse/SparseBlock.h"
+#include "src/Sparse/SparseMatrix.h"
+#include "src/Sparse/DynamicSparseMatrix.h"
+#include "src/Sparse/MappedSparseMatrix.h"
+#include "src/Sparse/SparseVector.h"
+#include "src/Sparse/CoreIterators.h"
+#include "src/Sparse/SparseTranspose.h"
+#include "src/Sparse/SparseCwise.h"
+#include "src/Sparse/SparseCwiseUnaryOp.h"
+#include "src/Sparse/SparseCwiseBinaryOp.h"
+#include "src/Sparse/SparseDot.h"
+#include "src/Sparse/SparseAssign.h"
+#include "src/Sparse/SparseRedux.h"
+#include "src/Sparse/SparseFuzzy.h"
+#include "src/Sparse/SparseFlagged.h"
+#include "src/Sparse/SparseProduct.h"
+#include "src/Sparse/SparseDiagonalProduct.h"
+#include "src/Sparse/TriangularSolver.h"
+#include "src/Sparse/SparseLLT.h"
+#include "src/Sparse/SparseLDLT.h"
+#include "src/Sparse/SparseLU.h"
+
+#ifdef EIGEN_CHOLMOD_SUPPORT
+# include "src/Sparse/CholmodSupport.h"
+#endif
+
+#ifdef EIGEN_TAUCS_SUPPORT
+# include "src/Sparse/TaucsSupport.h"
+#endif
+
+#ifdef EIGEN_SUPERLU_SUPPORT
+# include "src/Sparse/SuperLUSupport.h"
+#endif
+
+#ifdef EIGEN_UMFPACK_SUPPORT
+# include "src/Sparse/UmfPackSupport.h"
+#endif
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_SPARSE_MODULE_H
diff --git a/extern/Eigen2/Eigen/StdVector b/extern/Eigen2/Eigen/StdVector
new file mode 100644
index 00000000000..c0744d6a0f3
--- /dev/null
+++ b/extern/Eigen2/Eigen/StdVector
@@ -0,0 +1,147 @@
+#ifdef EIGEN_USE_NEW_STDVECTOR
+#include "NewStdVector"
+#else
+
+#ifndef EIGEN_STDVECTOR_MODULE_H
+#define EIGEN_STDVECTOR_MODULE_H
+
+#if defined(_GLIBCXX_VECTOR) || defined(_VECTOR_)
+#error you must include <Eigen/StdVector> before <vector>. Also note that <Eigen/Sparse> includes <vector>, so it must be included after <Eigen/StdVector> too.
+#endif                                                    
+
+#ifndef EIGEN_GNUC_AT_LEAST
+#ifdef __GNUC__
+  #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__>=x && __GNUC_MINOR__>=y) || __GNUC__>x)
+#else
+  #define EIGEN_GNUC_AT_LEAST(x,y) 0
+#endif
+#endif
+
+#define vector std_vector
+#include <vector>        
+#undef vector
+
+namespace Eigen {
+
+template<typename T> class aligned_allocator;
+
+// meta programming to determine if a class has a given member
+struct ei_does_not_have_aligned_operator_new_marker_sizeof {int a[1];};
+struct ei_has_aligned_operator_new_marker_sizeof {int a[2];};
+
+template<typename ClassType>
+struct ei_has_aligned_operator_new {
+    template<typename T>
+    static ei_has_aligned_operator_new_marker_sizeof
+    test(T const *, typename T::ei_operator_new_marker_type const * = 0);
+    static ei_does_not_have_aligned_operator_new_marker_sizeof
+    test(...);
+
+    // note that the following indirection is needed for gcc-3.3
+    enum {ret =  sizeof(test(static_cast<ClassType*>(0))) 
+              == sizeof(ei_has_aligned_operator_new_marker_sizeof) };
+};
+
+#ifdef _MSC_VER
+  
+  // sometimes, MSVC detects, at compile time, that the argument x
+  // in std::vector::resize(size_t s,T x) won't be aligned and generate an error
+  // even if this function is never called. Whence this little wrapper.
+  #define _EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) Eigen::ei_workaround_msvc_std_vector<T>
+  template<typename T> struct ei_workaround_msvc_std_vector : public T
+  {
+    inline ei_workaround_msvc_std_vector() : T() {}
+    inline ei_workaround_msvc_std_vector(const T& other) : T(other) {}
+    inline operator T& () { return *static_cast<T*>(this); }
+    inline operator const T& () const { return *static_cast<const T*>(this); }
+    template<typename OtherT>
+    inline T& operator=(const OtherT& other)
+    { T::operator=(other); return *this; }
+    inline ei_workaround_msvc_std_vector& operator=(const ei_workaround_msvc_std_vector& other)
+    { T::operator=(other); return *this; }
+  };
+
+#else
+
+  #define _EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) T
+
+#endif
+
+}
+
+namespace std {
+
+#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
+  public:  \
+    typedef T value_type; \
+    typedef typename vector_base::allocator_type allocator_type; \
+    typedef typename vector_base::size_type size_type;  \
+    typedef typename vector_base::iterator iterator;  \
+    explicit vector(const allocator_type& __a = allocator_type()) : vector_base(__a) {}  \
+    vector(const vector& c) : vector_base(c) {}  \
+    vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
+    vector(iterator start, iterator end) : vector_base(start, end) {}  \
+    vector& operator=(const vector& __x) {  \
+      vector_base::operator=(__x);  \
+      return *this;  \
+    }
+
+template<typename T,
+         typename AllocT = std::allocator<T>,
+         bool HasAlignedNew = Eigen::ei_has_aligned_operator_new<T>::ret>
+class vector : public std::std_vector<T,AllocT>
+{
+  typedef std_vector<T, AllocT> vector_base;
+  EIGEN_STD_VECTOR_SPECIALIZATION_BODY
+};
+
+template<typename T,typename DummyAlloc>
+class vector<T,DummyAlloc,true>
+  : public std::std_vector<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
+                           Eigen::aligned_allocator<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> >          
+{
+  typedef std_vector<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
+                     Eigen::aligned_allocator<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> > vector_base;
+  EIGEN_STD_VECTOR_SPECIALIZATION_BODY
+
+  void resize(size_type __new_size)
+  { resize(__new_size, T()); }     
+
+  #if defined(_VECTOR_)
+  // workaround MSVC std::vector implementation
+  void resize(size_type __new_size, const value_type& __x)                 
+  {                                                              
+    if (vector_base::size() < __new_size)                                 
+      vector_base::_Insert_n(vector_base::end(), __new_size - vector_base::size(), __x);
+    else if (__new_size < vector_base::size())
+      vector_base::erase(vector_base::begin() + __new_size, vector_base::end());
+  }
+  #elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
+  // workaround GCC std::vector implementation
+  void resize(size_type __new_size, const value_type& __x)
+  {                                              
+    if (__new_size < vector_base::size())               
+      vector_base::_M_erase_at_end(this->_M_impl._M_start + __new_size);
+    else                       
+      vector_base::insert(vector_base::end(), __new_size - vector_base::size(), __x); 
+  }                                                              
+  #elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,1)
+  void resize(size_type __new_size, const value_type& __x)
+  {
+    if (__new_size < vector_base::size())
+      vector_base::erase(vector_base::begin() + __new_size, vector_base::end());
+    else
+      vector_base::insert(vector_base::end(), __new_size - vector_base::size(), __x);
+  }
+  #else
+  // Before gcc-4.1 we already have: std::vector::resize(size_type,const T&),
+  // so no need for a workaround !
+  using vector_base::resize;
+  #endif  
+};
+
+}
+
+#endif // EIGEN_STDVECTOR_MODULE_H
+
+#endif // EIGEN_USE_NEW_STDVECTOR
\ No newline at end of file
diff --git a/extern/Eigen2/Eigen/src/Array/BooleanRedux.h b/extern/Eigen2/Eigen/src/Array/BooleanRedux.h
new file mode 100644
index 00000000000..4e8218327eb
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/BooleanRedux.h
@@ -0,0 +1,145 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ALLANDANY_H
+#define EIGEN_ALLANDANY_H
+
+template<typename Derived, int UnrollCount>
+struct ei_all_unroller
+{
+  enum {
+    col = (UnrollCount-1) / Derived::RowsAtCompileTime,
+    row = (UnrollCount-1) % Derived::RowsAtCompileTime
+  };
+
+  inline static bool run(const Derived &mat)
+  {
+    return ei_all_unroller<Derived, UnrollCount-1>::run(mat) && mat.coeff(row, col);
+  }
+};
+
+template<typename Derived>
+struct ei_all_unroller<Derived, 1>
+{
+  inline static bool run(const Derived &mat) { return mat.coeff(0, 0); }
+};
+
+template<typename Derived>
+struct ei_all_unroller<Derived, Dynamic>
+{
+  inline static bool run(const Derived &) { return false; }
+};
+
+template<typename Derived, int UnrollCount>
+struct ei_any_unroller
+{
+  enum {
+    col = (UnrollCount-1) / Derived::RowsAtCompileTime,
+    row = (UnrollCount-1) % Derived::RowsAtCompileTime
+  };
+
+  inline static bool run(const Derived &mat)
+  {
+    return ei_any_unroller<Derived, UnrollCount-1>::run(mat) || mat.coeff(row, col);
+  }
+};
+
+template<typename Derived>
+struct ei_any_unroller<Derived, 1>
+{
+  inline static bool run(const Derived &mat) { return mat.coeff(0, 0); }
+};
+
+template<typename Derived>
+struct ei_any_unroller<Derived, Dynamic>
+{
+  inline static bool run(const Derived &) { return false; }
+};
+
+/** \array_module
+  * 
+  * \returns true if all coefficients are true
+  *
+  * \addexample CwiseAll \label How to check whether a point is inside a box (using operator< and all())
+  *
+  * Example: \include MatrixBase_all.cpp
+  * Output: \verbinclude MatrixBase_all.out
+  *
+  * \sa MatrixBase::any(), Cwise::operator<()
+  */
+template<typename Derived>
+inline bool MatrixBase<Derived>::all() const
+{
+  const bool unroll = SizeAtCompileTime * (CoeffReadCost + NumTraits<Scalar>::AddCost)
+                      <= EIGEN_UNROLLING_LIMIT;
+  if(unroll)
+    return ei_all_unroller<Derived,
+                           unroll ? int(SizeAtCompileTime) : Dynamic
+     >::run(derived());
+  else
+  {
+    for(int j = 0; j < cols(); ++j)
+      for(int i = 0; i < rows(); ++i)
+        if (!coeff(i, j)) return false;
+    return true;
+  }
+}
+
+/** \array_module
+  * 
+  * \returns true if at least one coefficient is true
+  *
+  * \sa MatrixBase::all()
+  */
+template<typename Derived>
+inline bool MatrixBase<Derived>::any() const
+{
+  const bool unroll = SizeAtCompileTime * (CoeffReadCost + NumTraits<Scalar>::AddCost)
+                      <= EIGEN_UNROLLING_LIMIT;
+  if(unroll)
+    return ei_any_unroller<Derived,
+                           unroll ? int(SizeAtCompileTime) : Dynamic
+           >::run(derived());
+  else
+  {
+    for(int j = 0; j < cols(); ++j)
+      for(int i = 0; i < rows(); ++i)
+        if (coeff(i, j)) return true;
+    return false;
+  }
+}
+
+/** \array_module
+  * 
+  * \returns the number of coefficients which evaluate to true
+  *
+  * \sa MatrixBase::all(), MatrixBase::any()
+  */
+template<typename Derived>
+inline int MatrixBase<Derived>::count() const
+{
+  return this->cast<bool>().cast<int>().sum();
+}
+
+#endif // EIGEN_ALLANDANY_H
diff --git a/extern/Eigen2/Eigen/src/Array/CwiseOperators.h b/extern/Eigen2/Eigen/src/Array/CwiseOperators.h
new file mode 100644
index 00000000000..4b6346daa51
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/CwiseOperators.h
@@ -0,0 +1,453 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ARRAY_CWISE_OPERATORS_H
+#define EIGEN_ARRAY_CWISE_OPERATORS_H
+
+// -- unary operators --
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise square root of *this.
+  *
+  * Example: \include Cwise_sqrt.cpp
+  * Output: \verbinclude Cwise_sqrt.out
+  *
+  * \sa pow(), square()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sqrt_op)
+Cwise<ExpressionType>::sqrt() const
+{
+  return _expression();
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise exponential of *this.
+  *
+  * Example: \include Cwise_exp.cpp
+  * Output: \verbinclude Cwise_exp.out
+  *
+  * \sa pow(), log(), sin(), cos()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_exp_op)
+Cwise<ExpressionType>::exp() const
+{
+  return _expression();
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise logarithm of *this.
+  *
+  * Example: \include Cwise_log.cpp
+  * Output: \verbinclude Cwise_log.out
+  *
+  * \sa exp()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_log_op)
+Cwise<ExpressionType>::log() const
+{
+  return _expression();
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise cosine of *this.
+  *
+  * Example: \include Cwise_cos.cpp
+  * Output: \verbinclude Cwise_cos.out
+  *
+  * \sa sin(), exp()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cos_op)
+Cwise<ExpressionType>::cos() const
+{
+  return _expression();
+}
+
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise sine of *this.
+  *
+  * Example: \include Cwise_sin.cpp
+  * Output: \verbinclude Cwise_sin.out
+  *
+  * \sa cos(), exp()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sin_op)
+Cwise<ExpressionType>::sin() const
+{
+  return _expression();
+}
+
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise power of *this to the given exponent.
+  *
+  * Example: \include Cwise_pow.cpp
+  * Output: \verbinclude Cwise_pow.out
+  *
+  * \sa exp(), log()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)
+Cwise<ExpressionType>::pow(const Scalar& exponent) const
+{
+  return EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)(_expression(), ei_scalar_pow_op<Scalar>(exponent));
+}
+
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise inverse of *this.
+  *
+  * Example: \include Cwise_inverse.cpp
+  * Output: \verbinclude Cwise_inverse.out
+  *
+  * \sa operator/(), operator*()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_inverse_op)
+Cwise<ExpressionType>::inverse() const
+{
+  return _expression();
+}
+
+/** \array_module
+  *
+  * \returns an expression of the coefficient-wise square of *this.
+  *
+  * Example: \include Cwise_square.cpp
+  * Output: \verbinclude Cwise_square.out
+  *
+  * \sa operator/(), operator*(), abs2()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_square_op)
+Cwise<ExpressionType>::square() const
+{
+  return _expression();
+}
+
+/** \array_module
+  *
+  * \returns an expression of the coefficient-wise cube of *this.
+  *
+  * Example: \include Cwise_cube.cpp
+  * Output: \verbinclude Cwise_cube.out
+  *
+  * \sa square(), pow()
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cube_op)
+Cwise<ExpressionType>::cube() const
+{
+  return _expression();
+}
+
+
+// -- binary operators --
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \< operator of *this and \a other
+  *
+  * Example: \include Cwise_less.cpp
+  * Output: \verbinclude Cwise_less.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), operator>(), operator<=()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)
+Cwise<ExpressionType>::operator<(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)(_expression(), other.derived());
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \<= operator of *this and \a other
+  *
+  * Example: \include Cwise_less_equal.cpp
+  * Output: \verbinclude Cwise_less_equal.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), operator>=(), operator<()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)
+Cwise<ExpressionType>::operator<=(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)(_expression(), other.derived());
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \> operator of *this and \a other
+  *
+  * Example: \include Cwise_greater.cpp
+  * Output: \verbinclude Cwise_greater.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), operator>=(), operator<()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)
+Cwise<ExpressionType>::operator>(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)(_expression(), other.derived());
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \>= operator of *this and \a other
+  *
+  * Example: \include Cwise_greater_equal.cpp
+  * Output: \verbinclude Cwise_greater_equal.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), operator>(), operator<=()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)
+Cwise<ExpressionType>::operator>=(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)(_expression(), other.derived());
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise == operator of *this and \a other
+  *
+  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+  * generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
+  * MatrixBase::isMuchSmallerThan().
+  *
+  * Example: \include Cwise_equal_equal.cpp
+  * Output: \verbinclude Cwise_equal_equal.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), MatrixBase::isApprox(), MatrixBase::isMuchSmallerThan()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)
+Cwise<ExpressionType>::operator==(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)(_expression(), other.derived());
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise != operator of *this and \a other
+  *
+  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+  * generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
+  * MatrixBase::isMuchSmallerThan().
+  *
+  * Example: \include Cwise_not_equal.cpp
+  * Output: \verbinclude Cwise_not_equal.out
+  *
+  * \sa MatrixBase::all(), MatrixBase::any(), MatrixBase::isApprox(), MatrixBase::isMuchSmallerThan()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)
+Cwise<ExpressionType>::operator!=(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)(_expression(), other.derived());
+}
+
+// comparisons to scalar value
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \< operator of *this and a scalar \a s
+  *
+  * \sa operator<(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)
+Cwise<ExpressionType>::operator<(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \<= operator of *this and a scalar \a s
+  *
+  * \sa operator<=(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)
+Cwise<ExpressionType>::operator<=(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \> operator of *this and a scalar \a s
+  *
+  * \sa operator>(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)
+Cwise<ExpressionType>::operator>(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise \>= operator of *this and a scalar \a s
+  *
+  * \sa operator>=(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)
+Cwise<ExpressionType>::operator>=(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise == operator of *this and a scalar \a s
+  *
+  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+  * generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
+  * MatrixBase::isMuchSmallerThan().
+  *
+  * \sa operator==(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)
+Cwise<ExpressionType>::operator==(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+/** \array_module
+  * 
+  * \returns an expression of the coefficient-wise != operator of *this and a scalar \a s
+  *
+  * \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
+  * In order to check for equality between two vectors or matrices with floating-point coefficients, it is
+  * generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
+  * MatrixBase::isMuchSmallerThan().
+  *
+  * \sa operator!=(const MatrixBase<OtherDerived> &) const
+  */
+template<typename ExpressionType>
+inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)
+Cwise<ExpressionType>::operator!=(Scalar s) const
+{
+  return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)(_expression(),
+            typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
+}
+
+// scalar addition
+
+/** \array_module
+  *
+  * \returns an expression of \c *this with each coeff incremented by the constant \a scalar
+  *
+  * Example: \include Cwise_plus.cpp
+  * Output: \verbinclude Cwise_plus.out
+  *
+  * \sa operator+=(), operator-()
+  */
+template<typename ExpressionType>
+inline const typename Cwise<ExpressionType>::ScalarAddReturnType
+Cwise<ExpressionType>::operator+(const Scalar& scalar) const
+{
+  return typename Cwise<ExpressionType>::ScalarAddReturnType(m_matrix, ei_scalar_add_op<Scalar>(scalar));
+}
+
+/** \array_module
+  *
+  * Adds the given \a scalar to each coeff of this expression.
+  *
+  * Example: \include Cwise_plus_equal.cpp
+  * Output: \verbinclude Cwise_plus_equal.out
+  *
+  * \sa operator+(), operator-=()
+  */
+template<typename ExpressionType>
+inline ExpressionType& Cwise<ExpressionType>::operator+=(const Scalar& scalar)
+{
+  return m_matrix.const_cast_derived() = *this + scalar;
+}
+
+/** \array_module
+  *
+  * \returns an expression of \c *this with each coeff decremented by the constant \a scalar
+  *
+  * Example: \include Cwise_minus.cpp
+  * Output: \verbinclude Cwise_minus.out
+  *
+  * \sa operator+(), operator-=()
+  */
+template<typename ExpressionType>
+inline const typename Cwise<ExpressionType>::ScalarAddReturnType
+Cwise<ExpressionType>::operator-(const Scalar& scalar) const
+{
+  return *this + (-scalar);
+}
+
+/** \array_module
+  *
+  * Substracts the given \a scalar from each coeff of this expression.
+  *
+  * Example: \include Cwise_minus_equal.cpp
+  * Output: \verbinclude Cwise_minus_equal.out
+  *
+  * \sa operator+=(), operator-()
+  */
+
+template<typename ExpressionType>
+inline ExpressionType& Cwise<ExpressionType>::operator-=(const Scalar& scalar)
+{
+  return m_matrix.const_cast_derived() = *this - scalar;
+}
+
+#endif // EIGEN_ARRAY_CWISE_OPERATORS_H
diff --git a/extern/Eigen2/Eigen/src/Array/Functors.h b/extern/Eigen2/Eigen/src/Array/Functors.h
new file mode 100644
index 00000000000..0aae7fd2c40
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/Functors.h
@@ -0,0 +1,305 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ARRAY_FUNCTORS_H
+#define EIGEN_ARRAY_FUNCTORS_H
+
+/** \internal
+  * \array_module
+  *
+  * \brief Template functor to add a scalar to a fixed other one
+  *
+  * \sa class CwiseUnaryOp, Array::operator+
+  */
+/* If you wonder why doing the ei_pset1() in packetOp() is an optimization check ei_scalar_multiple_op */
+template<typename Scalar>
+struct ei_scalar_add_op {
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  inline ei_scalar_add_op(const ei_scalar_add_op& other) : m_other(other.m_other) { }
+  inline ei_scalar_add_op(const Scalar& other) : m_other(other) { }
+  inline Scalar operator() (const Scalar& a) const { return a + m_other; }
+  inline const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_padd(a, ei_pset1(m_other)); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_add_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the square root of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::sqrt()
+  */
+template<typename Scalar> struct ei_scalar_sqrt_op EIGEN_EMPTY_STRUCT {
+  inline const Scalar operator() (const Scalar& a) const { return ei_sqrt(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_sqrt_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the exponential of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::exp()
+  */
+template<typename Scalar> struct ei_scalar_exp_op EIGEN_EMPTY_STRUCT {
+  inline const Scalar operator() (const Scalar& a) const { return ei_exp(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_exp_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the logarithm of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::log()
+  */
+template<typename Scalar> struct ei_scalar_log_op EIGEN_EMPTY_STRUCT {
+  inline const Scalar operator() (const Scalar& a) const { return ei_log(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_log_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the cosine of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::cos()
+  */
+template<typename Scalar> struct ei_scalar_cos_op EIGEN_EMPTY_STRUCT {
+  inline const Scalar operator() (const Scalar& a) const { return ei_cos(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_cos_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the sine of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::sin()
+  */
+template<typename Scalar> struct ei_scalar_sin_op EIGEN_EMPTY_STRUCT {
+  inline const Scalar operator() (const Scalar& a) const { return ei_sin(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_sin_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to raise a scalar to a power
+  *
+  * \sa class CwiseUnaryOp, Cwise::pow
+  */
+template<typename Scalar>
+struct ei_scalar_pow_op {
+  // FIXME default copy constructors seems bugged with std::complex<>
+  inline ei_scalar_pow_op(const ei_scalar_pow_op& other) : m_exponent(other.m_exponent) { }
+  inline ei_scalar_pow_op(const Scalar& exponent) : m_exponent(exponent) {}
+  inline Scalar operator() (const Scalar& a) const { return ei_pow(a, m_exponent); }
+  const Scalar m_exponent;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_pow_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the inverse of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::inverse()
+  */
+template<typename Scalar>
+struct ei_scalar_inverse_op {
+  inline Scalar operator() (const Scalar& a) const { return Scalar(1)/a; }
+  template<typename PacketScalar>
+  inline const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pdiv(ei_pset1(Scalar(1)),a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_inverse_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the square of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::square()
+  */
+template<typename Scalar>
+struct ei_scalar_square_op {
+  inline Scalar operator() (const Scalar& a) const { return a*a; }
+  template<typename PacketScalar>
+  inline const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pmul(a,a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_square_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
+
+/** \internal
+  *
+  * \array_module
+  *
+  * \brief Template functor to compute the cube of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::cube()
+  */
+template<typename Scalar>
+struct ei_scalar_cube_op {
+  inline Scalar operator() (const Scalar& a) const { return a*a*a; }
+  template<typename PacketScalar>
+  inline const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pmul(a,ei_pmul(a,a)); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_cube_op<Scalar> >
+{ enum { Cost = 2*NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
+
+// default ei_functor_traits for STL functors:
+
+template<typename T>
+struct ei_functor_traits<std::multiplies<T> >
+{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::divides<T> >
+{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::plus<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::minus<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::negate<T> >
+{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::logical_or<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::logical_and<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::logical_not<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::greater<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::less<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::greater_equal<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::less_equal<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::equal_to<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::not_equal_to<T> >
+{ enum { Cost = 1, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::binder2nd<T> >
+{ enum { Cost = ei_functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::binder1st<T> >
+{ enum { Cost = ei_functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::unary_negate<T> >
+{ enum { Cost = 1 + ei_functor_traits<T>::Cost, PacketAccess = false }; };
+
+template<typename T>
+struct ei_functor_traits<std::binary_negate<T> >
+{ enum { Cost = 1 + ei_functor_traits<T>::Cost, PacketAccess = false }; };
+
+#ifdef EIGEN_STDEXT_SUPPORT
+
+template<typename T0,typename T1>
+struct ei_functor_traits<std::project1st<T0,T1> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct ei_functor_traits<std::project2nd<T0,T1> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct ei_functor_traits<std::select2nd<std::pair<T0,T1> > >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct ei_functor_traits<std::select1st<std::pair<T0,T1> > >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+template<typename T0,typename T1>
+struct ei_functor_traits<std::unary_compose<T0,T1> >
+{ enum { Cost = ei_functor_traits<T0>::Cost + ei_functor_traits<T1>::Cost, PacketAccess = false }; };
+
+template<typename T0,typename T1,typename T2>
+struct ei_functor_traits<std::binary_compose<T0,T1,T2> >
+{ enum { Cost = ei_functor_traits<T0>::Cost + ei_functor_traits<T1>::Cost + ei_functor_traits<T2>::Cost, PacketAccess = false }; };
+
+#endif // EIGEN_STDEXT_SUPPORT
+
+#endif // EIGEN_ARRAY_FUNCTORS_H
diff --git a/extern/Eigen2/Eigen/src/Array/Norms.h b/extern/Eigen2/Eigen/src/Array/Norms.h
new file mode 100644
index 00000000000..6b92e6a099d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/Norms.h
@@ -0,0 +1,80 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ARRAY_NORMS_H
+#define EIGEN_ARRAY_NORMS_H
+
+template<typename Derived, int p>
+struct ei_lpNorm_selector
+{
+  typedef typename NumTraits<typename ei_traits<Derived>::Scalar>::Real RealScalar;
+  inline static RealScalar run(const MatrixBase<Derived>& m)
+  {
+    return ei_pow(m.cwise().abs().cwise().pow(p).sum(), RealScalar(1)/p);
+  }
+};
+
+template<typename Derived>
+struct ei_lpNorm_selector<Derived, 1>
+{
+  inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
+  {
+    return m.cwise().abs().sum();
+  }
+};
+
+template<typename Derived>
+struct ei_lpNorm_selector<Derived, 2>
+{
+  inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
+  {
+    return m.norm();
+  }
+};
+
+template<typename Derived>
+struct ei_lpNorm_selector<Derived, Infinity>
+{
+  inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
+  {
+    return m.cwise().abs().maxCoeff();
+  }
+};
+
+/** \array_module
+  * 
+  * \returns the \f$ \ell^p \f$ norm of *this, that is, returns the p-th root of the sum of the p-th powers of the absolute values
+  *          of the coefficients of *this. If \a p is the special value \a Eigen::Infinity, this function returns the \f$ \ell^p\infty \f$
+  *          norm, that is the maximum of the absolute values of the coefficients of *this.
+  *
+  * \sa norm()
+  */
+template<typename Derived>
+template<int p>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::lpNorm() const
+{
+  return ei_lpNorm_selector<Derived, p>::run(*this);
+}
+
+#endif // EIGEN_ARRAY_NORMS_H
diff --git a/extern/Eigen2/Eigen/src/Array/PartialRedux.h b/extern/Eigen2/Eigen/src/Array/PartialRedux.h
new file mode 100644
index 00000000000..b1e8fd4babd
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/PartialRedux.h
@@ -0,0 +1,342 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PARTIAL_REDUX_H
+#define EIGEN_PARTIAL_REDUX_H
+
+/** \array_module \ingroup Array
+  *
+  * \class PartialReduxExpr
+  *
+  * \brief Generic expression of a partially reduxed matrix
+  *
+  * \param MatrixType the type of the matrix we are applying the redux operation
+  * \param MemberOp type of the member functor
+  * \param Direction indicates the direction of the redux (Vertical or Horizontal)
+  *
+  * This class represents an expression of a partial redux operator of a matrix.
+  * It is the return type of PartialRedux functions,
+  * and most of the time this is the only way it is used.
+  *
+  * \sa class PartialRedux
+  */
+
+template< typename MatrixType, typename MemberOp, int Direction>
+class PartialReduxExpr;
+
+template<typename MatrixType, typename MemberOp, int Direction>
+struct ei_traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
+{
+  typedef typename MemberOp::result_type Scalar;
+  typedef typename MatrixType::Scalar InputScalar;
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_cleantype<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    RowsAtCompileTime = Direction==Vertical   ? 1 : MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::ColsAtCompileTime,
+    MaxRowsAtCompileTime = Direction==Vertical   ? 1 : MatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
+    Flags = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits,
+    TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime
+  };
+  #if EIGEN_GNUC_AT_LEAST(3,4)
+  typedef typename MemberOp::template Cost<InputScalar,int(TraversalSize)> CostOpType;
+  #else
+  typedef typename MemberOp::template Cost<InputScalar,TraversalSize> CostOpType;
+  #endif
+  enum {
+    CoeffReadCost = TraversalSize * ei_traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
+  };
+};
+
+template< typename MatrixType, typename MemberOp, int Direction>
+class PartialReduxExpr : ei_no_assignment_operator,
+  public MatrixBase<PartialReduxExpr<MatrixType, MemberOp, Direction> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(PartialReduxExpr)
+    typedef typename ei_traits<PartialReduxExpr>::MatrixTypeNested MatrixTypeNested;
+    typedef typename ei_traits<PartialReduxExpr>::_MatrixTypeNested _MatrixTypeNested;
+
+    PartialReduxExpr(const MatrixType& mat, const MemberOp& func = MemberOp())
+      : m_matrix(mat), m_functor(func) {}
+
+    int rows() const { return (Direction==Vertical   ? 1 : m_matrix.rows()); }
+    int cols() const { return (Direction==Horizontal ? 1 : m_matrix.cols()); }
+
+    const Scalar coeff(int i, int j) const
+    {
+      if (Direction==Vertical)
+        return m_functor(m_matrix.col(j));
+      else
+        return m_functor(m_matrix.row(i));
+    }
+
+  protected:
+    const MatrixTypeNested m_matrix;
+    const MemberOp m_functor;
+};
+
+#define EIGEN_MEMBER_FUNCTOR(MEMBER,COST)                           \
+  template <typename ResultType>                                    \
+  struct ei_member_##MEMBER EIGEN_EMPTY_STRUCT {                    \
+    typedef ResultType result_type;                                 \
+    template<typename Scalar, int Size> struct Cost                 \
+    { enum { value = COST }; };                                     \
+    template<typename Derived>                                      \
+    inline ResultType operator()(const MatrixBase<Derived>& mat) const     \
+    { return mat.MEMBER(); } \
+  }
+
+EIGEN_MEMBER_FUNCTOR(squaredNorm, Size * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(norm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(sum, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(minCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(maxCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(all, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(any, (Size-1)*NumTraits<Scalar>::AddCost);
+EIGEN_MEMBER_FUNCTOR(count, (Size-1)*NumTraits<Scalar>::AddCost);
+
+/** \internal */
+template <typename BinaryOp, typename Scalar>
+struct ei_member_redux {
+  typedef typename ei_result_of<
+                     BinaryOp(Scalar)
+                   >::type  result_type;
+  template<typename _Scalar, int Size> struct Cost
+  { enum { value = (Size-1) * ei_functor_traits<BinaryOp>::Cost }; };
+  ei_member_redux(const BinaryOp func) : m_functor(func) {}
+  template<typename Derived>
+  inline result_type operator()(const MatrixBase<Derived>& mat) const
+  { return mat.redux(m_functor); }
+  const BinaryOp m_functor;
+};
+
+/** \array_module \ingroup Array
+  *
+  * \class PartialRedux
+  *
+  * \brief Pseudo expression providing partial reduction operations
+  *
+  * \param ExpressionType the type of the object on which to do partial reductions
+  * \param Direction indicates the direction of the redux (Vertical or Horizontal)
+  *
+  * This class represents a pseudo expression with partial reduction features.
+  * It is the return type of MatrixBase::colwise() and MatrixBase::rowwise()
+  * and most of the time this is the only way it is used.
+  *
+  * Example: \include MatrixBase_colwise.cpp
+  * Output: \verbinclude MatrixBase_colwise.out
+  *
+  * \sa MatrixBase::colwise(), MatrixBase::rowwise(), class PartialReduxExpr
+  */
+template<typename ExpressionType, int Direction> class PartialRedux
+{
+  public:
+
+    typedef typename ei_traits<ExpressionType>::Scalar Scalar;
+    typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
+        ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
+
+    template<template<typename _Scalar> class Functor> struct ReturnType
+    {
+      typedef PartialReduxExpr<ExpressionType,
+                               Functor<typename ei_traits<ExpressionType>::Scalar>,
+                               Direction
+                              > Type;
+    };
+
+    template<typename BinaryOp> struct ReduxReturnType
+    {
+      typedef PartialReduxExpr<ExpressionType,
+                               ei_member_redux<BinaryOp,typename ei_traits<ExpressionType>::Scalar>,
+                               Direction
+                              > Type;
+    };
+
+    typedef typename ExpressionType::PlainMatrixType CrossReturnType;
+    
+    inline PartialRedux(const ExpressionType& matrix) : m_matrix(matrix) {}
+
+    /** \internal */
+    inline const ExpressionType& _expression() const { return m_matrix; }
+
+    template<typename BinaryOp>
+    const typename ReduxReturnType<BinaryOp>::Type
+    redux(const BinaryOp& func = BinaryOp()) const;
+
+    /** \returns a row (or column) vector expression of the smallest coefficient
+      * of each column (or row) of the referenced expression.
+      *
+      * Example: \include PartialRedux_minCoeff.cpp
+      * Output: \verbinclude PartialRedux_minCoeff.out
+      *
+      * \sa MatrixBase::minCoeff() */
+    const typename ReturnType<ei_member_minCoeff>::Type minCoeff() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression of the largest coefficient
+      * of each column (or row) of the referenced expression.
+      *
+      * Example: \include PartialRedux_maxCoeff.cpp
+      * Output: \verbinclude PartialRedux_maxCoeff.out
+      *
+      * \sa MatrixBase::maxCoeff() */
+    const typename ReturnType<ei_member_maxCoeff>::Type maxCoeff() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression of the squared norm
+      * of each column (or row) of the referenced expression.
+      *
+      * Example: \include PartialRedux_squaredNorm.cpp
+      * Output: \verbinclude PartialRedux_squaredNorm.out
+      *
+      * \sa MatrixBase::squaredNorm() */
+    const typename ReturnType<ei_member_squaredNorm>::Type squaredNorm() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression of the norm
+      * of each column (or row) of the referenced expression.
+      *
+      * Example: \include PartialRedux_norm.cpp
+      * Output: \verbinclude PartialRedux_norm.out
+      *
+      * \sa MatrixBase::norm() */
+    const typename ReturnType<ei_member_norm>::Type norm() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression of the sum
+      * of each column (or row) of the referenced expression.
+      *
+      * Example: \include PartialRedux_sum.cpp
+      * Output: \verbinclude PartialRedux_sum.out
+      *
+      * \sa MatrixBase::sum() */
+    const typename ReturnType<ei_member_sum>::Type sum() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression representing
+      * whether \b all coefficients of each respective column (or row) are \c true.
+      *
+      * \sa MatrixBase::all() */
+    const typename ReturnType<ei_member_all>::Type all() const
+    { return _expression(); }
+
+    /** \returns a row (or column) vector expression representing
+      * whether \b at \b least one coefficient of each respective column (or row) is \c true.
+      *
+      * \sa MatrixBase::any() */
+    const typename ReturnType<ei_member_any>::Type any() const
+    { return _expression(); }
+    
+    /** \returns a row (or column) vector expression representing
+      * the number of \c true coefficients of each respective column (or row).
+      *
+      * Example: \include PartialRedux_count.cpp
+      * Output: \verbinclude PartialRedux_count.out
+      *
+      * \sa MatrixBase::count() */
+    const PartialReduxExpr<ExpressionType, ei_member_count<int>, Direction> count() const
+    { return _expression(); }
+
+    /** \returns a 3x3 matrix expression of the cross product
+      * of each column or row of the referenced expression with the \a other vector.
+      *
+      * \geometry_module
+      *
+      * \sa MatrixBase::cross() */
+    template<typename OtherDerived>
+    const CrossReturnType cross(const MatrixBase<OtherDerived>& other) const
+    {
+      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(CrossReturnType,3,3)
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
+      EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+      if(Direction==Vertical)
+        return (CrossReturnType()
+                                 << _expression().col(0).cross(other),
+                                    _expression().col(1).cross(other),
+                                    _expression().col(2).cross(other)).finished();
+      else
+        return (CrossReturnType() 
+                                 << _expression().row(0).cross(other),
+                                    _expression().row(1).cross(other),
+                                    _expression().row(2).cross(other)).finished();
+    }
+
+  protected:
+    ExpressionTypeNested m_matrix;
+};
+
+/** \array_module
+  *
+  * \returns a PartialRedux wrapper of *this providing additional partial reduction operations
+  *
+  * Example: \include MatrixBase_colwise.cpp
+  * Output: \verbinclude MatrixBase_colwise.out
+  *
+  * \sa rowwise(), class PartialRedux
+  */
+template<typename Derived>
+inline const PartialRedux<Derived,Vertical>
+MatrixBase<Derived>::colwise() const
+{
+  return derived();
+}
+
+/** \array_module
+  *
+  * \returns a PartialRedux wrapper of *this providing additional partial reduction operations
+  *
+  * Example: \include MatrixBase_rowwise.cpp
+  * Output: \verbinclude MatrixBase_rowwise.out
+  *
+  * \sa colwise(), class PartialRedux
+  */
+template<typename Derived>
+inline const PartialRedux<Derived,Horizontal>
+MatrixBase<Derived>::rowwise() const
+{
+  return derived();
+}
+
+/** \returns a row or column vector expression of \c *this reduxed by \a func
+  *
+  * The template parameter \a BinaryOp is the type of the functor
+  * of the custom redux operator. Note that func must be an associative operator.
+  *
+  * \sa class PartialRedux, MatrixBase::colwise(), MatrixBase::rowwise()
+  */
+template<typename ExpressionType, int Direction>
+template<typename BinaryOp>
+const typename PartialRedux<ExpressionType,Direction>::template ReduxReturnType<BinaryOp>::Type
+PartialRedux<ExpressionType,Direction>::redux(const BinaryOp& func) const
+{
+  return typename ReduxReturnType<BinaryOp>::Type(_expression(), func);
+}
+
+#endif // EIGEN_PARTIAL_REDUX_H
diff --git a/extern/Eigen2/Eigen/src/Array/Random.h b/extern/Eigen2/Eigen/src/Array/Random.h
new file mode 100644
index 00000000000..9185fe4a7d3
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/Random.h
@@ -0,0 +1,156 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_RANDOM_H
+#define EIGEN_RANDOM_H
+
+template<typename Scalar> struct ei_scalar_random_op EIGEN_EMPTY_STRUCT {
+  inline ei_scalar_random_op(void) {}
+  inline const Scalar operator() (int, int) const { return ei_random<Scalar>(); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_random_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
+
+/** \array_module
+  * 
+  * \returns a random matrix (not an expression, the matrix is immediately evaluated).
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so ei_random() should be used
+  * instead.
+  *
+  * \addexample RandomExample \label How to create a matrix with random coefficients
+  *
+  * Example: \include MatrixBase_random_int_int.cpp
+  * Output: \verbinclude MatrixBase_random_int_int.out
+  *
+  * \sa MatrixBase::setRandom(), MatrixBase::Random(int), MatrixBase::Random()
+  */
+template<typename Derived>
+inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
+MatrixBase<Derived>::Random(int rows, int cols)
+{
+  return NullaryExpr(rows, cols, ei_scalar_random_op<Scalar>());
+}
+
+/** \array_module
+  * 
+  * \returns a random vector (not an expression, the vector is immediately evaluated).
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so ei_random() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_random_int.cpp
+  * Output: \verbinclude MatrixBase_random_int.out
+  *
+  * \sa MatrixBase::setRandom(), MatrixBase::Random(int,int), MatrixBase::Random()
+  */
+template<typename Derived>
+inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
+MatrixBase<Derived>::Random(int size)
+{
+  return NullaryExpr(size, ei_scalar_random_op<Scalar>());
+}
+
+/** \array_module
+  * 
+  * \returns a fixed-size random matrix or vector
+  * (not an expression, the matrix is immediately evaluated).
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * Example: \include MatrixBase_random.cpp
+  * Output: \verbinclude MatrixBase_random.out
+  *
+  * \sa MatrixBase::setRandom(), MatrixBase::Random(int,int), MatrixBase::Random(int)
+  */
+template<typename Derived>
+inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
+MatrixBase<Derived>::Random()
+{
+  return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_random_op<Scalar>());
+}
+
+/** \array_module
+  * 
+  * Sets all coefficients in this expression to random values.
+  *
+  * Example: \include MatrixBase_setRandom.cpp
+  * Output: \verbinclude MatrixBase_setRandom.out
+  *
+  * \sa class CwiseNullaryOp, setRandom(int), setRandom(int,int)
+  */
+template<typename Derived>
+inline Derived& MatrixBase<Derived>::setRandom()
+{
+  return *this = Random(rows(), cols());
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to random values.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setRandom_int.cpp
+  * Output: \verbinclude Matrix_setRandom_int.out
+  *
+  * \sa MatrixBase::setRandom(), setRandom(int,int), class CwiseNullaryOp, MatrixBase::Random()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setRandom(int size)
+{
+  resize(size);
+  return setRandom();
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to random values.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setRandom_int_int.cpp
+  * Output: \verbinclude Matrix_setRandom_int_int.out
+  *
+  * \sa MatrixBase::setRandom(), setRandom(int), class CwiseNullaryOp, MatrixBase::Random()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setRandom(int rows, int cols)
+{
+  resize(rows, cols);
+  return setRandom();
+}
+
+#endif // EIGEN_RANDOM_H
diff --git a/extern/Eigen2/Eigen/src/Array/Select.h b/extern/Eigen2/Eigen/src/Array/Select.h
new file mode 100644
index 00000000000..9dc3fb1b27a
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Array/Select.h
@@ -0,0 +1,159 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SELECT_H
+#define EIGEN_SELECT_H
+
+/** \array_module \ingroup Array
+  *
+  * \class Select
+  *
+  * \brief Expression of a coefficient wise version of the C++ ternary operator ?:
+  *
+  * \param ConditionMatrixType the type of the \em condition expression which must be a boolean matrix
+  * \param ThenMatrixType the type of the \em then expression
+  * \param ElseMatrixType the type of the \em else expression
+  *
+  * This class represents an expression of a coefficient wise version of the C++ ternary operator ?:.
+  * It is the return type of MatrixBase::select() and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const
+  */
+
+template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+struct ei_traits<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
+{
+  typedef typename ei_traits<ThenMatrixType>::Scalar Scalar;
+  typedef typename ConditionMatrixType::Nested ConditionMatrixNested;
+  typedef typename ThenMatrixType::Nested ThenMatrixNested;
+  typedef typename ElseMatrixType::Nested ElseMatrixNested;
+  enum {
+    RowsAtCompileTime = ConditionMatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = ConditionMatrixType::ColsAtCompileTime,
+    MaxRowsAtCompileTime = ConditionMatrixType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = ConditionMatrixType::MaxColsAtCompileTime,
+    Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & HereditaryBits,
+	CoeffReadCost = ei_traits<typename ei_cleantype<ConditionMatrixNested>::type>::CoeffReadCost
+	+ EIGEN_ENUM_MAX(ei_traits<typename ei_cleantype<ThenMatrixNested>::type>::CoeffReadCost,
+	                 ei_traits<typename ei_cleantype<ElseMatrixNested>::type>::CoeffReadCost)
+  };
+};
+
+template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
+class Select : ei_no_assignment_operator,
+  public MatrixBase<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Select)
+
+    Select(const ConditionMatrixType& conditionMatrix,
+           const ThenMatrixType& thenMatrix,
+           const ElseMatrixType& elseMatrix)
+      : m_condition(conditionMatrix), m_then(thenMatrix), m_else(elseMatrix)
+    {
+      ei_assert(m_condition.rows() == m_then.rows() && m_condition.rows() == m_else.rows());
+      ei_assert(m_condition.cols() == m_then.cols() && m_condition.cols() == m_else.cols());
+    }
+
+    int rows() const { return m_condition.rows(); }
+    int cols() const { return m_condition.cols(); }
+
+    const Scalar coeff(int i, int j) const
+    {
+      if (m_condition.coeff(i,j))
+        return m_then.coeff(i,j);
+      else
+        return m_else.coeff(i,j);
+    }
+    
+    const Scalar coeff(int i) const
+    {
+      if (m_condition.coeff(i))
+        return m_then.coeff(i);
+      else
+        return m_else.coeff(i);
+    }
+
+  protected:
+    const typename ConditionMatrixType::Nested m_condition;
+    const typename ThenMatrixType::Nested m_then;
+    const typename ElseMatrixType::Nested m_else;
+};
+
+
+/** \array_module
+  *
+  * \returns a matrix where each coefficient (i,j) is equal to \a thenMatrix(i,j)
+  * if \c *this(i,j), and \a elseMatrix(i,j) otherwise.
+  *
+  * Example: \include MatrixBase_select.cpp
+  * Output: \verbinclude MatrixBase_select.out
+  *
+  * \sa class Select
+  */
+template<typename Derived>
+template<typename ThenDerived,typename ElseDerived>
+inline const Select<Derived,ThenDerived,ElseDerived>
+MatrixBase<Derived>::select(const MatrixBase<ThenDerived>& thenMatrix,
+                            const MatrixBase<ElseDerived>& elseMatrix) const
+{
+  return Select<Derived,ThenDerived,ElseDerived>(derived(), thenMatrix.derived(), elseMatrix.derived());
+}
+
+/** \array_module
+  *
+  * Version of MatrixBase::select(const MatrixBase&, const MatrixBase&) with
+  * the \em else expression being a scalar value.
+  *
+  * \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const, class Select
+  */
+template<typename Derived>
+template<typename ThenDerived>
+inline const Select<Derived,ThenDerived, NestByValue<typename ThenDerived::ConstantReturnType> >
+MatrixBase<Derived>::select(const MatrixBase<ThenDerived>& thenMatrix,
+                            typename ThenDerived::Scalar elseScalar) const
+{
+  return Select<Derived,ThenDerived,NestByValue<typename ThenDerived::ConstantReturnType> >(
+    derived(), thenMatrix.derived(), ThenDerived::Constant(rows(),cols(),elseScalar));
+}
+
+/** \array_module
+  *
+  * Version of MatrixBase::select(const MatrixBase&, const MatrixBase&) with
+  * the \em then expression being a scalar value.
+  *
+  * \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const, class Select
+  */
+template<typename Derived>
+template<typename ElseDerived>
+inline const Select<Derived, NestByValue<typename ElseDerived::ConstantReturnType>, ElseDerived >
+MatrixBase<Derived>::select(typename ElseDerived::Scalar thenScalar,
+                            const MatrixBase<ElseDerived>& elseMatrix) const
+{
+  return Select<Derived,NestByValue<typename ElseDerived::ConstantReturnType>,ElseDerived>(
+    derived(), ElseDerived::Constant(rows(),cols(),thenScalar), elseMatrix.derived());
+}
+
+#endif // EIGEN_SELECT_H
diff --git a/extern/Eigen2/Eigen/src/Cholesky/CholeskyInstantiations.cpp b/extern/Eigen2/Eigen/src/Cholesky/CholeskyInstantiations.cpp
new file mode 100644
index 00000000000..e7f40a2ce9c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Cholesky/CholeskyInstantiations.cpp
@@ -0,0 +1,35 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_EXTERN_INSTANTIATIONS
+#define EIGEN_EXTERN_INSTANTIATIONS
+#endif
+#include "../../Core"
+#undef EIGEN_EXTERN_INSTANTIATIONS
+
+#include "../../Cholesky"
+
+namespace Eigen {
+  EIGEN_CHOLESKY_MODULE_INSTANTIATE();
+}
diff --git a/extern/Eigen2/Eigen/src/Cholesky/LDLT.h b/extern/Eigen2/Eigen/src/Cholesky/LDLT.h
new file mode 100644
index 00000000000..205b78a6ded
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Cholesky/LDLT.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_LDLT_H
+#define EIGEN_LDLT_H
+
+/** \ingroup cholesky_Module
+  *
+  * \class LDLT
+  *
+  * \brief Robust Cholesky decomposition of a matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LDL^T Cholesky decomposition
+  *
+  * This class performs a Cholesky decomposition without square root of a symmetric, positive definite
+  * matrix A such that A = L D L^* = U^* D U, where L is lower triangular with a unit diagonal
+  * and D is a diagonal matrix.
+  *
+  * Compared to a standard Cholesky decomposition, avoiding the square roots allows for faster and more
+  * stable computation.
+  *
+  * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
+  * the strict lower part does not have to store correct values.
+  *
+  * \sa MatrixBase::ldlt(), class LLT
+  */
+template<typename MatrixType> class LDLT
+{
+  public:
+
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
+
+    LDLT(const MatrixType& matrix)
+      : m_matrix(matrix.rows(), matrix.cols())
+    {
+      compute(matrix);
+    }
+
+    /** \returns the lower triangular matrix L */
+    inline Part<MatrixType, UnitLowerTriangular> matrixL(void) const { return m_matrix; }
+
+    /** \returns the coefficients of the diagonal matrix D */
+    inline DiagonalCoeffs<MatrixType> vectorD(void) const { return m_matrix.diagonal(); }
+
+    /** \returns true if the matrix is positive definite */
+    inline bool isPositiveDefinite(void) const { return m_isPositiveDefinite; }
+
+    template<typename RhsDerived, typename ResultType>
+    bool solve(const MatrixBase<RhsDerived> &b, ResultType *result) const;
+
+    template<typename Derived>
+    bool solveInPlace(MatrixBase<Derived> &bAndX) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+    /** \internal
+      * Used to compute and store the cholesky decomposition A = L D L^* = U^* D U.
+      * The strict upper part is used during the decomposition, the strict lower
+      * part correspond to the coefficients of L (its diagonal is equal to 1 and
+      * is not stored), and the diagonal entries correspond to D.
+      */
+    MatrixType m_matrix;
+
+    bool m_isPositiveDefinite;
+};
+
+/** Compute / recompute the LLT decomposition A = L D L^* = U^* D U of \a matrix
+  */
+template<typename MatrixType>
+void LDLT<MatrixType>::compute(const MatrixType& a)
+{
+  assert(a.rows()==a.cols());
+  const int size = a.rows();
+  m_matrix.resize(size, size);
+  m_isPositiveDefinite = true;
+  const RealScalar eps = ei_sqrt(precision<Scalar>());
+
+  if (size<=1)
+  {
+    m_matrix = a;
+    return;
+  }
+
+  // Let's preallocate a temporay vector to evaluate the matrix-vector product into it.
+  // Unlike the standard LLT decomposition, here we cannot evaluate it to the destination
+  // matrix because it a sub-row which is not compatible suitable for efficient packet evaluation.
+  // (at least if we assume the matrix is col-major)
+  Matrix<Scalar,MatrixType::RowsAtCompileTime,1> _temporary(size);
+
+  // Note that, in this algorithm the rows of the strict upper part of m_matrix is used to store
+  // column vector, thus the strange .conjugate() and .transpose()...
+
+  m_matrix.row(0) = a.row(0).conjugate();
+  m_matrix.col(0).end(size-1) = m_matrix.row(0).end(size-1) / m_matrix.coeff(0,0);
+  for (int j = 1; j < size; ++j)
+  {
+    RealScalar tmp = ei_real(a.coeff(j,j) - (m_matrix.row(j).start(j) * m_matrix.col(j).start(j).conjugate()).coeff(0,0));
+    m_matrix.coeffRef(j,j) = tmp;
+
+    if (tmp < eps)
+    {
+      m_isPositiveDefinite = false;
+      return;
+    }
+
+    int endSize = size-j-1;
+    if (endSize>0)
+    {
+      _temporary.end(endSize) = ( m_matrix.block(j+1,0, endSize, j)
+                                  * m_matrix.col(j).start(j).conjugate() ).lazy();
+
+      m_matrix.row(j).end(endSize) = a.row(j).end(endSize).conjugate()
+                                   - _temporary.end(endSize).transpose();
+
+      m_matrix.col(j).end(endSize) = m_matrix.row(j).end(endSize) / tmp;
+    }
+  }
+}
+
+/** Computes the solution x of \f$ A x = b \f$ using the current decomposition of A.
+  * The result is stored in \a result
+  *
+  * \returns true in case of success, false otherwise.
+  *
+  * In other words, it computes \f$ b = A^{-1} b \f$ with
+  * \f$ {L^{*}}^{-1} D^{-1} L^{-1} b \f$ from right to left.
+  *
+  * \sa LDLT::solveInPlace(), MatrixBase::ldlt()
+  */
+template<typename MatrixType>
+template<typename RhsDerived, typename ResultType>
+bool LDLT<MatrixType>
+::solve(const MatrixBase<RhsDerived> &b, ResultType *result) const
+{
+  const int size = m_matrix.rows();
+  ei_assert(size==b.rows() && "LLT::solve(): invalid number of rows of the right hand side matrix b");
+  *result = b;
+  return solveInPlace(*result);
+}
+
+/** This is the \em in-place version of solve().
+  *
+  * \param bAndX represents both the right-hand side matrix b and result x.
+  *
+  * This version avoids a copy when the right hand side matrix b is not
+  * needed anymore.
+  *
+  * \sa LDLT::solve(), MatrixBase::ldlt()
+  */
+template<typename MatrixType>
+template<typename Derived>
+bool LDLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
+{
+  const int size = m_matrix.rows();
+  ei_assert(size==bAndX.rows());
+  if (!m_isPositiveDefinite)
+    return false;
+  matrixL().solveTriangularInPlace(bAndX);
+  bAndX = (m_matrix.cwise().inverse().template part<Diagonal>() * bAndX).lazy();
+  m_matrix.adjoint().template part<UnitUpperTriangular>().solveTriangularInPlace(bAndX);
+  return true;
+}
+
+/** \cholesky_module
+  * \returns the Cholesky decomposition without square root of \c *this
+  */
+template<typename Derived>
+inline const LDLT<typename MatrixBase<Derived>::PlainMatrixType>
+MatrixBase<Derived>::ldlt() const
+{
+  return derived();
+}
+
+#endif // EIGEN_LDLT_H
diff --git a/extern/Eigen2/Eigen/src/Cholesky/LLT.h b/extern/Eigen2/Eigen/src/Cholesky/LLT.h
new file mode 100644
index 00000000000..42c959f83a2
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Cholesky/LLT.h
@@ -0,0 +1,219 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_LLT_H
+#define EIGEN_LLT_H
+
+/** \ingroup cholesky_Module
+  *
+  * \class LLT
+  *
+  * \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
+  *
+  * This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
+  * matrix A such that A = LL^* = U^*U, where L is lower triangular.
+  *
+  * While the Cholesky decomposition is particularly useful to solve selfadjoint problems like  D^*D x = b,
+  * for that purpose, we recommend the Cholesky decomposition without square root which is more stable
+  * and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other
+  * situations like generalised eigen problems with hermitian matrices.
+  *
+  * Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive definite matrices,
+  * use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine whether a system of equations
+  * has a solution.
+  *
+  * \sa MatrixBase::llt(), class LDLT
+  */
+ /* HEY THIS DOX IS DISABLED BECAUSE THERE's A BUG EITHER HERE OR IN LDLT ABOUT THAT (OR BOTH)
+  * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
+  * the strict lower part does not have to store correct values.
+  */
+template<typename MatrixType> class LLT
+{
+  private:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
+
+    enum {
+      PacketSize = ei_packet_traits<Scalar>::size,
+      AlignmentMask = int(PacketSize)-1
+    };
+
+  public:
+
+    /** 
+    * \brief Default Constructor.
+    *
+    * The default constructor is useful in cases in which the user intends to
+    * perform decompositions via LLT::compute(const MatrixType&).
+    */
+    LLT() : m_matrix(), m_isInitialized(false) {}
+
+    LLT(const MatrixType& matrix)
+      : m_matrix(matrix.rows(), matrix.cols()),
+        m_isInitialized(false)
+    {
+      compute(matrix);
+    }
+
+    /** \returns the lower triangular matrix L */
+    inline Part<MatrixType, LowerTriangular> matrixL(void) const 
+    { 
+      ei_assert(m_isInitialized && "LLT is not initialized.");
+      return m_matrix; 
+    }
+    
+    /** \deprecated */
+    inline bool isPositiveDefinite(void) const { return m_isInitialized && m_isPositiveDefinite; }
+
+    template<typename RhsDerived, typename ResultType>
+    bool solve(const MatrixBase<RhsDerived> &b, ResultType *result) const;
+
+    template<typename Derived>
+    bool solveInPlace(MatrixBase<Derived> &bAndX) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+    /** \internal
+      * Used to compute and store L
+      * The strict upper part is not used and even not initialized.
+      */
+    MatrixType m_matrix;
+    bool m_isInitialized;
+    bool m_isPositiveDefinite;
+};
+
+/** Computes / recomputes the Cholesky decomposition A = LL^* = U^*U of \a matrix
+  */
+template<typename MatrixType>
+void LLT<MatrixType>::compute(const MatrixType& a)
+{
+  assert(a.rows()==a.cols());
+  m_isPositiveDefinite = true;
+  const int size = a.rows();
+  m_matrix.resize(size, size);
+  // The biggest overall is the point of reference to which further diagonals
+  // are compared; if any diagonal is negligible compared
+  // to the largest overall, the algorithm bails.  This cutoff is suggested
+  // in "Analysis of the Cholesky Decomposition of a Semi-definite Matrix" by
+  // Nicholas J. Higham. Also see "Accuracy and Stability of Numerical
+  // Algorithms" page 217, also by Higham.
+  const RealScalar cutoff = machine_epsilon<Scalar>() * size * a.diagonal().cwise().abs().maxCoeff();
+  RealScalar x;
+  x = ei_real(a.coeff(0,0));
+  m_matrix.coeffRef(0,0) = ei_sqrt(x);
+  if(size==1)
+  {
+    m_isInitialized = true;
+    return;
+  }
+  m_matrix.col(0).end(size-1) = a.row(0).end(size-1).adjoint() / ei_real(m_matrix.coeff(0,0));
+  for (int j = 1; j < size; ++j)
+  {
+    x = ei_real(a.coeff(j,j)) - m_matrix.row(j).start(j).squaredNorm();
+    if (x < cutoff)
+    {
+      m_isPositiveDefinite = false;
+      continue;
+    }
+
+    m_matrix.coeffRef(j,j) = x = ei_sqrt(x);
+
+    int endSize = size-j-1;
+    if (endSize>0) {
+      // Note that when all matrix columns have good alignment, then the following
+      // product is guaranteed to be optimal with respect to alignment.
+      m_matrix.col(j).end(endSize) =
+        (m_matrix.block(j+1, 0, endSize, j) * m_matrix.row(j).start(j).adjoint()).lazy();
+
+      // FIXME could use a.col instead of a.row
+      m_matrix.col(j).end(endSize) = (a.row(j).end(endSize).adjoint()
+        - m_matrix.col(j).end(endSize) ) / x;
+    }
+  }
+
+  m_isInitialized = true;
+}
+
+/** Computes the solution x of \f$ A x = b \f$ using the current decomposition of A.
+  * The result is stored in \a result
+  *
+  * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
+  *
+  * In other words, it computes \f$ b = A^{-1} b \f$ with
+  * \f$ {L^{*}}^{-1} L^{-1} b \f$ from right to left.
+  *
+  * Example: \include LLT_solve.cpp
+  * Output: \verbinclude LLT_solve.out
+  *
+  * \sa LLT::solveInPlace(), MatrixBase::llt()
+  */
+template<typename MatrixType>
+template<typename RhsDerived, typename ResultType>
+bool LLT<MatrixType>::solve(const MatrixBase<RhsDerived> &b, ResultType *result) const
+{
+  ei_assert(m_isInitialized && "LLT is not initialized.");
+  const int size = m_matrix.rows();
+  ei_assert(size==b.rows() && "LLT::solve(): invalid number of rows of the right hand side matrix b");
+  return solveInPlace((*result) = b);
+}
+
+/** This is the \em in-place version of solve().
+  *
+  * \param bAndX represents both the right-hand side matrix b and result x.
+  *
+  * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
+  *
+  * This version avoids a copy when the right hand side matrix b is not
+  * needed anymore.
+  *
+  * \sa LLT::solve(), MatrixBase::llt()
+  */
+template<typename MatrixType>
+template<typename Derived>
+bool LLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
+{
+  ei_assert(m_isInitialized && "LLT is not initialized.");
+  const int size = m_matrix.rows();
+  ei_assert(size==bAndX.rows());
+  matrixL().solveTriangularInPlace(bAndX);
+  m_matrix.adjoint().template part<UpperTriangular>().solveTriangularInPlace(bAndX);
+  return true;
+}
+
+/** \cholesky_module
+  * \returns the LLT decomposition of \c *this
+  */
+template<typename Derived>
+inline const LLT<typename MatrixBase<Derived>::PlainMatrixType>
+MatrixBase<Derived>::llt() const
+{
+  return LLT<PlainMatrixType>(derived());
+}
+
+#endif // EIGEN_LLT_H
diff --git a/extern/Eigen2/Eigen/src/Core/Assign.h b/extern/Eigen2/Eigen/src/Core/Assign.h
new file mode 100644
index 00000000000..57205075596
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Assign.h
@@ -0,0 +1,445 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2007 Michael Olbrich <michael.olbrich@gmx.net>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ASSIGN_H
+#define EIGEN_ASSIGN_H
+
+/***************************************************************************
+* Part 1 : the logic deciding a strategy for vectorization and unrolling
+***************************************************************************/
+
+template <typename Derived, typename OtherDerived>
+struct ei_assign_traits
+{
+public:
+  enum {
+    DstIsAligned = Derived::Flags & AlignedBit,
+    SrcIsAligned = OtherDerived::Flags & AlignedBit,
+    SrcAlignment = DstIsAligned && SrcIsAligned ? Aligned : Unaligned
+  };
+
+private:
+  enum {
+    InnerSize = int(Derived::Flags)&RowMajorBit
+              ? Derived::ColsAtCompileTime
+              : Derived::RowsAtCompileTime,
+    InnerMaxSize = int(Derived::Flags)&RowMajorBit
+              ? Derived::MaxColsAtCompileTime
+              : Derived::MaxRowsAtCompileTime,
+    PacketSize = ei_packet_traits<typename Derived::Scalar>::size
+  };
+
+  enum {
+    MightVectorize = (int(Derived::Flags) & int(OtherDerived::Flags) & ActualPacketAccessBit)
+                  && ((int(Derived::Flags)&RowMajorBit)==(int(OtherDerived::Flags)&RowMajorBit)),
+    MayInnerVectorize  = MightVectorize && int(InnerSize)!=Dynamic && int(InnerSize)%int(PacketSize)==0
+                       && int(DstIsAligned) && int(SrcIsAligned),
+    MayLinearVectorize = MightVectorize && (int(Derived::Flags) & int(OtherDerived::Flags) & LinearAccessBit),
+    MaySliceVectorize  = MightVectorize && int(InnerMaxSize)>=3*PacketSize /* slice vectorization can be slow, so we only
+      want it if the slices are big, which is indicated by InnerMaxSize rather than InnerSize, think of the case
+      of a dynamic block in a fixed-size matrix */
+  };
+
+public:
+  enum {
+    Vectorization = int(MayInnerVectorize)  ? int(InnerVectorization)
+                  : int(MayLinearVectorize) ? int(LinearVectorization)
+                  : int(MaySliceVectorize)  ? int(SliceVectorization)
+                                            : int(NoVectorization)
+  };
+
+private:
+  enum {
+    UnrollingLimit      = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize)),
+    MayUnrollCompletely = int(Derived::SizeAtCompileTime) * int(OtherDerived::CoeffReadCost) <= int(UnrollingLimit),
+    MayUnrollInner      = int(InnerSize * OtherDerived::CoeffReadCost) <= int(UnrollingLimit)
+  };
+
+public:
+  enum {
+    Unrolling = (int(Vectorization) == int(InnerVectorization) || int(Vectorization) == int(NoVectorization))
+              ? (
+                   int(MayUnrollCompletely) ? int(CompleteUnrolling)
+                 : int(MayUnrollInner)      ? int(InnerUnrolling)
+                                            : int(NoUnrolling)
+                )
+              : int(Vectorization) == int(LinearVectorization)
+              ? ( int(MayUnrollCompletely) && int(DstIsAligned) ? int(CompleteUnrolling) : int(NoUnrolling) )
+              : int(NoUnrolling)
+  };
+};
+
+/***************************************************************************
+* Part 2 : meta-unrollers
+***************************************************************************/
+
+/***********************
+*** No vectorization ***
+***********************/
+
+template<typename Derived1, typename Derived2, int Index, int Stop>
+struct ei_assign_novec_CompleteUnrolling
+{
+  enum {
+    row = int(Derived1::Flags)&RowMajorBit
+        ? Index / int(Derived1::ColsAtCompileTime)
+        : Index % Derived1::RowsAtCompileTime,
+    col = int(Derived1::Flags)&RowMajorBit
+        ? Index % int(Derived1::ColsAtCompileTime)
+        : Index / Derived1::RowsAtCompileTime
+  };
+
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    dst.copyCoeff(row, col, src);
+    ei_assign_novec_CompleteUnrolling<Derived1, Derived2, Index+1, Stop>::run(dst, src);
+  }
+};
+
+template<typename Derived1, typename Derived2, int Stop>
+struct ei_assign_novec_CompleteUnrolling<Derived1, Derived2, Stop, Stop>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &) {}
+};
+
+template<typename Derived1, typename Derived2, int Index, int Stop>
+struct ei_assign_novec_InnerUnrolling
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src, int row_or_col)
+  {
+    const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
+    const int row = rowMajor ? row_or_col : Index;
+    const int col = rowMajor ? Index : row_or_col;
+    dst.copyCoeff(row, col, src);
+    ei_assign_novec_InnerUnrolling<Derived1, Derived2, Index+1, Stop>::run(dst, src, row_or_col);
+  }
+};
+
+template<typename Derived1, typename Derived2, int Stop>
+struct ei_assign_novec_InnerUnrolling<Derived1, Derived2, Stop, Stop>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &, int) {}
+};
+
+/**************************
+*** Inner vectorization ***
+**************************/
+
+template<typename Derived1, typename Derived2, int Index, int Stop>
+struct ei_assign_innervec_CompleteUnrolling
+{
+  enum {
+    row = int(Derived1::Flags)&RowMajorBit
+        ? Index / int(Derived1::ColsAtCompileTime)
+        : Index % Derived1::RowsAtCompileTime,
+    col = int(Derived1::Flags)&RowMajorBit
+        ? Index % int(Derived1::ColsAtCompileTime)
+        : Index / Derived1::RowsAtCompileTime,
+    SrcAlignment = ei_assign_traits<Derived1,Derived2>::SrcAlignment
+  };
+
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    dst.template copyPacket<Derived2, Aligned, SrcAlignment>(row, col, src);
+    ei_assign_innervec_CompleteUnrolling<Derived1, Derived2,
+      Index+ei_packet_traits<typename Derived1::Scalar>::size, Stop>::run(dst, src);
+  }
+};
+
+template<typename Derived1, typename Derived2, int Stop>
+struct ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, Stop, Stop>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &) {}
+};
+
+template<typename Derived1, typename Derived2, int Index, int Stop>
+struct ei_assign_innervec_InnerUnrolling
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src, int row_or_col)
+  {
+    const int row = int(Derived1::Flags)&RowMajorBit ? row_or_col : Index;
+    const int col = int(Derived1::Flags)&RowMajorBit ? Index : row_or_col;
+    dst.template copyPacket<Derived2, Aligned, Aligned>(row, col, src);
+    ei_assign_innervec_InnerUnrolling<Derived1, Derived2,
+      Index+ei_packet_traits<typename Derived1::Scalar>::size, Stop>::run(dst, src, row_or_col);
+  }
+};
+
+template<typename Derived1, typename Derived2, int Stop>
+struct ei_assign_innervec_InnerUnrolling<Derived1, Derived2, Stop, Stop>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &, int) {}
+};
+
+/***************************************************************************
+* Part 3 : implementation of all cases
+***************************************************************************/
+
+template<typename Derived1, typename Derived2,
+         int Vectorization = ei_assign_traits<Derived1, Derived2>::Vectorization,
+         int Unrolling = ei_assign_traits<Derived1, Derived2>::Unrolling>
+struct ei_assign_impl;
+
+/***********************
+*** No vectorization ***
+***********************/
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, NoVectorization, NoUnrolling>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const int innerSize = dst.innerSize();
+    const int outerSize = dst.outerSize();
+    for(int j = 0; j < outerSize; ++j)
+      for(int i = 0; i < innerSize; ++i)
+      {
+        if(int(Derived1::Flags)&RowMajorBit)
+          dst.copyCoeff(j, i, src);
+        else
+          dst.copyCoeff(i, j, src);
+      }
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, NoVectorization, CompleteUnrolling>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    ei_assign_novec_CompleteUnrolling<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
+      ::run(dst, src);
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, NoVectorization, InnerUnrolling>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
+    const int innerSize = rowMajor ? Derived1::ColsAtCompileTime : Derived1::RowsAtCompileTime;
+    const int outerSize = dst.outerSize();
+    for(int j = 0; j < outerSize; ++j)
+      ei_assign_novec_InnerUnrolling<Derived1, Derived2, 0, innerSize>
+        ::run(dst, src, j);
+  }
+};
+
+/**************************
+*** Inner vectorization ***
+**************************/
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, InnerVectorization, NoUnrolling>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const int innerSize = dst.innerSize();
+    const int outerSize = dst.outerSize();
+    const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
+    for(int j = 0; j < outerSize; ++j)
+      for(int i = 0; i < innerSize; i+=packetSize)
+      {
+        if(int(Derived1::Flags)&RowMajorBit)
+          dst.template copyPacket<Derived2, Aligned, Aligned>(j, i, src);
+        else
+          dst.template copyPacket<Derived2, Aligned, Aligned>(i, j, src);
+      }
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, InnerVectorization, CompleteUnrolling>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
+      ::run(dst, src);
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, InnerVectorization, InnerUnrolling>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
+    const int innerSize = rowMajor ? Derived1::ColsAtCompileTime : Derived1::RowsAtCompileTime;
+    const int outerSize = dst.outerSize();
+    for(int j = 0; j < outerSize; ++j)
+      ei_assign_innervec_InnerUnrolling<Derived1, Derived2, 0, innerSize>
+        ::run(dst, src, j);
+  }
+};
+
+/***************************
+*** Linear vectorization ***
+***************************/
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, LinearVectorization, NoUnrolling>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const int size = dst.size();
+    const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
+    const int alignedStart = ei_assign_traits<Derived1,Derived2>::DstIsAligned ? 0
+                           : ei_alignmentOffset(&dst.coeffRef(0), size);
+    const int alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize;
+
+    for(int index = 0; index < alignedStart; ++index)
+      dst.copyCoeff(index, src);
+
+    for(int index = alignedStart; index < alignedEnd; index += packetSize)
+    {
+      dst.template copyPacket<Derived2, Aligned, ei_assign_traits<Derived1,Derived2>::SrcAlignment>(index, src);
+    }
+
+    for(int index = alignedEnd; index < size; ++index)
+      dst.copyCoeff(index, src);
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, LinearVectorization, CompleteUnrolling>
+{
+  EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const int size = Derived1::SizeAtCompileTime;
+    const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
+    const int alignedSize = (size/packetSize)*packetSize;
+
+    ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, 0, alignedSize>::run(dst, src);
+    ei_assign_novec_CompleteUnrolling<Derived1, Derived2, alignedSize, size>::run(dst, src);
+  }
+};
+
+/**************************
+*** Slice vectorization ***
+***************************/
+
+template<typename Derived1, typename Derived2>
+struct ei_assign_impl<Derived1, Derived2, SliceVectorization, NoUnrolling>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
+    const int packetAlignedMask = packetSize - 1;
+    const int innerSize = dst.innerSize();
+    const int outerSize = dst.outerSize();
+    const int alignedStep = (packetSize - dst.stride() % packetSize) & packetAlignedMask;
+    int alignedStart = ei_assign_traits<Derived1,Derived2>::DstIsAligned ? 0
+                     : ei_alignmentOffset(&dst.coeffRef(0,0), innerSize);
+
+    for(int i = 0; i < outerSize; ++i)
+    {
+      const int alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask);
+
+      // do the non-vectorizable part of the assignment
+      for (int index = 0; index<alignedStart ; ++index)
+      {
+        if(Derived1::Flags&RowMajorBit)
+          dst.copyCoeff(i, index, src);
+        else
+          dst.copyCoeff(index, i, src);
+      }
+
+      // do the vectorizable part of the assignment
+      for (int index = alignedStart; index<alignedEnd; index+=packetSize)
+      {
+        if(Derived1::Flags&RowMajorBit)
+          dst.template copyPacket<Derived2, Aligned, Unaligned>(i, index, src);
+        else
+          dst.template copyPacket<Derived2, Aligned, Unaligned>(index, i, src);
+      }
+
+      // do the non-vectorizable part of the assignment
+      for (int index = alignedEnd; index<innerSize ; ++index)
+      {
+        if(Derived1::Flags&RowMajorBit)
+          dst.copyCoeff(i, index, src);
+        else
+          dst.copyCoeff(index, i, src);
+      }
+
+      alignedStart = std::min<int>((alignedStart+alignedStep)%packetSize, innerSize);
+    }
+  }
+};
+
+/***************************************************************************
+* Part 4 : implementation of MatrixBase methods
+***************************************************************************/
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>
+  ::lazyAssign(const MatrixBase<OtherDerived>& other)
+{
+  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename OtherDerived::Scalar>::ret),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+  ei_assert(rows() == other.rows() && cols() == other.cols());
+  ei_assign_impl<Derived, OtherDerived>::run(derived(),other.derived());
+  return derived();
+}
+
+template<typename Derived, typename OtherDerived,
+         bool EvalBeforeAssigning = (int(OtherDerived::Flags) & EvalBeforeAssigningBit) != 0,
+         bool NeedToTranspose = Derived::IsVectorAtCompileTime
+                && OtherDerived::IsVectorAtCompileTime
+                && int(Derived::RowsAtCompileTime) == int(OtherDerived::ColsAtCompileTime)
+                && int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime)
+                && int(Derived::SizeAtCompileTime) != 1>
+struct ei_assign_selector;
+
+template<typename Derived, typename OtherDerived>
+struct ei_assign_selector<Derived,OtherDerived,false,false> {
+  EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.derived()); }
+};
+template<typename Derived, typename OtherDerived>
+struct ei_assign_selector<Derived,OtherDerived,true,false> {
+  EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.eval()); }
+};
+template<typename Derived, typename OtherDerived>
+struct ei_assign_selector<Derived,OtherDerived,false,true> {
+  EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose()); }
+};
+template<typename Derived, typename OtherDerived>
+struct ei_assign_selector<Derived,OtherDerived,true,true> {
+  EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose().eval()); }
+};
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>
+  ::operator=(const MatrixBase<OtherDerived>& other)
+{
+  return ei_assign_selector<Derived,OtherDerived>::run(derived(), other.derived());
+}
+
+#endif // EIGEN_ASSIGN_H
diff --git a/extern/Eigen2/Eigen/src/Core/Block.h b/extern/Eigen2/Eigen/src/Core/Block.h
new file mode 100644
index 00000000000..7f422aa5c07
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Block.h
@@ -0,0 +1,752 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_BLOCK_H
+#define EIGEN_BLOCK_H
+
+/** \class Block
+  *
+  * \brief Expression of a fixed-size or dynamic-size block
+  *
+  * \param MatrixType the type of the object in which we are taking a block
+  * \param BlockRows the number of rows of the block we are taking at compile time (optional)
+  * \param BlockCols the number of columns of the block we are taking at compile time (optional)
+  * \param _PacketAccess allows to enforce aligned loads and stores if set to ForceAligned.
+  *                      The default is AsRequested. This parameter is internaly used by Eigen
+  *                      in expressions such as \code mat.block() += other; \endcode and most of
+  *                      the time this is the only way it is used.
+  * \param _DirectAccessStatus \internal used for partial specialization
+  *
+  * This class represents an expression of either a fixed-size or dynamic-size block. It is the return
+  * type of MatrixBase::block(int,int,int,int) and MatrixBase::block<int,int>(int,int) and
+  * most of the time this is the only way it is used.
+  *
+  * However, if you want to directly maniputate block expressions,
+  * for instance if you want to write a function returning such an expression, you
+  * will need to use this class.
+  *
+  * Here is an example illustrating the dynamic case:
+  * \include class_Block.cpp
+  * Output: \verbinclude class_Block.out
+  *
+  * \note Even though this expression has dynamic size, in the case where \a MatrixType
+  * has fixed size, this expression inherits a fixed maximal size which means that evaluating
+  * it does not cause a dynamic memory allocation.
+  *
+  * Here is an example illustrating the fixed-size case:
+  * \include class_FixedBlock.cpp
+  * Output: \verbinclude class_FixedBlock.out
+  *
+  * \sa MatrixBase::block(int,int,int,int), MatrixBase::block(int,int), class VectorBlock
+  */
+
+template<typename MatrixType, int BlockRows, int BlockCols, int _PacketAccess, int _DirectAccessStatus>
+struct ei_traits<Block<MatrixType, BlockRows, BlockCols, _PacketAccess, _DirectAccessStatus> >
+{
+  typedef typename ei_traits<MatrixType>::Scalar Scalar;
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum{
+    RowsAtCompileTime = ei_traits<MatrixType>::RowsAtCompileTime == 1 ? 1 : BlockRows,
+    ColsAtCompileTime = ei_traits<MatrixType>::ColsAtCompileTime == 1 ? 1 : BlockCols,
+    MaxRowsAtCompileTime = RowsAtCompileTime == 1 ? 1
+      : (BlockRows==Dynamic ? int(ei_traits<MatrixType>::MaxRowsAtCompileTime) : BlockRows),
+    MaxColsAtCompileTime = ColsAtCompileTime == 1 ? 1
+      : (BlockCols==Dynamic ? int(ei_traits<MatrixType>::MaxColsAtCompileTime) : BlockCols),
+    RowMajor = int(ei_traits<MatrixType>::Flags)&RowMajorBit,
+    InnerSize = RowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+    InnerMaxSize = RowMajor ? int(MaxColsAtCompileTime) : int(MaxRowsAtCompileTime),
+    MaskPacketAccessBit = (InnerMaxSize == Dynamic || (InnerSize >= ei_packet_traits<Scalar>::size))
+                        ? PacketAccessBit : 0,
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    Flags = (ei_traits<MatrixType>::Flags & (HereditaryBits | MaskPacketAccessBit | DirectAccessBit)) | FlagsLinearAccessBit,
+    CoeffReadCost = ei_traits<MatrixType>::CoeffReadCost,
+    PacketAccess = _PacketAccess
+  };
+  typedef typename ei_meta_if<int(PacketAccess)==ForceAligned,
+                 Block<MatrixType, BlockRows, BlockCols, _PacketAccess, _DirectAccessStatus>&,
+                 Block<MatrixType, BlockRows, BlockCols, ForceAligned, _DirectAccessStatus> >::ret AlignedDerivedType;
+};
+
+template<typename MatrixType, int BlockRows, int BlockCols, int PacketAccess, int _DirectAccessStatus> class Block
+  : public MatrixBase<Block<MatrixType, BlockRows, BlockCols, PacketAccess, _DirectAccessStatus> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
+
+    class InnerIterator;
+
+    /** Column or Row constructor
+      */
+    inline Block(const MatrixType& matrix, int i)
+      : m_matrix(matrix),
+        // It is a row if and only if BlockRows==1 and BlockCols==MatrixType::ColsAtCompileTime,
+        // and it is a column if and only if BlockRows==MatrixType::RowsAtCompileTime and BlockCols==1,
+        // all other cases are invalid.
+        // The case a 1x1 matrix seems ambiguous, but the result is the same anyway.
+        m_startRow( (BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) ? i : 0),
+        m_startCol( (BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
+        m_blockRows(matrix.rows()), // if it is a row, then m_blockRows has a fixed-size of 1, so no pb to try to overwrite it
+        m_blockCols(matrix.cols())  // same for m_blockCols
+    {
+      ei_assert( (i>=0) && (
+          ((BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) && i<matrix.rows())
+        ||((BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) && i<matrix.cols())));
+    }
+
+    /** Fixed-size constructor
+      */
+    inline Block(const MatrixType& matrix, int startRow, int startCol)
+      : m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
+        m_blockRows(matrix.rows()), m_blockCols(matrix.cols())
+    {
+      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+      ei_assert(startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= matrix.rows()
+          && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= matrix.cols());
+    }
+
+    /** Dynamic-size constructor
+      */
+    inline Block(const MatrixType& matrix,
+          int startRow, int startCol,
+          int blockRows, int blockCols)
+      : m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
+                          m_blockRows(blockRows), m_blockCols(blockCols)
+    {
+      ei_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
+          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
+      ei_assert(startRow >= 0 && blockRows >= 1 && startRow + blockRows <= matrix.rows()
+          && startCol >= 0 && blockCols >= 1 && startCol + blockCols <= matrix.cols());
+    }
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
+
+    inline int rows() const { return m_blockRows.value(); }
+    inline int cols() const { return m_blockCols.value(); }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived()
+               .coeffRef(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_matrix.const_cast_derived()
+             .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                       m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix
+             .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                    m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(int row, int col) const
+    {
+      return m_matrix.template packet<Unaligned>
+              (row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int row, int col, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<Unaligned>
+              (row + m_startRow.value(), col + m_startCol.value(), x);
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(int index) const
+    {
+      return m_matrix.template packet<Unaligned>
+              (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+               m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int index, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<Unaligned>
+         (m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+          m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), x);
+    }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+    const ei_int_if_dynamic<MatrixType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
+    const ei_int_if_dynamic<MatrixType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
+    const ei_int_if_dynamic<RowsAtCompileTime> m_blockRows;
+    const ei_int_if_dynamic<ColsAtCompileTime> m_blockCols;
+};
+
+/** \internal */
+template<typename MatrixType, int BlockRows, int BlockCols, int PacketAccess>
+class Block<MatrixType,BlockRows,BlockCols,PacketAccess,HasDirectAccess>
+  : public MapBase<Block<MatrixType, BlockRows, BlockCols,PacketAccess,HasDirectAccess> >
+{
+  public:
+
+    _EIGEN_GENERIC_PUBLIC_INTERFACE(Block, MapBase<Block>)
+
+    class InnerIterator;
+    typedef typename ei_traits<Block>::AlignedDerivedType AlignedDerivedType;
+    friend class Block<MatrixType,BlockRows,BlockCols,PacketAccess==AsRequested?ForceAligned:AsRequested,HasDirectAccess>;
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
+
+    AlignedDerivedType _convertToForceAligned()
+    {
+      return Block<MatrixType,BlockRows,BlockCols,ForceAligned,HasDirectAccess>
+                    (m_matrix, Base::m_data, Base::m_rows.value(), Base::m_cols.value());
+    }
+
+    /** Column or Row constructor
+      */
+    inline Block(const MatrixType& matrix, int i)
+      : Base(&matrix.const_cast_derived().coeffRef(
+              (BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) ? i : 0,
+              (BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
+             BlockRows==1 ? 1 : matrix.rows(),
+             BlockCols==1 ? 1 : matrix.cols()),
+        m_matrix(matrix)
+    {
+      ei_assert( (i>=0) && (
+          ((BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) && i<matrix.rows())
+        ||((BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) && i<matrix.cols())));
+    }
+
+    /** Fixed-size constructor
+      */
+    inline Block(const MatrixType& matrix, int startRow, int startCol)
+      : Base(&matrix.const_cast_derived().coeffRef(startRow,startCol)), m_matrix(matrix)
+    {
+      ei_assert(startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= matrix.rows()
+             && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= matrix.cols());
+    }
+
+    /** Dynamic-size constructor
+      */
+    inline Block(const MatrixType& matrix,
+          int startRow, int startCol,
+          int blockRows, int blockCols)
+      : Base(&matrix.const_cast_derived().coeffRef(startRow,startCol), blockRows, blockCols),
+        m_matrix(matrix)
+    {
+      ei_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
+             && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
+      ei_assert(startRow >= 0 && blockRows >= 1 && startRow + blockRows <= matrix.rows()
+             && startCol >= 0 && blockCols >= 1 && startCol + blockCols <= matrix.cols());
+    }
+
+    inline int stride(void) const { return m_matrix.stride(); }
+
+  protected:
+
+    /** \internal used by allowAligned() */
+    inline Block(const MatrixType& matrix, const Scalar* data, int blockRows, int blockCols)
+      : Base(data, blockRows, blockCols), m_matrix(matrix)
+    {}
+
+    const typename MatrixType::Nested m_matrix;
+};
+
+/** \returns a dynamic-size expression of a block in *this.
+  *
+  * \param startRow the first row in the block
+  * \param startCol the first column in the block
+  * \param blockRows the number of rows in the block
+  * \param blockCols the number of columns in the block
+  *
+  * \addexample BlockIntIntIntInt \label How to reference a sub-matrix (dynamic-size)
+  *
+  * Example: \include MatrixBase_block_int_int_int_int.cpp
+  * Output: \verbinclude MatrixBase_block_int_int_int_int.out
+  *
+  * \note Even though the returned expression has dynamic size, in the case
+  * when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
+  * which means that evaluating it does not cause a dynamic memory allocation.
+  *
+  * \sa class Block, block(int,int)
+  */
+template<typename Derived>
+inline typename BlockReturnType<Derived>::Type MatrixBase<Derived>
+  ::block(int startRow, int startCol, int blockRows, int blockCols)
+{
+  return typename BlockReturnType<Derived>::Type(derived(), startRow, startCol, blockRows, blockCols);
+}
+
+/** This is the const version of block(int,int,int,int). */
+template<typename Derived>
+inline const typename BlockReturnType<Derived>::Type MatrixBase<Derived>
+  ::block(int startRow, int startCol, int blockRows, int blockCols) const
+{
+  return typename BlockReturnType<Derived>::Type(derived(), startRow, startCol, blockRows, blockCols);
+}
+
+/** \returns a dynamic-size expression of a segment (i.e. a vector block) in *this.
+  *
+  * \only_for_vectors
+  *
+  * \addexample SegmentIntInt \label How to reference a sub-vector (dynamic size)
+  *
+  * \param start the first coefficient in the segment
+  * \param size the number of coefficients in the segment
+  *
+  * Example: \include MatrixBase_segment_int_int.cpp
+  * Output: \verbinclude MatrixBase_segment_int_int.out
+  *
+  * \note Even though the returned expression has dynamic size, in the case
+  * when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+  * which means that evaluating it does not cause a dynamic memory allocation.
+  *
+  * \sa class Block, segment(int)
+  */
+template<typename Derived>
+inline typename BlockReturnType<Derived>::SubVectorType MatrixBase<Derived>
+  ::segment(int start, int size)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename BlockReturnType<Derived>::SubVectorType(derived(), RowsAtCompileTime == 1 ? 0 : start,
+                                   ColsAtCompileTime == 1 ? 0 : start,
+                                   RowsAtCompileTime == 1 ? 1 : size,
+                                   ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** This is the const version of segment(int,int).*/
+template<typename Derived>
+inline const typename BlockReturnType<Derived>::SubVectorType
+MatrixBase<Derived>::segment(int start, int size) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return typename BlockReturnType<Derived>::SubVectorType(derived(), RowsAtCompileTime == 1 ? 0 : start,
+                                   ColsAtCompileTime == 1 ? 0 : start,
+                                   RowsAtCompileTime == 1 ? 1 : size,
+                                   ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** \returns a dynamic-size expression of the first coefficients of *this.
+  *
+  * \only_for_vectors
+  *
+  * \param size the number of coefficients in the block
+  *
+  * \addexample BlockInt \label How to reference a sub-vector (fixed-size)
+  *
+  * Example: \include MatrixBase_start_int.cpp
+  * Output: \verbinclude MatrixBase_start_int.out
+  *
+  * \note Even though the returned expression has dynamic size, in the case
+  * when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+  * which means that evaluating it does not cause a dynamic memory allocation.
+  *
+  * \sa class Block, block(int,int)
+  */
+template<typename Derived>
+inline typename BlockReturnType<Derived,Dynamic>::SubVectorType
+MatrixBase<Derived>::start(int size)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,
+               RowsAtCompileTime == 1 ? 1 : Dynamic,
+               ColsAtCompileTime == 1 ? 1 : Dynamic>
+              (derived(), 0, 0,
+               RowsAtCompileTime == 1 ? 1 : size,
+               ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** This is the const version of start(int).*/
+template<typename Derived>
+inline const typename BlockReturnType<Derived,Dynamic>::SubVectorType
+MatrixBase<Derived>::start(int size) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,
+               RowsAtCompileTime == 1 ? 1 : Dynamic,
+               ColsAtCompileTime == 1 ? 1 : Dynamic>
+              (derived(), 0, 0,
+               RowsAtCompileTime == 1 ? 1 : size,
+               ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** \returns a dynamic-size expression of the last coefficients of *this.
+  *
+  * \only_for_vectors
+  *
+  * \param size the number of coefficients in the block
+  *
+  * \addexample BlockEnd \label How to reference the end of a vector (fixed-size)
+  *
+  * Example: \include MatrixBase_end_int.cpp
+  * Output: \verbinclude MatrixBase_end_int.out
+  *
+  * \note Even though the returned expression has dynamic size, in the case
+  * when it is applied to a fixed-size vector, it inherits a fixed maximal size,
+  * which means that evaluating it does not cause a dynamic memory allocation.
+  *
+  * \sa class Block, block(int,int)
+  */
+template<typename Derived>
+inline typename BlockReturnType<Derived,Dynamic>::SubVectorType
+MatrixBase<Derived>::end(int size)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,
+               RowsAtCompileTime == 1 ? 1 : Dynamic,
+               ColsAtCompileTime == 1 ? 1 : Dynamic>
+              (derived(),
+               RowsAtCompileTime == 1 ? 0 : rows() - size,
+               ColsAtCompileTime == 1 ? 0 : cols() - size,
+               RowsAtCompileTime == 1 ? 1 : size,
+               ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** This is the const version of end(int).*/
+template<typename Derived>
+inline const typename BlockReturnType<Derived,Dynamic>::SubVectorType
+MatrixBase<Derived>::end(int size) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,
+               RowsAtCompileTime == 1 ? 1 : Dynamic,
+               ColsAtCompileTime == 1 ? 1 : Dynamic>
+              (derived(),
+               RowsAtCompileTime == 1 ? 0 : rows() - size,
+               ColsAtCompileTime == 1 ? 0 : cols() - size,
+               RowsAtCompileTime == 1 ? 1 : size,
+               ColsAtCompileTime == 1 ? 1 : size);
+}
+
+/** \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this
+  *
+  * \only_for_vectors
+  *
+  * The template parameter \a Size is the number of coefficients in the block
+  *
+  * \param start the index of the first element of the sub-vector
+  *
+  * Example: \include MatrixBase_template_int_segment.cpp
+  * Output: \verbinclude MatrixBase_template_int_segment.out
+  *
+  * \sa class Block
+  */
+template<typename Derived>
+template<int Size>
+inline typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::segment(int start)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,  (RowsAtCompileTime == 1 ? 1 : Size),
+                         (ColsAtCompileTime == 1 ? 1 : Size)>
+              (derived(), RowsAtCompileTime == 1 ? 0 : start,
+                          ColsAtCompileTime == 1 ? 0 : start);
+}
+
+/** This is the const version of segment<int>(int).*/
+template<typename Derived>
+template<int Size>
+inline const typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::segment(int start) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived,  (RowsAtCompileTime == 1 ? 1 : Size),
+                         (ColsAtCompileTime == 1 ? 1 : Size)>
+              (derived(), RowsAtCompileTime == 1 ? 0 : start,
+                          ColsAtCompileTime == 1 ? 0 : start);
+}
+
+/** \returns a fixed-size expression of the first coefficients of *this.
+  *
+  * \only_for_vectors
+  *
+  * The template parameter \a Size is the number of coefficients in the block
+  *
+  * \addexample BlockStart \label How to reference the start of a vector (fixed-size)
+  *
+  * Example: \include MatrixBase_template_int_start.cpp
+  * Output: \verbinclude MatrixBase_template_int_start.out
+  *
+  * \sa class Block
+  */
+template<typename Derived>
+template<int Size>
+inline typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::start()
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
+                        (ColsAtCompileTime == 1 ? 1 : Size)>(derived(), 0, 0);
+}
+
+/** This is the const version of start<int>().*/
+template<typename Derived>
+template<int Size>
+inline const typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::start() const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
+                        (ColsAtCompileTime == 1 ? 1 : Size)>(derived(), 0, 0);
+}
+
+/** \returns a fixed-size expression of the last coefficients of *this.
+  *
+  * \only_for_vectors
+  *
+  * The template parameter \a Size is the number of coefficients in the block
+  *
+  * Example: \include MatrixBase_template_int_end.cpp
+  * Output: \verbinclude MatrixBase_template_int_end.out
+  *
+  * \sa class Block
+  */
+template<typename Derived>
+template<int Size>
+inline typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::end()
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived, RowsAtCompileTime == 1 ? 1 : Size,
+                        ColsAtCompileTime == 1 ? 1 : Size>
+           (derived(),
+            RowsAtCompileTime == 1 ? 0 : rows() - Size,
+            ColsAtCompileTime == 1 ? 0 : cols() - Size);
+}
+
+/** This is the const version of end<int>.*/
+template<typename Derived>
+template<int Size>
+inline const typename BlockReturnType<Derived,Size>::SubVectorType
+MatrixBase<Derived>::end() const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return Block<Derived, RowsAtCompileTime == 1 ? 1 : Size,
+                        ColsAtCompileTime == 1 ? 1 : Size>
+           (derived(),
+            RowsAtCompileTime == 1 ? 0 : rows() - Size,
+            ColsAtCompileTime == 1 ? 0 : cols() - Size);
+}
+
+/** \returns a dynamic-size expression of a corner of *this.
+  *
+  * \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight,
+  * \a Eigen::BottomLeft, \a Eigen::BottomRight.
+  * \param cRows the number of rows in the corner
+  * \param cCols the number of columns in the corner
+  *
+  * \addexample BlockCornerDynamicSize \label How to reference a sub-corner of a matrix
+  *
+  * Example: \include MatrixBase_corner_enum_int_int.cpp
+  * Output: \verbinclude MatrixBase_corner_enum_int_int.out
+  *
+  * \note Even though the returned expression has dynamic size, in the case
+  * when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
+  * which means that evaluating it does not cause a dynamic memory allocation.
+  *
+  * \sa class Block, block(int,int,int,int)
+  */
+template<typename Derived>
+inline typename BlockReturnType<Derived>::Type MatrixBase<Derived>
+  ::corner(CornerType type, int cRows, int cCols)
+{
+  switch(type)
+  {
+    default:
+      ei_assert(false && "Bad corner type.");
+    case TopLeft:
+      return typename BlockReturnType<Derived>::Type(derived(), 0, 0, cRows, cCols);
+    case TopRight:
+      return typename BlockReturnType<Derived>::Type(derived(), 0, cols() - cCols, cRows, cCols);
+    case BottomLeft:
+      return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, 0, cRows, cCols);
+    case BottomRight:
+      return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+  }
+}
+
+/** This is the const version of corner(CornerType, int, int).*/
+template<typename Derived>
+inline const typename BlockReturnType<Derived>::Type
+MatrixBase<Derived>::corner(CornerType type, int cRows, int cCols) const
+{
+  switch(type)
+  {
+    default:
+      ei_assert(false && "Bad corner type.");
+    case TopLeft:
+      return typename BlockReturnType<Derived>::Type(derived(), 0, 0, cRows, cCols);
+    case TopRight:
+      return typename BlockReturnType<Derived>::Type(derived(), 0, cols() - cCols, cRows, cCols);
+    case BottomLeft:
+      return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, 0, cRows, cCols);
+    case BottomRight:
+      return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+  }
+}
+
+/** \returns a fixed-size expression of a corner of *this.
+  *
+  * \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight,
+  * \a Eigen::BottomLeft, \a Eigen::BottomRight.
+  *
+  * The template parameters CRows and CCols arethe number of rows and columns in the corner.
+  *
+  * Example: \include MatrixBase_template_int_int_corner_enum.cpp
+  * Output: \verbinclude MatrixBase_template_int_int_corner_enum.out
+  *
+  * \sa class Block, block(int,int,int,int)
+  */
+template<typename Derived>
+template<int CRows, int CCols>
+inline typename BlockReturnType<Derived, CRows, CCols>::Type
+MatrixBase<Derived>::corner(CornerType type)
+{
+  switch(type)
+  {
+    default:
+      ei_assert(false && "Bad corner type.");
+    case TopLeft:
+      return Block<Derived, CRows, CCols>(derived(), 0, 0);
+    case TopRight:
+      return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols);
+    case BottomLeft:
+      return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0);
+    case BottomRight:
+      return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols);
+  }
+}
+
+/** This is the const version of corner<int, int>(CornerType).*/
+template<typename Derived>
+template<int CRows, int CCols>
+inline const typename BlockReturnType<Derived, CRows, CCols>::Type
+MatrixBase<Derived>::corner(CornerType type) const
+{
+  switch(type)
+  {
+    default:
+      ei_assert(false && "Bad corner type.");
+    case TopLeft:
+      return Block<Derived, CRows, CCols>(derived(), 0, 0);
+    case TopRight:
+      return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols);
+    case BottomLeft:
+      return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0);
+    case BottomRight:
+      return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols);
+  }
+}
+
+/** \returns a fixed-size expression of a block in *this.
+  *
+  * The template parameters \a BlockRows and \a BlockCols are the number of
+  * rows and columns in the block.
+  *
+  * \param startRow the first row in the block
+  * \param startCol the first column in the block
+  *
+  * \addexample BlockSubMatrixFixedSize \label How to reference a sub-matrix (fixed-size)
+  *
+  * Example: \include MatrixBase_block_int_int.cpp
+  * Output: \verbinclude MatrixBase_block_int_int.out
+  *
+  * \note since block is a templated member, the keyword template has to be used
+  * if the matrix type is also a template parameter: \code m.template block<3,3>(1,1); \endcode
+  *
+  * \sa class Block, block(int,int,int,int)
+  */
+template<typename Derived>
+template<int BlockRows, int BlockCols>
+inline typename BlockReturnType<Derived, BlockRows, BlockCols>::Type
+MatrixBase<Derived>::block(int startRow, int startCol)
+{
+  return Block<Derived, BlockRows, BlockCols>(derived(), startRow, startCol);
+}
+
+/** This is the const version of block<>(int, int). */
+template<typename Derived>
+template<int BlockRows, int BlockCols>
+inline const typename BlockReturnType<Derived, BlockRows, BlockCols>::Type
+MatrixBase<Derived>::block(int startRow, int startCol) const
+{
+  return Block<Derived, BlockRows, BlockCols>(derived(), startRow, startCol);
+}
+
+/** \returns an expression of the \a i-th column of *this. Note that the numbering starts at 0.
+  *
+  * \addexample BlockColumn \label How to reference a single column of a matrix
+  *
+  * Example: \include MatrixBase_col.cpp
+  * Output: \verbinclude MatrixBase_col.out
+  *
+  * \sa row(), class Block */
+template<typename Derived>
+inline typename MatrixBase<Derived>::ColXpr
+MatrixBase<Derived>::col(int i)
+{
+  return ColXpr(derived(), i);
+}
+
+/** This is the const version of col(). */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::ColXpr
+MatrixBase<Derived>::col(int i) const
+{
+  return ColXpr(derived(), i);
+}
+
+/** \returns an expression of the \a i-th row of *this. Note that the numbering starts at 0.
+  *
+  * \addexample BlockRow \label How to reference a single row of a matrix
+  *
+  * Example: \include MatrixBase_row.cpp
+  * Output: \verbinclude MatrixBase_row.out
+  *
+  * \sa col(), class Block */
+template<typename Derived>
+inline typename MatrixBase<Derived>::RowXpr
+MatrixBase<Derived>::row(int i)
+{
+  return RowXpr(derived(), i);
+}
+
+/** This is the const version of row(). */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::RowXpr
+MatrixBase<Derived>::row(int i) const
+{
+  return RowXpr(derived(), i);
+}
+
+#endif // EIGEN_BLOCK_H
diff --git a/extern/Eigen2/Eigen/src/Core/CacheFriendlyProduct.h b/extern/Eigen2/Eigen/src/Core/CacheFriendlyProduct.h
new file mode 100644
index 00000000000..b1362b0a80c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CacheFriendlyProduct.h
@@ -0,0 +1,753 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CACHE_FRIENDLY_PRODUCT_H
+#define EIGEN_CACHE_FRIENDLY_PRODUCT_H
+
+template <int L2MemorySize,typename Scalar>
+struct ei_L2_block_traits {
+  enum {width = 8 * ei_meta_sqrt<L2MemorySize/(64*sizeof(Scalar))>::ret };
+};
+
+#ifndef EIGEN_EXTERN_INSTANTIATIONS
+
+template<typename Scalar>
+static void ei_cache_friendly_product(
+  int _rows, int _cols, int depth,
+  bool _lhsRowMajor, const Scalar* _lhs, int _lhsStride,
+  bool _rhsRowMajor, const Scalar* _rhs, int _rhsStride,
+  bool resRowMajor, Scalar* res, int resStride)
+{
+  const Scalar* EIGEN_RESTRICT lhs;
+  const Scalar* EIGEN_RESTRICT rhs;
+  int lhsStride, rhsStride, rows, cols;
+  bool lhsRowMajor;
+
+  if (resRowMajor)
+  {
+    lhs = _rhs;
+    rhs = _lhs;
+    lhsStride = _rhsStride;
+    rhsStride = _lhsStride;
+    cols = _rows;
+    rows = _cols;
+    lhsRowMajor = !_rhsRowMajor;
+    ei_assert(_lhsRowMajor);
+  }
+  else
+  {
+    lhs = _lhs;
+    rhs = _rhs;
+    lhsStride = _lhsStride;
+    rhsStride = _rhsStride;
+    rows = _rows;
+    cols = _cols;
+    lhsRowMajor = _lhsRowMajor;
+    ei_assert(!_rhsRowMajor);
+  }
+
+  typedef typename ei_packet_traits<Scalar>::type PacketType;
+
+  enum {
+    PacketSize = sizeof(PacketType)/sizeof(Scalar),
+    #if (defined __i386__)
+    // i386 architecture provides only 8 xmm registers,
+    // so let's reduce the max number of rows processed at once.
+    MaxBlockRows = 4,
+    MaxBlockRows_ClampingMask = 0xFFFFFC,
+    #else
+    MaxBlockRows = 8,
+    MaxBlockRows_ClampingMask = 0xFFFFF8,
+    #endif
+    // maximal size of the blocks fitted in L2 cache
+    MaxL2BlockSize = ei_L2_block_traits<EIGEN_TUNE_FOR_CPU_CACHE_SIZE,Scalar>::width
+  };
+
+  const bool resIsAligned = (PacketSize==1) || (((resStride%PacketSize) == 0) && (size_t(res)%16==0));
+
+  const int remainingSize = depth % PacketSize;
+  const int size = depth - remainingSize; // third dimension of the product clamped to packet boundaries
+  const int l2BlockRows = MaxL2BlockSize > rows ? rows : MaxL2BlockSize;
+  const int l2BlockCols = MaxL2BlockSize > cols ? cols : MaxL2BlockSize;
+  const int l2BlockSize = MaxL2BlockSize > size ? size : MaxL2BlockSize;
+  const int l2BlockSizeAligned = (1 + std::max(l2BlockSize,l2BlockCols)/PacketSize)*PacketSize;
+  const bool needRhsCopy = (PacketSize>1) && ((rhsStride%PacketSize!=0) || (size_t(rhs)%16!=0));
+  Scalar* EIGEN_RESTRICT block = 0;
+  const int allocBlockSize = l2BlockRows*size;
+  block = ei_aligned_stack_new(Scalar, allocBlockSize);
+  Scalar* EIGEN_RESTRICT rhsCopy
+    = ei_aligned_stack_new(Scalar, l2BlockSizeAligned*l2BlockSizeAligned);
+
+  // loops on each L2 cache friendly blocks of the result
+  for(int l2i=0; l2i<rows; l2i+=l2BlockRows)
+  {
+    const int l2blockRowEnd = std::min(l2i+l2BlockRows, rows);
+    const int l2blockRowEndBW = l2blockRowEnd & MaxBlockRows_ClampingMask;    // end of the rows aligned to bw
+    const int l2blockRemainingRows = l2blockRowEnd - l2blockRowEndBW;         // number of remaining rows
+    //const int l2blockRowEndBWPlusOne = l2blockRowEndBW + (l2blockRemainingRows?0:MaxBlockRows);
+
+    // build a cache friendly blocky matrix
+    int count = 0;
+
+    // copy l2blocksize rows of m_lhs to blocks of ps x bw
+    for(int l2k=0; l2k<size; l2k+=l2BlockSize)
+    {
+      const int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
+
+      for (int i = l2i; i<l2blockRowEndBW/*PlusOne*/; i+=MaxBlockRows)
+      {
+        // TODO merge the "if l2blockRemainingRows" using something like:
+        // const int blockRows = std::min(i+MaxBlockRows, rows) - i;
+
+        for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
+        {
+          // TODO write these loops using meta unrolling
+          // negligible for large matrices but useful for small ones
+          if (lhsRowMajor)
+          {
+            for (int w=0; w<MaxBlockRows; ++w)
+              for (int s=0; s<PacketSize; ++s)
+                block[count++] = lhs[(i+w)*lhsStride + (k+s)];
+          }
+          else
+          {
+            for (int w=0; w<MaxBlockRows; ++w)
+              for (int s=0; s<PacketSize; ++s)
+                block[count++] = lhs[(i+w) + (k+s)*lhsStride];
+          }
+        }
+      }
+      if (l2blockRemainingRows>0)
+      {
+        for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
+        {
+          if (lhsRowMajor)
+          {
+            for (int w=0; w<l2blockRemainingRows; ++w)
+              for (int s=0; s<PacketSize; ++s)
+                block[count++] = lhs[(l2blockRowEndBW+w)*lhsStride + (k+s)];
+          }
+          else
+          {
+            for (int w=0; w<l2blockRemainingRows; ++w)
+              for (int s=0; s<PacketSize; ++s)
+                block[count++] = lhs[(l2blockRowEndBW+w) + (k+s)*lhsStride];
+          }
+        }
+      }
+    }
+
+    for(int l2j=0; l2j<cols; l2j+=l2BlockCols)
+    {
+      int l2blockColEnd = std::min(l2j+l2BlockCols, cols);
+
+      for(int l2k=0; l2k<size; l2k+=l2BlockSize)
+      {
+        // acumulate bw rows of lhs time a single column of rhs to a bw x 1 block of res
+        int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
+
+        // if not aligned, copy the rhs block
+        if (needRhsCopy)
+          for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
+          {
+            ei_internal_assert(l2BlockSizeAligned*(l1j-l2j)+(l2blockSizeEnd-l2k) < l2BlockSizeAligned*l2BlockSizeAligned);
+            memcpy(rhsCopy+l2BlockSizeAligned*(l1j-l2j),&(rhs[l1j*rhsStride+l2k]),(l2blockSizeEnd-l2k)*sizeof(Scalar));
+          }
+
+        // for each bw x 1 result's block
+        for(int l1i=l2i; l1i<l2blockRowEndBW; l1i+=MaxBlockRows)
+        {
+          int offsetblock = l2k * (l2blockRowEnd-l2i) + (l1i-l2i)*(l2blockSizeEnd-l2k) - l2k*MaxBlockRows;
+          const Scalar* EIGEN_RESTRICT localB = &block[offsetblock];
+
+          for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
+          {
+            const Scalar* EIGEN_RESTRICT rhsColumn;
+            if (needRhsCopy)
+              rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
+            else
+              rhsColumn = &(rhs[l1j*rhsStride]);
+
+            PacketType dst[MaxBlockRows];
+            dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
+            if (MaxBlockRows==8)
+              dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
+
+            PacketType tmp;
+
+            for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
+            {
+              tmp = ei_ploadu(&rhsColumn[k]);
+              PacketType A0, A1, A2, A3, A4, A5;
+              A0 = ei_pload(localB + k*MaxBlockRows);
+              A1 = ei_pload(localB + k*MaxBlockRows+1*PacketSize);
+              A2 = ei_pload(localB + k*MaxBlockRows+2*PacketSize);
+              A3 = ei_pload(localB + k*MaxBlockRows+3*PacketSize);
+              if (MaxBlockRows==8) A4 = ei_pload(localB + k*MaxBlockRows+4*PacketSize);
+              if (MaxBlockRows==8) A5 = ei_pload(localB + k*MaxBlockRows+5*PacketSize);
+              dst[0] = ei_pmadd(tmp, A0, dst[0]);
+              if (MaxBlockRows==8) A0 = ei_pload(localB + k*MaxBlockRows+6*PacketSize);
+              dst[1] = ei_pmadd(tmp, A1, dst[1]);
+              if (MaxBlockRows==8) A1 = ei_pload(localB + k*MaxBlockRows+7*PacketSize);
+              dst[2] = ei_pmadd(tmp, A2, dst[2]);
+              dst[3] = ei_pmadd(tmp, A3, dst[3]);
+              if (MaxBlockRows==8)
+              {
+                dst[4] = ei_pmadd(tmp, A4, dst[4]);
+                dst[5] = ei_pmadd(tmp, A5, dst[5]);
+                dst[6] = ei_pmadd(tmp, A0, dst[6]);
+                dst[7] = ei_pmadd(tmp, A1, dst[7]);
+              }
+            }
+
+            Scalar* EIGEN_RESTRICT localRes = &(res[l1i + l1j*resStride]);
+
+            if (PacketSize>1 && resIsAligned)
+            {
+              // the result is aligned: let's do packet reduction
+              ei_pstore(&(localRes[0]), ei_padd(ei_pload(&(localRes[0])), ei_preduxp(&dst[0])));
+              if (PacketSize==2)
+                ei_pstore(&(localRes[2]), ei_padd(ei_pload(&(localRes[2])), ei_preduxp(&(dst[2]))));
+              if (MaxBlockRows==8)
+              {
+                ei_pstore(&(localRes[4]), ei_padd(ei_pload(&(localRes[4])), ei_preduxp(&(dst[4]))));
+                if (PacketSize==2)
+                  ei_pstore(&(localRes[6]), ei_padd(ei_pload(&(localRes[6])), ei_preduxp(&(dst[6]))));
+              }
+            }
+            else
+            {
+              // not aligned => per coeff packet reduction
+              localRes[0] += ei_predux(dst[0]);
+              localRes[1] += ei_predux(dst[1]);
+              localRes[2] += ei_predux(dst[2]);
+              localRes[3] += ei_predux(dst[3]);
+              if (MaxBlockRows==8)
+              {
+                localRes[4] += ei_predux(dst[4]);
+                localRes[5] += ei_predux(dst[5]);
+                localRes[6] += ei_predux(dst[6]);
+                localRes[7] += ei_predux(dst[7]);
+              }
+            }
+          }
+        }
+        if (l2blockRemainingRows>0)
+        {
+          int offsetblock = l2k * (l2blockRowEnd-l2i) + (l2blockRowEndBW-l2i)*(l2blockSizeEnd-l2k) - l2k*l2blockRemainingRows;
+          const Scalar* localB = &block[offsetblock];
+
+          for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
+          {
+            const Scalar* EIGEN_RESTRICT rhsColumn;
+            if (needRhsCopy)
+              rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
+            else
+              rhsColumn = &(rhs[l1j*rhsStride]);
+
+            PacketType dst[MaxBlockRows];
+            dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
+            if (MaxBlockRows==8)
+              dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
+
+            // let's declare a few other temporary registers
+            PacketType tmp;
+
+            for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
+            {
+              tmp = ei_pload(&rhsColumn[k]);
+
+                                           dst[0] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows             ])), dst[0]);
+              if (l2blockRemainingRows>=2) dst[1] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+  PacketSize])), dst[1]);
+              if (l2blockRemainingRows>=3) dst[2] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+2*PacketSize])), dst[2]);
+              if (l2blockRemainingRows>=4) dst[3] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+3*PacketSize])), dst[3]);
+              if (MaxBlockRows==8)
+              {
+                if (l2blockRemainingRows>=5) dst[4] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+4*PacketSize])), dst[4]);
+                if (l2blockRemainingRows>=6) dst[5] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+5*PacketSize])), dst[5]);
+                if (l2blockRemainingRows>=7) dst[6] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+6*PacketSize])), dst[6]);
+                if (l2blockRemainingRows>=8) dst[7] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+7*PacketSize])), dst[7]);
+              }
+            }
+
+            Scalar* EIGEN_RESTRICT localRes = &(res[l2blockRowEndBW + l1j*resStride]);
+
+            // process the remaining rows once at a time
+                                         localRes[0] += ei_predux(dst[0]);
+            if (l2blockRemainingRows>=2) localRes[1] += ei_predux(dst[1]);
+            if (l2blockRemainingRows>=3) localRes[2] += ei_predux(dst[2]);
+            if (l2blockRemainingRows>=4) localRes[3] += ei_predux(dst[3]);
+            if (MaxBlockRows==8)
+            {
+              if (l2blockRemainingRows>=5) localRes[4] += ei_predux(dst[4]);
+              if (l2blockRemainingRows>=6) localRes[5] += ei_predux(dst[5]);
+              if (l2blockRemainingRows>=7) localRes[6] += ei_predux(dst[6]);
+              if (l2blockRemainingRows>=8) localRes[7] += ei_predux(dst[7]);
+            }
+
+          }
+        }
+      }
+    }
+  }
+  if (PacketSize>1 && remainingSize)
+  {
+    if (lhsRowMajor)
+    {
+      for (int j=0; j<cols; ++j)
+        for (int i=0; i<rows; ++i)
+        {
+          Scalar tmp = lhs[i*lhsStride+size] * rhs[j*rhsStride+size];
+          // FIXME this loop get vectorized by the compiler !
+          for (int k=1; k<remainingSize; ++k)
+            tmp += lhs[i*lhsStride+size+k] * rhs[j*rhsStride+size+k];
+          res[i+j*resStride] += tmp;
+        }
+    }
+    else
+    {
+      for (int j=0; j<cols; ++j)
+        for (int i=0; i<rows; ++i)
+        {
+          Scalar tmp = lhs[i+size*lhsStride] * rhs[j*rhsStride+size];
+          for (int k=1; k<remainingSize; ++k)
+            tmp += lhs[i+(size+k)*lhsStride] * rhs[j*rhsStride+size+k];
+          res[i+j*resStride] += tmp;
+        }
+    }
+  }
+
+  ei_aligned_stack_delete(Scalar, block, allocBlockSize);
+  ei_aligned_stack_delete(Scalar, rhsCopy, l2BlockSizeAligned*l2BlockSizeAligned);
+}
+
+#endif // EIGEN_EXTERN_INSTANTIATIONS
+
+/* Optimized col-major matrix * vector product:
+ * This algorithm processes 4 columns at onces that allows to both reduce
+ * the number of load/stores of the result by a factor 4 and to reduce
+ * the instruction dependency. Moreover, we know that all bands have the
+ * same alignment pattern.
+ * TODO: since rhs gets evaluated only once, no need to evaluate it
+ */
+template<typename Scalar, typename RhsType>
+static EIGEN_DONT_INLINE void ei_cache_friendly_product_colmajor_times_vector(
+  int size,
+  const Scalar* lhs, int lhsStride,
+  const RhsType& rhs,
+  Scalar* res)
+{
+  #ifdef _EIGEN_ACCUMULATE_PACKETS
+  #error _EIGEN_ACCUMULATE_PACKETS has already been defined
+  #endif
+  #define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) \
+    ei_pstore(&res[j], \
+      ei_padd(ei_pload(&res[j]), \
+        ei_padd( \
+          ei_padd(ei_pmul(ptmp0,EIGEN_CAT(ei_ploa , A0)(&lhs0[j])), \
+                  ei_pmul(ptmp1,EIGEN_CAT(ei_ploa , A13)(&lhs1[j]))), \
+          ei_padd(ei_pmul(ptmp2,EIGEN_CAT(ei_ploa , A2)(&lhs2[j])), \
+                  ei_pmul(ptmp3,EIGEN_CAT(ei_ploa , A13)(&lhs3[j]))) )))
+
+  typedef typename ei_packet_traits<Scalar>::type Packet;
+  const int PacketSize = sizeof(Packet)/sizeof(Scalar);
+
+  enum { AllAligned = 0, EvenAligned, FirstAligned, NoneAligned };
+  const int columnsAtOnce = 4;
+  const int peels = 2;
+  const int PacketAlignedMask = PacketSize-1;
+  const int PeelAlignedMask = PacketSize*peels-1;
+
+  // How many coeffs of the result do we have to skip to be aligned.
+  // Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
+  const int alignedStart = ei_alignmentOffset(res,size);
+  const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
+  const int peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
+
+  const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
+  int alignmentPattern = alignmentStep==0 ? AllAligned
+                       : alignmentStep==(PacketSize/2) ? EvenAligned
+                       : FirstAligned;
+
+  // we cannot assume the first element is aligned because of sub-matrices
+  const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
+
+  // find how many columns do we have to skip to be aligned with the result (if possible)
+  int skipColumns = 0;
+  if (PacketSize>1)
+  {
+    ei_internal_assert(size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0 || size<PacketSize);
+
+    while (skipColumns<PacketSize &&
+           alignedStart != ((lhsAlignmentOffset + alignmentStep*skipColumns)%PacketSize))
+      ++skipColumns;
+    if (skipColumns==PacketSize)
+    {
+      // nothing can be aligned, no need to skip any column
+      alignmentPattern = NoneAligned;
+      skipColumns = 0;
+    }
+    else
+    {
+      skipColumns = std::min(skipColumns,rhs.size());
+      // note that the skiped columns are processed later.
+    }
+
+    ei_internal_assert((alignmentPattern==NoneAligned) || (size_t(lhs+alignedStart+lhsStride*skipColumns)%sizeof(Packet))==0);
+  }
+
+  int offset1 = (FirstAligned && alignmentStep==1?3:1);
+  int offset3 = (FirstAligned && alignmentStep==1?1:3);
+
+  int columnBound = ((rhs.size()-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
+  for (int i=skipColumns; i<columnBound; i+=columnsAtOnce)
+  {
+    Packet ptmp0 = ei_pset1(rhs[i]),   ptmp1 = ei_pset1(rhs[i+offset1]),
+           ptmp2 = ei_pset1(rhs[i+2]), ptmp3 = ei_pset1(rhs[i+offset3]);
+
+    // this helps a lot generating better binary code
+    const Scalar *lhs0 = lhs + i*lhsStride, *lhs1 = lhs + (i+offset1)*lhsStride,
+                 *lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
+
+    if (PacketSize>1)
+    {
+      /* explicit vectorization */
+      // process initial unaligned coeffs
+      for (int j=0; j<alignedStart; ++j)
+        res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
+
+      if (alignedSize>alignedStart)
+      {
+        switch(alignmentPattern)
+        {
+          case AllAligned:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,d,d);
+            break;
+          case EvenAligned:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,du,d);
+            break;
+          case FirstAligned:
+            if(peels>1)
+            {
+              Packet A00, A01, A02, A03, A10, A11, A12, A13;
+
+              A01 = ei_pload(&lhs1[alignedStart-1]);
+              A02 = ei_pload(&lhs2[alignedStart-2]);
+              A03 = ei_pload(&lhs3[alignedStart-3]);
+
+              for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
+              {
+                A11 = ei_pload(&lhs1[j-1+PacketSize]);  ei_palign<1>(A01,A11);
+                A12 = ei_pload(&lhs2[j-2+PacketSize]);  ei_palign<2>(A02,A12);
+                A13 = ei_pload(&lhs3[j-3+PacketSize]);  ei_palign<3>(A03,A13);
+
+                A00 = ei_pload (&lhs0[j]);
+                A10 = ei_pload (&lhs0[j+PacketSize]);
+                A00 = ei_pmadd(ptmp0, A00, ei_pload(&res[j]));
+                A10 = ei_pmadd(ptmp0, A10, ei_pload(&res[j+PacketSize]));
+
+                A00 = ei_pmadd(ptmp1, A01, A00);
+                A01 = ei_pload(&lhs1[j-1+2*PacketSize]);  ei_palign<1>(A11,A01);
+                A00 = ei_pmadd(ptmp2, A02, A00);
+                A02 = ei_pload(&lhs2[j-2+2*PacketSize]);  ei_palign<2>(A12,A02);
+                A00 = ei_pmadd(ptmp3, A03, A00);
+                ei_pstore(&res[j],A00);
+                A03 = ei_pload(&lhs3[j-3+2*PacketSize]);  ei_palign<3>(A13,A03);
+                A10 = ei_pmadd(ptmp1, A11, A10);
+                A10 = ei_pmadd(ptmp2, A12, A10);
+                A10 = ei_pmadd(ptmp3, A13, A10);
+                ei_pstore(&res[j+PacketSize],A10);
+              }
+            }
+            for (int j = peeledSize; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,du,du);
+            break;
+          default:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(du,du,du);
+            break;
+        }
+      }
+    } // end explicit vectorization
+
+    /* process remaining coeffs (or all if there is no explicit vectorization) */
+    for (int j=alignedSize; j<size; ++j)
+      res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
+  }
+
+  // process remaining first and last columns (at most columnsAtOnce-1)
+  int end = rhs.size();
+  int start = columnBound;
+  do
+  {
+    for (int i=start; i<end; ++i)
+    {
+      Packet ptmp0 = ei_pset1(rhs[i]);
+      const Scalar* lhs0 = lhs + i*lhsStride;
+
+      if (PacketSize>1)
+      {
+        /* explicit vectorization */
+        // process first unaligned result's coeffs
+        for (int j=0; j<alignedStart; ++j)
+          res[j] += ei_pfirst(ptmp0) * lhs0[j];
+
+        // process aligned result's coeffs
+        if ((size_t(lhs0+alignedStart)%sizeof(Packet))==0)
+          for (int j = alignedStart;j<alignedSize;j+=PacketSize)
+            ei_pstore(&res[j], ei_pmadd(ptmp0,ei_pload(&lhs0[j]),ei_pload(&res[j])));
+        else
+          for (int j = alignedStart;j<alignedSize;j+=PacketSize)
+            ei_pstore(&res[j], ei_pmadd(ptmp0,ei_ploadu(&lhs0[j]),ei_pload(&res[j])));
+      }
+
+      // process remaining scalars (or all if no explicit vectorization)
+      for (int j=alignedSize; j<size; ++j)
+        res[j] += ei_pfirst(ptmp0) * lhs0[j];
+    }
+    if (skipColumns)
+    {
+      start = 0;
+      end = skipColumns;
+      skipColumns = 0;
+    }
+    else
+      break;
+  } while(PacketSize>1);
+  #undef _EIGEN_ACCUMULATE_PACKETS
+}
+
+// TODO add peeling to mask unaligned load/stores
+template<typename Scalar, typename ResType>
+static EIGEN_DONT_INLINE void ei_cache_friendly_product_rowmajor_times_vector(
+  const Scalar* lhs, int lhsStride,
+  const Scalar* rhs, int rhsSize,
+  ResType& res)
+{
+  #ifdef _EIGEN_ACCUMULATE_PACKETS
+  #error _EIGEN_ACCUMULATE_PACKETS has already been defined
+  #endif
+
+  #define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) {\
+    Packet b = ei_pload(&rhs[j]); \
+    ptmp0 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A0) (&lhs0[j]), ptmp0); \
+    ptmp1 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs1[j]), ptmp1); \
+    ptmp2 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A2) (&lhs2[j]), ptmp2); \
+    ptmp3 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs3[j]), ptmp3); }
+
+  typedef typename ei_packet_traits<Scalar>::type Packet;
+  const int PacketSize = sizeof(Packet)/sizeof(Scalar);
+
+  enum { AllAligned=0, EvenAligned=1, FirstAligned=2, NoneAligned=3 };
+  const int rowsAtOnce = 4;
+  const int peels = 2;
+  const int PacketAlignedMask = PacketSize-1;
+  const int PeelAlignedMask = PacketSize*peels-1;
+  const int size = rhsSize;
+
+  // How many coeffs of the result do we have to skip to be aligned.
+  // Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
+  const int alignedStart = ei_alignmentOffset(rhs, size);
+  const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
+  const int peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
+
+  const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
+  int alignmentPattern = alignmentStep==0 ? AllAligned
+                       : alignmentStep==(PacketSize/2) ? EvenAligned
+                       : FirstAligned;
+
+  // we cannot assume the first element is aligned because of sub-matrices
+  const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
+
+  // find how many rows do we have to skip to be aligned with rhs (if possible)
+  int skipRows = 0;
+  if (PacketSize>1)
+  {
+    ei_internal_assert(size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0  || size<PacketSize);
+
+    while (skipRows<PacketSize &&
+           alignedStart != ((lhsAlignmentOffset + alignmentStep*skipRows)%PacketSize))
+      ++skipRows;
+    if (skipRows==PacketSize)
+    {
+      // nothing can be aligned, no need to skip any column
+      alignmentPattern = NoneAligned;
+      skipRows = 0;
+    }
+    else
+    {
+      skipRows = std::min(skipRows,res.size());
+      // note that the skiped columns are processed later.
+    }
+    ei_internal_assert((alignmentPattern==NoneAligned) || PacketSize==1
+      || (size_t(lhs+alignedStart+lhsStride*skipRows)%sizeof(Packet))==0);
+  }
+
+  int offset1 = (FirstAligned && alignmentStep==1?3:1);
+  int offset3 = (FirstAligned && alignmentStep==1?1:3);
+
+  int rowBound = ((res.size()-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
+  for (int i=skipRows; i<rowBound; i+=rowsAtOnce)
+  {
+    Scalar tmp0 = Scalar(0), tmp1 = Scalar(0), tmp2 = Scalar(0), tmp3 = Scalar(0);
+
+    // this helps the compiler generating good binary code
+    const Scalar *lhs0 = lhs + i*lhsStride,     *lhs1 = lhs + (i+offset1)*lhsStride,
+                 *lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
+
+    if (PacketSize>1)
+    {
+      /* explicit vectorization */
+      Packet ptmp0 = ei_pset1(Scalar(0)), ptmp1 = ei_pset1(Scalar(0)), ptmp2 = ei_pset1(Scalar(0)), ptmp3 = ei_pset1(Scalar(0));
+
+      // process initial unaligned coeffs
+      // FIXME this loop get vectorized by the compiler !
+      for (int j=0; j<alignedStart; ++j)
+      {
+        Scalar b = rhs[j];
+        tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
+      }
+
+      if (alignedSize>alignedStart)
+      {
+        switch(alignmentPattern)
+        {
+          case AllAligned:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,d,d);
+            break;
+          case EvenAligned:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,du,d);
+            break;
+          case FirstAligned:
+            if (peels>1)
+            {
+              /* Here we proccess 4 rows with with two peeled iterations to hide
+               * tghe overhead of unaligned loads. Moreover unaligned loads are handled
+               * using special shift/move operations between the two aligned packets
+               * overlaping the desired unaligned packet. This is *much* more efficient
+               * than basic unaligned loads.
+               */
+              Packet A01, A02, A03, b, A11, A12, A13;
+              A01 = ei_pload(&lhs1[alignedStart-1]);
+              A02 = ei_pload(&lhs2[alignedStart-2]);
+              A03 = ei_pload(&lhs3[alignedStart-3]);
+
+              for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
+              {
+                b = ei_pload(&rhs[j]);
+                A11 = ei_pload(&lhs1[j-1+PacketSize]);  ei_palign<1>(A01,A11);
+                A12 = ei_pload(&lhs2[j-2+PacketSize]);  ei_palign<2>(A02,A12);
+                A13 = ei_pload(&lhs3[j-3+PacketSize]);  ei_palign<3>(A03,A13);
+
+                ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j]), ptmp0);
+                ptmp1 = ei_pmadd(b, A01, ptmp1);
+                A01 = ei_pload(&lhs1[j-1+2*PacketSize]);  ei_palign<1>(A11,A01);
+                ptmp2 = ei_pmadd(b, A02, ptmp2);
+                A02 = ei_pload(&lhs2[j-2+2*PacketSize]);  ei_palign<2>(A12,A02);
+                ptmp3 = ei_pmadd(b, A03, ptmp3);
+                A03 = ei_pload(&lhs3[j-3+2*PacketSize]);  ei_palign<3>(A13,A03);
+
+                b = ei_pload(&rhs[j+PacketSize]);
+                ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j+PacketSize]), ptmp0);
+                ptmp1 = ei_pmadd(b, A11, ptmp1);
+                ptmp2 = ei_pmadd(b, A12, ptmp2);
+                ptmp3 = ei_pmadd(b, A13, ptmp3);
+              }
+            }
+            for (int j = peeledSize; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(d,du,du);
+            break;
+          default:
+            for (int j = alignedStart; j<alignedSize; j+=PacketSize)
+              _EIGEN_ACCUMULATE_PACKETS(du,du,du);
+            break;
+        }
+        tmp0 += ei_predux(ptmp0);
+        tmp1 += ei_predux(ptmp1);
+        tmp2 += ei_predux(ptmp2);
+        tmp3 += ei_predux(ptmp3);
+      }
+    } // end explicit vectorization
+
+    // process remaining coeffs (or all if no explicit vectorization)
+    // FIXME this loop get vectorized by the compiler !
+    for (int j=alignedSize; j<size; ++j)
+    {
+      Scalar b = rhs[j];
+      tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
+    }
+    res[i] += tmp0; res[i+offset1] += tmp1; res[i+2] += tmp2; res[i+offset3] += tmp3;
+  }
+
+  // process remaining first and last rows (at most columnsAtOnce-1)
+  int end = res.size();
+  int start = rowBound;
+  do
+  {
+    for (int i=start; i<end; ++i)
+    {
+      Scalar tmp0 = Scalar(0);
+      Packet ptmp0 = ei_pset1(tmp0);
+      const Scalar* lhs0 = lhs + i*lhsStride;
+      // process first unaligned result's coeffs
+      // FIXME this loop get vectorized by the compiler !
+      for (int j=0; j<alignedStart; ++j)
+        tmp0 += rhs[j] * lhs0[j];
+
+      if (alignedSize>alignedStart)
+      {
+        // process aligned rhs coeffs
+        if ((size_t(lhs0+alignedStart)%sizeof(Packet))==0)
+          for (int j = alignedStart;j<alignedSize;j+=PacketSize)
+            ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_pload(&lhs0[j]), ptmp0);
+        else
+          for (int j = alignedStart;j<alignedSize;j+=PacketSize)
+            ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_ploadu(&lhs0[j]), ptmp0);
+        tmp0 += ei_predux(ptmp0);
+      }
+
+      // process remaining scalars
+      // FIXME this loop get vectorized by the compiler !
+      for (int j=alignedSize; j<size; ++j)
+        tmp0 += rhs[j] * lhs0[j];
+      res[i] += tmp0;
+    }
+    if (skipRows)
+    {
+      start = 0;
+      end = skipRows;
+      skipRows = 0;
+    }
+    else
+      break;
+  } while(PacketSize>1);
+
+  #undef _EIGEN_ACCUMULATE_PACKETS
+}
+
+#endif // EIGEN_CACHE_FRIENDLY_PRODUCT_H
diff --git a/extern/Eigen2/Eigen/src/Core/Coeffs.h b/extern/Eigen2/Eigen/src/Core/Coeffs.h
new file mode 100644
index 00000000000..23a84228b24
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Coeffs.h
@@ -0,0 +1,384 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_COEFFS_H
+#define EIGEN_COEFFS_H
+
+/** Short version: don't use this function, use
+  * \link operator()(int,int) const \endlink instead.
+  *
+  * Long version: this function is similar to
+  * \link operator()(int,int) const \endlink, but without the assertion.
+  * Use this for limiting the performance cost of debugging code when doing
+  * repeated coefficient access. Only use this when it is guaranteed that the
+  * parameters \a row and \a col are in range.
+  *
+  * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+  * function equivalent to \link operator()(int,int) const \endlink.
+  *
+  * \sa operator()(int,int) const, coeffRef(int,int), coeff(int) const
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::coeff(int row, int col) const
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  return derived().coeff(row, col);
+}
+
+/** \returns the coefficient at given the given row and column.
+  *
+  * \sa operator()(int,int), operator[](int) const
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::operator()(int row, int col) const
+{
+  ei_assert(row >= 0 && row < rows()
+      && col >= 0 && col < cols());
+  return derived().coeff(row, col);
+}
+
+/** Short version: don't use this function, use
+  * \link operator()(int,int) \endlink instead.
+  *
+  * Long version: this function is similar to
+  * \link operator()(int,int) \endlink, but without the assertion.
+  * Use this for limiting the performance cost of debugging code when doing
+  * repeated coefficient access. Only use this when it is guaranteed that the
+  * parameters \a row and \a col are in range.
+  *
+  * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+  * function equivalent to \link operator()(int,int) \endlink.
+  *
+  * \sa operator()(int,int), coeff(int, int) const, coeffRef(int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::coeffRef(int row, int col)
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  return derived().coeffRef(row, col);
+}
+
+/** \returns a reference to the coefficient at given the given row and column.
+  *
+  * \sa operator()(int,int) const, operator[](int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::operator()(int row, int col)
+{
+  ei_assert(row >= 0 && row < rows()
+      && col >= 0 && col < cols());
+  return derived().coeffRef(row, col);
+}
+
+/** Short version: don't use this function, use
+  * \link operator[](int) const \endlink instead.
+  *
+  * Long version: this function is similar to
+  * \link operator[](int) const \endlink, but without the assertion.
+  * Use this for limiting the performance cost of debugging code when doing
+  * repeated coefficient access. Only use this when it is guaranteed that the
+  * parameter \a index is in range.
+  *
+  * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+  * function equivalent to \link operator[](int) const \endlink.
+  *
+  * \sa operator[](int) const, coeffRef(int), coeff(int,int) const
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::coeff(int index) const
+{
+  ei_internal_assert(index >= 0 && index < size());
+  return derived().coeff(index);
+}
+
+/** \returns the coefficient at given index.
+  *
+  * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+  *
+  * \sa operator[](int), operator()(int,int) const, x() const, y() const,
+  * z() const, w() const
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::operator[](int index) const
+{
+  ei_assert(index >= 0 && index < size());
+  return derived().coeff(index);
+}
+
+/** \returns the coefficient at given index.
+  *
+  * This is synonymous to operator[](int) const.
+  *
+  * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+  *
+  * \sa operator[](int), operator()(int,int) const, x() const, y() const,
+  * z() const, w() const
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::operator()(int index) const
+{
+  ei_assert(index >= 0 && index < size());
+  return derived().coeff(index);
+}
+
+/** Short version: don't use this function, use
+  * \link operator[](int) \endlink instead.
+  *
+  * Long version: this function is similar to
+  * \link operator[](int) \endlink, but without the assertion.
+  * Use this for limiting the performance cost of debugging code when doing
+  * repeated coefficient access. Only use this when it is guaranteed that the
+  * parameters \a row and \a col are in range.
+  *
+  * If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
+  * function equivalent to \link operator[](int) \endlink.
+  *
+  * \sa operator[](int), coeff(int) const, coeffRef(int,int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::coeffRef(int index)
+{
+  ei_internal_assert(index >= 0 && index < size());
+  return derived().coeffRef(index);
+}
+
+/** \returns a reference to the coefficient at given index.
+  *
+  * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+  *
+  * \sa operator[](int) const, operator()(int,int), x(), y(), z(), w()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::operator[](int index)
+{
+  ei_assert(index >= 0 && index < size());
+  return derived().coeffRef(index);
+}
+
+/** \returns a reference to the coefficient at given index.
+  *
+  * This is synonymous to operator[](int).
+  *
+  * This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
+  *
+  * \sa operator[](int) const, operator()(int,int), x(), y(), z(), w()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::operator()(int index)
+{
+  ei_assert(index >= 0 && index < size());
+  return derived().coeffRef(index);
+}
+
+/** equivalent to operator[](0).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::x() const { return (*this)[0]; }
+
+/** equivalent to operator[](1).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::y() const { return (*this)[1]; }
+
+/** equivalent to operator[](2).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::z() const { return (*this)[2]; }
+
+/** equivalent to operator[](3).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
+  ::w() const { return (*this)[3]; }
+
+/** equivalent to operator[](0).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::x() { return (*this)[0]; }
+
+/** equivalent to operator[](1).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::y() { return (*this)[1]; }
+
+/** equivalent to operator[](2).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::z() { return (*this)[2]; }
+
+/** equivalent to operator[](3).  */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
+  ::w() { return (*this)[3]; }
+
+/** \returns the packet of coefficients starting at the given row and column. It is your responsibility
+  * to ensure that a packet really starts there. This method is only available on expressions having the
+  * PacketAccessBit.
+  *
+  * The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
+  * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+  * starting at an address which is a multiple of the packet size.
+  */
+template<typename Derived>
+template<int LoadMode>
+EIGEN_STRONG_INLINE typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type
+MatrixBase<Derived>::packet(int row, int col) const
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  return derived().template packet<LoadMode>(row,col);
+}
+
+/** Stores the given packet of coefficients, at the given row and column of this expression. It is your responsibility
+  * to ensure that a packet really starts there. This method is only available on expressions having the
+  * PacketAccessBit.
+  *
+  * The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
+  * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+  * starting at an address which is a multiple of the packet size.
+  */
+template<typename Derived>
+template<int StoreMode>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::writePacket
+(int row, int col, const typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type& x)
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  derived().template writePacket<StoreMode>(row,col,x);
+}
+
+/** \returns the packet of coefficients starting at the given index. It is your responsibility
+  * to ensure that a packet really starts there. This method is only available on expressions having the
+  * PacketAccessBit and the LinearAccessBit.
+  *
+  * The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
+  * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+  * starting at an address which is a multiple of the packet size.
+  */
+template<typename Derived>
+template<int LoadMode>
+EIGEN_STRONG_INLINE typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type
+MatrixBase<Derived>::packet(int index) const
+{
+  ei_internal_assert(index >= 0 && index < size());
+  return derived().template packet<LoadMode>(index);
+}
+
+/** Stores the given packet of coefficients, at the given index in this expression. It is your responsibility
+  * to ensure that a packet really starts there. This method is only available on expressions having the
+  * PacketAccessBit and the LinearAccessBit.
+  *
+  * The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
+  * the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
+  * starting at an address which is a multiple of the packet size.
+  */
+template<typename Derived>
+template<int StoreMode>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::writePacket
+(int index, const typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type& x)
+{
+  ei_internal_assert(index >= 0 && index < size());
+  derived().template writePacket<StoreMode>(index,x);
+}
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+/** \internal Copies the coefficient at position (row,col) of other into *this.
+  *
+  * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
+  * with usual assignments.
+  *
+  * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other)
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  derived().coeffRef(row, col) = other.derived().coeff(row, col);
+}
+
+/** \internal Copies the coefficient at the given index of other into *this.
+  *
+  * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
+  * with usual assignments.
+  *
+  * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyCoeff(int index, const MatrixBase<OtherDerived>& other)
+{
+  ei_internal_assert(index >= 0 && index < size());
+  derived().coeffRef(index) = other.derived().coeff(index);
+}
+
+/** \internal Copies the packet at position (row,col) of other into *this.
+  *
+  * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
+  * with usual assignments.
+  *
+  * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
+  */
+template<typename Derived>
+template<typename OtherDerived, int StoreMode, int LoadMode>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyPacket(int row, int col, const MatrixBase<OtherDerived>& other)
+{
+  ei_internal_assert(row >= 0 && row < rows()
+                     && col >= 0 && col < cols());
+  derived().template writePacket<StoreMode>(row, col,
+    other.derived().template packet<LoadMode>(row, col));
+}
+
+/** \internal Copies the packet at the given index of other into *this.
+  *
+  * This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
+  * with usual assignments.
+  *
+  * Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
+  */
+template<typename Derived>
+template<typename OtherDerived, int StoreMode, int LoadMode>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyPacket(int index, const MatrixBase<OtherDerived>& other)
+{
+  ei_internal_assert(index >= 0 && index < size());
+  derived().template writePacket<StoreMode>(index,
+    other.derived().template packet<LoadMode>(index));
+}
+
+#endif
+
+#endif // EIGEN_COEFFS_H
diff --git a/extern/Eigen2/Eigen/src/Core/CommaInitializer.h b/extern/Eigen2/Eigen/src/Core/CommaInitializer.h
new file mode 100644
index 00000000000..ed28e0ca371
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CommaInitializer.h
@@ -0,0 +1,149 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_COMMAINITIALIZER_H
+#define EIGEN_COMMAINITIALIZER_H
+
+/** \class CommaInitializer
+  *
+  * \brief Helper class used by the comma initializer operator
+  *
+  * This class is internally used to implement the comma initializer feature. It is
+  * the return type of MatrixBase::operator<<, and most of the time this is the only
+  * way it is used.
+  *
+  * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
+  */
+template<typename MatrixType>
+struct CommaInitializer
+{
+  typedef typename ei_traits<MatrixType>::Scalar Scalar;
+  inline CommaInitializer(MatrixType& mat, const Scalar& s)
+    : m_matrix(mat), m_row(0), m_col(1), m_currentBlockRows(1)
+  {
+    m_matrix.coeffRef(0,0) = s;
+  }
+
+  template<typename OtherDerived>
+  inline CommaInitializer(MatrixType& mat, const MatrixBase<OtherDerived>& other)
+    : m_matrix(mat), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
+  {
+    m_matrix.block(0, 0, other.rows(), other.cols()) = other;
+  }
+
+  /* inserts a scalar value in the target matrix */
+  CommaInitializer& operator,(const Scalar& s)
+  {
+    if (m_col==m_matrix.cols())
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = 1;
+      ei_assert(m_row<m_matrix.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    ei_assert(m_col<m_matrix.cols()
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    ei_assert(m_currentBlockRows==1);
+    m_matrix.coeffRef(m_row, m_col++) = s;
+    return *this;
+  }
+
+  /* inserts a matrix expression in the target matrix */
+  template<typename OtherDerived>
+  CommaInitializer& operator,(const MatrixBase<OtherDerived>& other)
+  {
+    if (m_col==m_matrix.cols())
+    {
+      m_row+=m_currentBlockRows;
+      m_col = 0;
+      m_currentBlockRows = other.rows();
+      ei_assert(m_row+m_currentBlockRows<=m_matrix.rows()
+        && "Too many rows passed to comma initializer (operator<<)");
+    }
+    ei_assert(m_col<m_matrix.cols()
+      && "Too many coefficients passed to comma initializer (operator<<)");
+    ei_assert(m_currentBlockRows==other.rows());
+    if (OtherDerived::SizeAtCompileTime != Dynamic)
+      m_matrix.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
+                              OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
+                    (m_row, m_col) = other;
+    else
+      m_matrix.block(m_row, m_col, other.rows(), other.cols()) = other;
+    m_col += other.cols();
+    return *this;
+  }
+
+  inline ~CommaInitializer()
+  {
+    ei_assert((m_row+m_currentBlockRows) == m_matrix.rows()
+         && m_col == m_matrix.cols()
+         && "Too few coefficients passed to comma initializer (operator<<)");
+  }
+
+  /** \returns the built matrix once all its coefficients have been set.
+    * Calling finished is 100% optional. Its purpose is to write expressions
+    * like this:
+    * \code
+    * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
+    * \endcode
+    */
+  inline MatrixType& finished() { return m_matrix; }
+
+  MatrixType& m_matrix;   // target matrix
+  int m_row;              // current row id
+  int m_col;              // current col id
+  int m_currentBlockRows; // current block height
+};
+
+/** \anchor MatrixBaseCommaInitRef
+  * Convenient operator to set the coefficients of a matrix.
+  *
+  * The coefficients must be provided in a row major order and exactly match
+  * the size of the matrix. Otherwise an assertion is raised.
+  *
+  * \addexample CommaInit \label How to easily set all the coefficients of a matrix
+  *
+  * Example: \include MatrixBase_set.cpp
+  * Output: \verbinclude MatrixBase_set.out
+  *
+  * \sa CommaInitializer::finished(), class CommaInitializer
+  */
+template<typename Derived>
+inline CommaInitializer<Derived> MatrixBase<Derived>::operator<< (const Scalar& s)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
+}
+
+/** \sa operator<<(const Scalar&) */
+template<typename Derived>
+template<typename OtherDerived>
+inline CommaInitializer<Derived>
+MatrixBase<Derived>::operator<<(const MatrixBase<OtherDerived>& other)
+{
+  return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
+}
+
+#endif // EIGEN_COMMAINITIALIZER_H
diff --git a/extern/Eigen2/Eigen/src/Core/CoreInstantiations.cpp b/extern/Eigen2/Eigen/src/Core/CoreInstantiations.cpp
new file mode 100644
index 00000000000..56a9448917a
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CoreInstantiations.cpp
@@ -0,0 +1,47 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifdef EIGEN_EXTERN_INSTANTIATIONS
+#undef EIGEN_EXTERN_INSTANTIATIONS
+#endif
+
+#include "../../Core"
+
+namespace Eigen
+{
+
+#define EIGEN_INSTANTIATE_PRODUCT(TYPE) \
+template static void ei_cache_friendly_product<TYPE>( \
+  int _rows, int _cols, int depth, \
+  bool _lhsRowMajor, const TYPE* _lhs, int _lhsStride, \
+  bool _rhsRowMajor, const TYPE* _rhs, int _rhsStride, \
+  bool resRowMajor, TYPE* res, int resStride)
+
+EIGEN_INSTANTIATE_PRODUCT(float);
+EIGEN_INSTANTIATE_PRODUCT(double);
+EIGEN_INSTANTIATE_PRODUCT(int);
+EIGEN_INSTANTIATE_PRODUCT(std::complex<float>);
+EIGEN_INSTANTIATE_PRODUCT(std::complex<double>);
+
+}
diff --git a/extern/Eigen2/Eigen/src/Core/Cwise.h b/extern/Eigen2/Eigen/src/Core/Cwise.h
new file mode 100644
index 00000000000..0e92dce4e12
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Cwise.h
@@ -0,0 +1,211 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CWISE_H
+#define EIGEN_CWISE_H
+
+/** \internal
+  * convenient macro to defined the return type of a cwise binary operation */
+#define EIGEN_CWISE_BINOP_RETURN_TYPE(OP) \
+    CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, OtherDerived>
+
+#define EIGEN_CWISE_PRODUCT_RETURN_TYPE \
+    CwiseBinaryOp< \
+      ei_scalar_product_op< \
+        typename ei_scalar_product_traits< \
+          typename ei_traits<ExpressionType>::Scalar, \
+          typename ei_traits<OtherDerived>::Scalar \
+        >::ReturnType \
+      >, \
+      ExpressionType, \
+      OtherDerived \
+    >
+
+/** \internal
+  * convenient macro to defined the return type of a cwise unary operation */
+#define EIGEN_CWISE_UNOP_RETURN_TYPE(OP) \
+    CwiseUnaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType>
+
+/** \internal
+  * convenient macro to defined the return type of a cwise comparison to a scalar */
+#define EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(OP) \
+    CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, \
+        NestByValue<typename ExpressionType::ConstantReturnType> >
+
+/** \class Cwise
+  *
+  * \brief Pseudo expression providing additional coefficient-wise operations
+  *
+  * \param ExpressionType the type of the object on which to do coefficient-wise operations
+  *
+  * This class represents an expression with additional coefficient-wise features.
+  * It is the return type of MatrixBase::cwise()
+  * and most of the time this is the only way it is used.
+  *
+  * Note that some methods are defined in the \ref Array module.
+  *
+  * Example: \include MatrixBase_cwise_const.cpp
+  * Output: \verbinclude MatrixBase_cwise_const.out
+  *
+  * \sa MatrixBase::cwise() const, MatrixBase::cwise()
+  */
+template<typename ExpressionType> class Cwise
+{
+  public:
+
+    typedef typename ei_traits<ExpressionType>::Scalar Scalar;
+    typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
+        ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
+    typedef CwiseUnaryOp<ei_scalar_add_op<Scalar>, ExpressionType> ScalarAddReturnType;
+
+    inline Cwise(const ExpressionType& matrix) : m_matrix(matrix) {}
+
+    /** \internal */
+    inline const ExpressionType& _expression() const { return m_matrix; }
+
+    template<typename OtherDerived>
+    const EIGEN_CWISE_PRODUCT_RETURN_TYPE
+    operator*(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+    operator/(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
+    min(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
+    max(const MatrixBase<OtherDerived> &other) const;
+
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op)      abs() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op)     abs2() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_square_op)   square() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cube_op)     cube() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_inverse_op)  inverse() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sqrt_op)     sqrt() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_exp_op)      exp() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_log_op)      log() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cos_op)      cos() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sin_op)      sin() const;
+    const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)      pow(const Scalar& exponent) const;
+
+    const ScalarAddReturnType
+    operator+(const Scalar& scalar) const;
+
+    /** \relates Cwise */
+    friend const ScalarAddReturnType
+    operator+(const Scalar& scalar, const Cwise& mat)
+    { return mat + scalar; }
+
+    ExpressionType& operator+=(const Scalar& scalar);
+
+    const ScalarAddReturnType
+    operator-(const Scalar& scalar) const;
+
+    ExpressionType& operator-=(const Scalar& scalar);
+
+    template<typename OtherDerived>
+    inline ExpressionType& operator*=(const MatrixBase<OtherDerived> &other);
+
+    template<typename OtherDerived>
+    inline ExpressionType& operator/=(const MatrixBase<OtherDerived> &other);
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)
+    operator<(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)
+    operator<=(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)
+    operator>(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)
+    operator>=(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)
+    operator==(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)
+    operator!=(const MatrixBase<OtherDerived>& other) const;
+
+    // comparisons to a scalar value
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)
+    operator<(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)
+    operator<=(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)
+    operator>(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)
+    operator>=(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)
+    operator==(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)
+    operator!=(Scalar s) const;
+
+    // allow to extend Cwise outside Eigen
+    #ifdef EIGEN_CWISE_PLUGIN
+    #include EIGEN_CWISE_PLUGIN
+    #endif
+
+  protected:
+    ExpressionTypeNested m_matrix;
+};
+
+/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations
+  *
+  * Example: \include MatrixBase_cwise_const.cpp
+  * Output: \verbinclude MatrixBase_cwise_const.out
+  *
+  * \sa class Cwise, cwise()
+  */
+template<typename Derived>
+inline const Cwise<Derived>
+MatrixBase<Derived>::cwise() const
+{
+  return derived();
+}
+
+/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations
+  *
+  * Example: \include MatrixBase_cwise.cpp
+  * Output: \verbinclude MatrixBase_cwise.out
+  *
+  * \sa class Cwise, cwise() const
+  */
+template<typename Derived>
+inline Cwise<Derived>
+MatrixBase<Derived>::cwise()
+{
+  return derived();
+}
+
+#endif // EIGEN_CWISE_H
diff --git a/extern/Eigen2/Eigen/src/Core/CwiseBinaryOp.h b/extern/Eigen2/Eigen/src/Core/CwiseBinaryOp.h
new file mode 100644
index 00000000000..c4223e2204e
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CwiseBinaryOp.h
@@ -0,0 +1,304 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CWISE_BINARY_OP_H
+#define EIGEN_CWISE_BINARY_OP_H
+
+/** \class CwiseBinaryOp
+  *
+  * \brief Generic expression of a coefficient-wise operator between two matrices or vectors
+  *
+  * \param BinaryOp template functor implementing the operator
+  * \param Lhs the type of the left-hand side
+  * \param Rhs the type of the right-hand side
+  *
+  * This class represents an expression of a generic binary operator of two matrices or vectors.
+  * It is the return type of the operator+, operator-, and the Cwise methods, and most
+  * of the time this is the only way it is used.
+  *
+  * However, if you want to write a function returning such an expression, you
+  * will need to use this class.
+  *
+  * \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp
+  */
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct ei_traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  // even though we require Lhs and Rhs to have the same scalar type (see CwiseBinaryOp constructor),
+  // we still want to handle the case when the result type is different.
+  typedef typename ei_result_of<
+                     BinaryOp(
+                       typename Lhs::Scalar,
+                       typename Rhs::Scalar
+                     )
+                   >::type Scalar;
+  typedef typename Lhs::Nested LhsNested;
+  typedef typename Rhs::Nested RhsNested;
+  typedef typename ei_unref<LhsNested>::type _LhsNested;
+  typedef typename ei_unref<RhsNested>::type _RhsNested;
+  enum {
+    LhsCoeffReadCost = _LhsNested::CoeffReadCost,
+    RhsCoeffReadCost = _RhsNested::CoeffReadCost,
+    LhsFlags = _LhsNested::Flags,
+    RhsFlags = _RhsNested::Flags,
+    RowsAtCompileTime = Lhs::RowsAtCompileTime,
+    ColsAtCompileTime = Lhs::ColsAtCompileTime,
+    MaxRowsAtCompileTime = Lhs::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Lhs::MaxColsAtCompileTime,
+    Flags = (int(LhsFlags) | int(RhsFlags)) & (
+        HereditaryBits
+      | (int(LhsFlags) & int(RhsFlags) & (LinearAccessBit | AlignedBit))
+      | (ei_functor_traits<BinaryOp>::PacketAccess && ((int(LhsFlags) & RowMajorBit)==(int(RhsFlags) & RowMajorBit))
+        ? (int(LhsFlags) & int(RhsFlags) & PacketAccessBit) : 0)),
+    CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + ei_functor_traits<BinaryOp>::Cost
+  };
+};
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+class CwiseBinaryOp : ei_no_assignment_operator,
+  public MatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp)
+    typedef typename ei_traits<CwiseBinaryOp>::LhsNested LhsNested;
+    typedef typename ei_traits<CwiseBinaryOp>::RhsNested RhsNested;
+
+    EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& lhs, const Rhs& rhs, const BinaryOp& func = BinaryOp())
+      : m_lhs(lhs), m_rhs(rhs), m_functor(func)
+    {
+      // we require Lhs and Rhs to have the same scalar type. Currently there is no example of a binary functor
+      // that would take two operands of different types. If there were such an example, then this check should be
+      // moved to the BinaryOp functors, on a per-case basis. This would however require a change in the BinaryOp functors, as
+      // currently they take only one typename Scalar template parameter.
+      // It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths.
+      // So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to
+      // add together a float matrix and a double matrix.
+      EIGEN_STATIC_ASSERT((ei_functor_allows_mixing_real_and_complex<BinaryOp>::ret
+                           ? int(ei_is_same_type<typename Lhs::RealScalar, typename Rhs::RealScalar>::ret)
+                           : int(ei_is_same_type<typename Lhs::Scalar, typename Rhs::Scalar>::ret)),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+      // require the sizes to match
+      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
+      ei_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_lhs.cols(); }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
+    {
+      return m_functor(m_lhs.coeff(row, col), m_rhs.coeff(row, col));
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
+    {
+      return m_functor.packetOp(m_lhs.template packet<LoadMode>(row, col), m_rhs.template packet<LoadMode>(row, col));
+    }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int index) const
+    {
+      return m_functor(m_lhs.coeff(index), m_rhs.coeff(index));
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int index) const
+    {
+      return m_functor.packetOp(m_lhs.template packet<LoadMode>(index), m_rhs.template packet<LoadMode>(index));
+    }
+
+  protected:
+    const LhsNested m_lhs;
+    const RhsNested m_rhs;
+    const BinaryOp m_functor;
+};
+
+/**\returns an expression of the difference of \c *this and \a other
+  *
+  * \note If you want to substract a given scalar from all coefficients, see Cwise::operator-().
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator-=(), Cwise::operator-()
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>,
+                                 Derived, OtherDerived>
+MatrixBase<Derived>::operator-(const MatrixBase<OtherDerived> &other) const
+{
+  return CwiseBinaryOp<ei_scalar_difference_op<Scalar>,
+                       Derived, OtherDerived>(derived(), other.derived());
+}
+
+/** replaces \c *this by \c *this - \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived> &other)
+{
+  return *this = *this - other;
+}
+
+/** \relates MatrixBase
+  *
+  * \returns an expression of the sum of \c *this and \a other
+  *
+  * \note If you want to add a given scalar to all coefficients, see Cwise::operator+().
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator+=(), Cwise::operator+()
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+MatrixBase<Derived>::operator+(const MatrixBase<OtherDerived> &other) const
+{
+  return CwiseBinaryOp<ei_scalar_sum_op<Scalar>, Derived, OtherDerived>(derived(), other.derived());
+}
+
+/** replaces \c *this by \c *this + \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other)
+{
+  return *this = *this + other;
+}
+
+/** \returns an expression of the Schur product (coefficient wise product) of *this and \a other
+  *
+  * Example: \include Cwise_product.cpp
+  * Output: \verbinclude Cwise_product.out
+  *
+  * \sa class CwiseBinaryOp, operator/(), square()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_PRODUCT_RETURN_TYPE
+Cwise<ExpressionType>::operator*(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_PRODUCT_RETURN_TYPE(_expression(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise quotient of *this and \a other
+  *
+  * Example: \include Cwise_quotient.cpp
+  * Output: \verbinclude Cwise_quotient.out
+  *
+  * \sa class CwiseBinaryOp, operator*(), inverse()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+Cwise<ExpressionType>::operator/(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)(_expression(), other.derived());
+}
+
+/** Replaces this expression by its coefficient-wise product with \a other.
+  *
+  * Example: \include Cwise_times_equal.cpp
+  * Output: \verbinclude Cwise_times_equal.out
+  *
+  * \sa operator*(), operator/=()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline ExpressionType& Cwise<ExpressionType>::operator*=(const MatrixBase<OtherDerived> &other)
+{
+  return m_matrix.const_cast_derived() = *this * other;
+}
+
+/** Replaces this expression by its coefficient-wise quotient by \a other.
+  *
+  * Example: \include Cwise_slash_equal.cpp
+  * Output: \verbinclude Cwise_slash_equal.out
+  *
+  * \sa operator/(), operator*=()
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline ExpressionType& Cwise<ExpressionType>::operator/=(const MatrixBase<OtherDerived> &other)
+{
+  return m_matrix.const_cast_derived() = *this / other;
+}
+
+/** \returns an expression of the coefficient-wise min of *this and \a other
+  *
+  * Example: \include Cwise_min.cpp
+  * Output: \verbinclude Cwise_min.out
+  *
+  * \sa class CwiseBinaryOp
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
+Cwise<ExpressionType>::min(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)(_expression(), other.derived());
+}
+
+/** \returns an expression of the coefficient-wise max of *this and \a other
+  *
+  * Example: \include Cwise_max.cpp
+  * Output: \verbinclude Cwise_max.out
+  *
+  * \sa class CwiseBinaryOp
+  */
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
+Cwise<ExpressionType>::max(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)(_expression(), other.derived());
+}
+
+/** \returns an expression of a custom coefficient-wise operator \a func of *this and \a other
+  *
+  * The template parameter \a CustomBinaryOp is the type of the functor
+  * of the custom operator (see class CwiseBinaryOp for an example)
+  *
+  * \addexample CustomCwiseBinaryFunctors \label How to use custom coeff wise binary functors
+  *
+  * Here is an example illustrating the use of custom functors:
+  * \include class_CwiseBinaryOp.cpp
+  * Output: \verbinclude class_CwiseBinaryOp.out
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator+, MatrixBase::operator-, Cwise::operator*, Cwise::operator/
+  */
+template<typename Derived>
+template<typename CustomBinaryOp, typename OtherDerived>
+EIGEN_STRONG_INLINE const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
+MatrixBase<Derived>::binaryExpr(const MatrixBase<OtherDerived> &other, const CustomBinaryOp& func) const
+{
+  return CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>(derived(), other.derived(), func);
+}
+
+#endif // EIGEN_CWISE_BINARY_OP_H
diff --git a/extern/Eigen2/Eigen/src/Core/CwiseNullaryOp.h b/extern/Eigen2/Eigen/src/Core/CwiseNullaryOp.h
new file mode 100644
index 00000000000..4ee5b58afec
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CwiseNullaryOp.h
@@ -0,0 +1,763 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CWISE_NULLARY_OP_H
+#define EIGEN_CWISE_NULLARY_OP_H
+
+/** \class CwiseNullaryOp
+  *
+  * \brief Generic expression of a matrix where all coefficients are defined by a functor
+  *
+  * \param NullaryOp template functor implementing the operator
+  *
+  * This class represents an expression of a generic nullary operator.
+  * It is the return type of the Ones(), Zero(), Constant(), Identity() and Random() functions,
+  * and most of the time this is the only way it is used.
+  *
+  * However, if you want to write a function returning such an expression, you
+  * will need to use this class.
+  *
+  * \sa class CwiseUnaryOp, class CwiseBinaryOp, MatrixBase::NullaryExpr()
+  */
+template<typename NullaryOp, typename MatrixType>
+struct ei_traits<CwiseNullaryOp<NullaryOp, MatrixType> > : ei_traits<MatrixType>
+{
+  enum {
+    Flags = (ei_traits<MatrixType>::Flags
+      & (  HereditaryBits
+         | (ei_functor_has_linear_access<NullaryOp>::ret ? LinearAccessBit : 0)
+         | (ei_functor_traits<NullaryOp>::PacketAccess ? PacketAccessBit : 0)))
+      | (ei_functor_traits<NullaryOp>::IsRepeatable ? 0 : EvalBeforeNestingBit),
+    CoeffReadCost = ei_functor_traits<NullaryOp>::Cost
+  };
+};
+
+template<typename NullaryOp, typename MatrixType>
+class CwiseNullaryOp : ei_no_assignment_operator,
+  public MatrixBase<CwiseNullaryOp<NullaryOp, MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseNullaryOp)
+
+    CwiseNullaryOp(int rows, int cols, const NullaryOp& func = NullaryOp())
+      : m_rows(rows), m_cols(cols), m_functor(func)
+    {
+      ei_assert(rows > 0
+          && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
+          && cols > 0
+          && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_rows.value(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_cols.value(); }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int rows, int cols) const
+    {
+      return m_functor(rows, cols);
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int, int) const
+    {
+      return m_functor.packetOp();
+    }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int index) const
+    {
+      if(RowsAtCompileTime == 1)
+        return m_functor(0, index);
+      else
+        return m_functor(index, 0);
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int) const
+    {
+      return m_functor.packetOp();
+    }
+
+  protected:
+    const ei_int_if_dynamic<RowsAtCompileTime> m_rows;
+    const ei_int_if_dynamic<ColsAtCompileTime> m_cols;
+    const NullaryOp m_functor;
+};
+
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
+MatrixBase<Derived>::NullaryExpr(int rows, int cols, const CustomNullaryOp& func)
+{
+  return CwiseNullaryOp<CustomNullaryOp, Derived>(rows, cols, func);
+}
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
+MatrixBase<Derived>::NullaryExpr(int size, const CustomNullaryOp& func)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  ei_assert(IsVectorAtCompileTime);
+  if(RowsAtCompileTime == 1) return CwiseNullaryOp<CustomNullaryOp, Derived>(1, size, func);
+  else return CwiseNullaryOp<CustomNullaryOp, Derived>(size, 1, func);
+}
+
+/** \returns an expression of a matrix defined by a custom functor \a func
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+template<typename CustomNullaryOp>
+EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
+MatrixBase<Derived>::NullaryExpr(const CustomNullaryOp& func)
+{
+  return CwiseNullaryOp<CustomNullaryOp, Derived>(RowsAtCompileTime, ColsAtCompileTime, func);
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Constant(int rows, int cols, const Scalar& value)
+{
+  return NullaryExpr(rows, cols, ei_scalar_constant_op<Scalar>(value));
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Constant(int size, const Scalar& value)
+{
+  return NullaryExpr(size, ei_scalar_constant_op<Scalar>(value));
+}
+
+/** \returns an expression of a constant matrix of value \a value
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * The template parameter \a CustomNullaryOp is the type of the functor.
+  *
+  * \sa class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Constant(const Scalar& value)
+{
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_constant_op<Scalar>(value));
+}
+
+/** \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
+template<typename Derived>
+bool MatrixBase<Derived>::isApproxToConstant
+(const Scalar& value, RealScalar prec) const
+{
+  for(int j = 0; j < cols(); ++j)
+    for(int i = 0; i < rows(); ++i)
+      if(!ei_isApprox(coeff(i, j), value, prec))
+        return false;
+  return true;
+}
+
+/** This is just an alias for isApproxToConstant().
+  *
+  * \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
+template<typename Derived>
+bool MatrixBase<Derived>::isConstant
+(const Scalar& value, RealScalar prec) const
+{
+  return isApproxToConstant(value, prec);
+}
+
+/** Alias for setConstant(): sets all coefficients in this expression to \a value.
+  *
+  * \sa setConstant(), Constant(), class CwiseNullaryOp
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE void MatrixBase<Derived>::fill(const Scalar& value)
+{
+  setConstant(value);
+}
+
+/** Sets all coefficients in this expression to \a value.
+  *
+  * \sa fill(), setConstant(int,const Scalar&), setConstant(int,int,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setConstant(const Scalar& value)
+{
+  return derived() = Constant(rows(), cols(), value);
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to the given \a value.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_set_int.cpp
+  * Output: \verbinclude Matrix_setConstant_int.out
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(int,int,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setConstant(int size, const Scalar& value)
+{
+  resize(size);
+  return setConstant(value);
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to the given \a value.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setConstant_int_int.cpp
+  * Output: \verbinclude Matrix_setConstant_int_int.out
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(int,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setConstant(int rows, int cols, const Scalar& value)
+{
+  resize(rows, cols);
+  return setConstant(value);
+}
+
+
+// zero:
+
+/** \returns an expression of a zero matrix.
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
+  * instead.
+  *
+  * \addexample Zero \label How to take get a zero matrix
+  *
+  * Example: \include MatrixBase_zero_int_int.cpp
+  * Output: \verbinclude MatrixBase_zero_int_int.out
+  *
+  * \sa Zero(), Zero(int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Zero(int rows, int cols)
+{
+  return Constant(rows, cols, Scalar(0));
+}
+
+/** \returns an expression of a zero vector.
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Zero() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_zero_int.cpp
+  * Output: \verbinclude MatrixBase_zero_int.out
+  *
+  * \sa Zero(), Zero(int,int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Zero(int size)
+{
+  return Constant(size, Scalar(0));
+}
+
+/** \returns an expression of a fixed-size zero matrix or vector.
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * Example: \include MatrixBase_zero.cpp
+  * Output: \verbinclude MatrixBase_zero.out
+  *
+  * \sa Zero(int), Zero(int,int)
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Zero()
+{
+  return Constant(Scalar(0));
+}
+
+/** \returns true if *this is approximately equal to the zero matrix,
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isZero.cpp
+  * Output: \verbinclude MatrixBase_isZero.out
+  *
+  * \sa class CwiseNullaryOp, Zero()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isZero(RealScalar prec) const
+{
+  for(int j = 0; j < cols(); ++j)
+    for(int i = 0; i < rows(); ++i)
+      if(!ei_isMuchSmallerThan(coeff(i, j), static_cast<Scalar>(1), prec))
+        return false;
+  return true;
+}
+
+/** Sets all coefficients in this expression to zero.
+  *
+  * Example: \include MatrixBase_setZero.cpp
+  * Output: \verbinclude MatrixBase_setZero.out
+  *
+  * \sa class CwiseNullaryOp, Zero()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setZero()
+{
+  return setConstant(Scalar(0));
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to zero.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setZero_int.cpp
+  * Output: \verbinclude Matrix_setZero_int.out
+  *
+  * \sa MatrixBase::setZero(), setZero(int,int), class CwiseNullaryOp, MatrixBase::Zero()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setZero(int size)
+{
+  resize(size);
+  return setConstant(Scalar(0));
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to zero.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setZero_int_int.cpp
+  * Output: \verbinclude Matrix_setZero_int_int.out
+  *
+  * \sa MatrixBase::setZero(), setZero(int), class CwiseNullaryOp, MatrixBase::Zero()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setZero(int rows, int cols)
+{
+  resize(rows, cols);
+  return setConstant(Scalar(0));
+}
+
+// ones:
+
+/** \returns an expression of a matrix where all coefficients equal one.
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used
+  * instead.
+  *
+  * \addexample One \label How to get a matrix with all coefficients equal one
+  *
+  * Example: \include MatrixBase_ones_int_int.cpp
+  * Output: \verbinclude MatrixBase_ones_int_int.out
+  *
+  * \sa Ones(), Ones(int), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Ones(int rows, int cols)
+{
+  return Constant(rows, cols, Scalar(1));
+}
+
+/** \returns an expression of a vector where all coefficients equal one.
+  *
+  * The parameter \a size is the size of the returned vector.
+  * Must be compatible with this MatrixBase type.
+  *
+  * \only_for_vectors
+  *
+  * This variant is meant to be used for dynamic-size vector types. For fixed-size types,
+  * it is redundant to pass \a size as argument, so Ones() should be used
+  * instead.
+  *
+  * Example: \include MatrixBase_ones_int.cpp
+  * Output: \verbinclude MatrixBase_ones_int.out
+  *
+  * \sa Ones(), Ones(int,int), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Ones(int size)
+{
+  return Constant(size, Scalar(1));
+}
+
+/** \returns an expression of a fixed-size matrix or vector where all coefficients equal one.
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variants taking size arguments.
+  *
+  * Example: \include MatrixBase_ones.cpp
+  * Output: \verbinclude MatrixBase_ones.out
+  *
+  * \sa Ones(int), Ones(int,int), isOnes(), class Ones
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
+MatrixBase<Derived>::Ones()
+{
+  return Constant(Scalar(1));
+}
+
+/** \returns true if *this is approximately equal to the matrix where all coefficients
+  *          are equal to 1, within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isOnes.cpp
+  * Output: \verbinclude MatrixBase_isOnes.out
+  *
+  * \sa class CwiseNullaryOp, Ones()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isOnes
+(RealScalar prec) const
+{
+  return isApproxToConstant(Scalar(1), prec);
+}
+
+/** Sets all coefficients in this expression to one.
+  *
+  * Example: \include MatrixBase_setOnes.cpp
+  * Output: \verbinclude MatrixBase_setOnes.out
+  *
+  * \sa class CwiseNullaryOp, Ones()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setOnes()
+{
+  return setConstant(Scalar(1));
+}
+
+/** Resizes to the given \a size, and sets all coefficients in this expression to one.
+  *
+  * \only_for_vectors
+  *
+  * Example: \include Matrix_setOnes_int.cpp
+  * Output: \verbinclude Matrix_setOnes_int.out
+  *
+  * \sa MatrixBase::setOnes(), setOnes(int,int), class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setOnes(int size)
+{
+  resize(size);
+  return setConstant(Scalar(1));
+}
+
+/** Resizes to the given size, and sets all coefficients in this expression to one.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setOnes_int_int.cpp
+  * Output: \verbinclude Matrix_setOnes_int_int.out
+  *
+  * \sa MatrixBase::setOnes(), setOnes(int), class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setOnes(int rows, int cols)
+{
+  resize(rows, cols);
+  return setConstant(Scalar(1));
+}
+
+// Identity:
+
+/** \returns an expression of the identity matrix (not necessarily square).
+  *
+  * The parameters \a rows and \a cols are the number of rows and of columns of
+  * the returned matrix. Must be compatible with this MatrixBase type.
+  *
+  * This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
+  * it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used
+  * instead.
+  *
+  * \addexample Identity \label How to get an identity matrix
+  *
+  * Example: \include MatrixBase_identity_int_int.cpp
+  * Output: \verbinclude MatrixBase_identity_int_int.out
+  *
+  * \sa Identity(), setIdentity(), isIdentity()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
+MatrixBase<Derived>::Identity(int rows, int cols)
+{
+  return NullaryExpr(rows, cols, ei_scalar_identity_op<Scalar>());
+}
+
+/** \returns an expression of the identity matrix (not necessarily square).
+  *
+  * This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
+  * need to use the variant taking size arguments.
+  *
+  * Example: \include MatrixBase_identity.cpp
+  * Output: \verbinclude MatrixBase_identity.out
+  *
+  * \sa Identity(int,int), setIdentity(), isIdentity()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
+MatrixBase<Derived>::Identity()
+{
+  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+  return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_identity_op<Scalar>());
+}
+
+/** \returns true if *this is approximately equal to the identity matrix
+  *          (not necessarily square),
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isIdentity.cpp
+  * Output: \verbinclude MatrixBase_isIdentity.out
+  *
+  * \sa class CwiseNullaryOp, Identity(), Identity(int,int), setIdentity()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isIdentity
+(RealScalar prec) const
+{
+  for(int j = 0; j < cols(); ++j)
+  {
+    for(int i = 0; i < rows(); ++i)
+    {
+      if(i == j)
+      {
+        if(!ei_isApprox(coeff(i, j), static_cast<Scalar>(1), prec))
+          return false;
+      }
+      else
+      {
+        if(!ei_isMuchSmallerThan(coeff(i, j), static_cast<RealScalar>(1), prec))
+          return false;
+      }
+    }
+  }
+  return true;
+}
+
+template<typename Derived, bool Big = (Derived::SizeAtCompileTime>=16)>
+struct ei_setIdentity_impl
+{
+  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
+  {
+    return m = Derived::Identity(m.rows(), m.cols());
+  }
+};
+
+template<typename Derived>
+struct ei_setIdentity_impl<Derived, true>
+{
+  static EIGEN_STRONG_INLINE Derived& run(Derived& m)
+  {
+    m.setZero();
+    const int size = std::min(m.rows(), m.cols());
+    for(int i = 0; i < size; ++i) m.coeffRef(i,i) = typename Derived::Scalar(1);
+    return m;
+  }
+};
+
+/** Writes the identity expression (not necessarily square) into *this.
+  *
+  * Example: \include MatrixBase_setIdentity.cpp
+  * Output: \verbinclude MatrixBase_setIdentity.out
+  *
+  * \sa class CwiseNullaryOp, Identity(), Identity(int,int), isIdentity()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity()
+{
+  return ei_setIdentity_impl<Derived>::run(derived());
+}
+
+/** Resizes to the given size, and writes the identity expression (not necessarily square) into *this.
+  *
+  * \param rows the new number of rows
+  * \param cols the new number of columns
+  *
+  * Example: \include Matrix_setIdentity_int_int.cpp
+  * Output: \verbinclude Matrix_setIdentity_int_int.out
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity()
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setIdentity(int rows, int cols)
+{
+  resize(rows, cols);
+  return setIdentity();
+}
+
+/** \returns an expression of the i-th unit (basis) vector.
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int size, int i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return BasisReturnType(SquareMatrixType::Identity(size,size), i);
+}
+
+/** \returns an expression of the i-th unit (basis) vector.
+  *
+  * \only_for_vectors
+  *
+  * This variant is for fixed-size vector only.
+  *
+  * \sa MatrixBase::Unit(int,int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return BasisReturnType(SquareMatrixType::Identity(),i);
+}
+
+/** \returns an expression of the X axis unit vector (1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX()
+{ return Derived::Unit(0); }
+
+/** \returns an expression of the Y axis unit vector (0,1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY()
+{ return Derived::Unit(1); }
+
+/** \returns an expression of the Z axis unit vector (0,0,1{,0}^*)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ()
+{ return Derived::Unit(2); }
+
+/** \returns an expression of the W axis unit vector (0,0,0,1)
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW()
+{ return Derived::Unit(3); }
+
+#endif // EIGEN_CWISE_NULLARY_OP_H
diff --git a/extern/Eigen2/Eigen/src/Core/CwiseUnaryOp.h b/extern/Eigen2/Eigen/src/Core/CwiseUnaryOp.h
new file mode 100644
index 00000000000..076d568e023
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/CwiseUnaryOp.h
@@ -0,0 +1,229 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CWISE_UNARY_OP_H
+#define EIGEN_CWISE_UNARY_OP_H
+
+/** \class CwiseUnaryOp
+  *
+  * \brief Generic expression of a coefficient-wise unary operator of a matrix or a vector
+  *
+  * \param UnaryOp template functor implementing the operator
+  * \param MatrixType the type of the matrix we are applying the unary operator
+  *
+  * This class represents an expression of a generic unary operator of a matrix or a vector.
+  * It is the return type of the unary operator-, of a matrix or a vector, and most
+  * of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp
+  */
+template<typename UnaryOp, typename MatrixType>
+struct ei_traits<CwiseUnaryOp<UnaryOp, MatrixType> >
+ : ei_traits<MatrixType>
+{
+  typedef typename ei_result_of<
+                     UnaryOp(typename MatrixType::Scalar)
+                   >::type Scalar;
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    Flags = (_MatrixTypeNested::Flags & (
+      HereditaryBits | LinearAccessBit | AlignedBit
+      | (ei_functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0))),
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost + ei_functor_traits<UnaryOp>::Cost
+  };
+};
+
+template<typename UnaryOp, typename MatrixType>
+class CwiseUnaryOp : ei_no_assignment_operator,
+  public MatrixBase<CwiseUnaryOp<UnaryOp, MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp)
+
+    inline CwiseUnaryOp(const MatrixType& mat, const UnaryOp& func = UnaryOp())
+      : m_matrix(mat), m_functor(func) {}
+
+    EIGEN_STRONG_INLINE int rows() const { return m_matrix.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_matrix.cols(); }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
+    {
+      return m_functor(m_matrix.coeff(row, col));
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
+    {
+      return m_functor.packetOp(m_matrix.template packet<LoadMode>(row, col));
+    }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int index) const
+    {
+      return m_functor(m_matrix.coeff(index));
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int index) const
+    {
+      return m_functor.packetOp(m_matrix.template packet<LoadMode>(index));
+    }
+
+  protected:
+    const typename MatrixType::Nested m_matrix;
+    const UnaryOp m_functor;
+};
+
+/** \returns an expression of a custom coefficient-wise unary operator \a func of *this
+  *
+  * The template parameter \a CustomUnaryOp is the type of the functor
+  * of the custom unary operator.
+  *
+  * \addexample CustomCwiseUnaryFunctors \label How to use custom coeff wise unary functors
+  *
+  * Example:
+  * \include class_CwiseUnaryOp.cpp
+  * Output: \verbinclude class_CwiseUnaryOp.out
+  *
+  * \sa class CwiseUnaryOp, class CwiseBinarOp, MatrixBase::operator-, Cwise::abs
+  */
+template<typename Derived>
+template<typename CustomUnaryOp>
+EIGEN_STRONG_INLINE const CwiseUnaryOp<CustomUnaryOp, Derived>
+MatrixBase<Derived>::unaryExpr(const CustomUnaryOp& func) const
+{
+  return CwiseUnaryOp<CustomUnaryOp, Derived>(derived(), func);
+}
+
+/** \returns an expression of the opposite of \c *this
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived>
+MatrixBase<Derived>::operator-() const
+{
+  return derived();
+}
+
+/** \returns an expression of the coefficient-wise absolute value of \c *this
+  *
+  * Example: \include Cwise_abs.cpp
+  * Output: \verbinclude Cwise_abs.out
+  *
+  * \sa abs2()
+  */
+template<typename ExpressionType>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op)
+Cwise<ExpressionType>::abs() const
+{
+  return _expression();
+}
+
+/** \returns an expression of the coefficient-wise squared absolute value of \c *this
+  *
+  * Example: \include Cwise_abs2.cpp
+  * Output: \verbinclude Cwise_abs2.out
+  *
+  * \sa abs(), square()
+  */
+template<typename ExpressionType>
+EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op)
+Cwise<ExpressionType>::abs2() const
+{
+  return _expression();
+}
+
+/** \returns an expression of the complex conjugate of \c *this.
+  *
+  * \sa adjoint() */
+template<typename Derived>
+EIGEN_STRONG_INLINE typename MatrixBase<Derived>::ConjugateReturnType
+MatrixBase<Derived>::conjugate() const
+{
+  return ConjugateReturnType(derived());
+}
+
+/** \returns an expression of the real part of \c *this.
+  *
+  * \sa imag() */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::RealReturnType
+MatrixBase<Derived>::real() const { return derived(); }
+
+/** \returns an expression of the imaginary part of \c *this.
+  *
+  * \sa real() */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ImagReturnType
+MatrixBase<Derived>::imag() const { return derived(); }
+
+/** \returns an expression of *this with the \a Scalar type casted to
+  * \a NewScalar.
+  *
+  * The template parameter \a NewScalar is the type we are casting the scalars to.
+  *
+  * \sa class CwiseUnaryOp
+  */
+template<typename Derived>
+template<typename NewType>
+EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived>
+MatrixBase<Derived>::cast() const
+{
+  return derived();
+}
+
+/** \relates MatrixBase */
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ScalarMultipleReturnType
+MatrixBase<Derived>::operator*(const Scalar& scalar) const
+{
+  return CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, Derived>
+    (derived(), ei_scalar_multiple_op<Scalar>(scalar));
+}
+
+/** \relates MatrixBase */
+template<typename Derived>
+EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
+MatrixBase<Derived>::operator/(const Scalar& scalar) const
+{
+  return CwiseUnaryOp<ei_scalar_quotient1_op<Scalar>, Derived>
+    (derived(), ei_scalar_quotient1_op<Scalar>(scalar));
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+MatrixBase<Derived>::operator*=(const Scalar& other)
+{
+  return *this = *this * other;
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+MatrixBase<Derived>::operator/=(const Scalar& other)
+{
+  return *this = *this / other;
+}
+
+#endif // EIGEN_CWISE_UNARY_OP_H
diff --git a/extern/Eigen2/Eigen/src/Core/DiagonalCoeffs.h b/extern/Eigen2/Eigen/src/Core/DiagonalCoeffs.h
new file mode 100644
index 00000000000..767fe5fb7c0
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/DiagonalCoeffs.h
@@ -0,0 +1,124 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DIAGONALCOEFFS_H
+#define EIGEN_DIAGONALCOEFFS_H
+
+/** \class DiagonalCoeffs
+  *
+  * \brief Expression of the main diagonal of a matrix
+  *
+  * \param MatrixType the type of the object in which we are taking the main diagonal
+  *
+  * The matrix is not required to be square.
+  *
+  * This class represents an expression of the main diagonal of a square matrix.
+  * It is the return type of MatrixBase::diagonal() and most of the time this is
+  * the only way it is used.
+  *
+  * \sa MatrixBase::diagonal()
+  */
+template<typename MatrixType>
+struct ei_traits<DiagonalCoeffs<MatrixType> >
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    RowsAtCompileTime = int(MatrixType::SizeAtCompileTime) == Dynamic ? Dynamic
+                      : EIGEN_ENUM_MIN(MatrixType::RowsAtCompileTime,
+                                       MatrixType::ColsAtCompileTime),
+    ColsAtCompileTime = 1,
+    MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic
+                            : EIGEN_ENUM_MIN(MatrixType::MaxRowsAtCompileTime,
+                                             MatrixType::MaxColsAtCompileTime),
+    MaxColsAtCompileTime = 1,
+    Flags = (unsigned int)_MatrixTypeNested::Flags & (HereditaryBits | LinearAccessBit),
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost
+  };
+};
+
+template<typename MatrixType> class DiagonalCoeffs
+   : public MatrixBase<DiagonalCoeffs<MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(DiagonalCoeffs)
+
+    inline DiagonalCoeffs(const MatrixType& matrix) : m_matrix(matrix) {}
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(DiagonalCoeffs)
+
+    inline int rows() const { return std::min(m_matrix.rows(), m_matrix.cols()); }
+    inline int cols() const { return 1; }
+
+    inline Scalar& coeffRef(int row, int)
+    {
+      return m_matrix.const_cast_derived().coeffRef(row, row);
+    }
+
+    inline const Scalar coeff(int row, int) const
+    {
+      return m_matrix.coeff(row, row);
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_matrix.const_cast_derived().coeffRef(index, index);
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(index, index);
+    }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+};
+
+/** \returns an expression of the main diagonal of the matrix \c *this
+  *
+  * \c *this is not required to be square.
+  *
+  * Example: \include MatrixBase_diagonal.cpp
+  * Output: \verbinclude MatrixBase_diagonal.out
+  *
+  * \sa class DiagonalCoeffs */
+template<typename Derived>
+inline DiagonalCoeffs<Derived>
+MatrixBase<Derived>::diagonal()
+{
+  return DiagonalCoeffs<Derived>(derived());
+}
+
+/** This is the const version of diagonal(). */
+template<typename Derived>
+inline const DiagonalCoeffs<Derived>
+MatrixBase<Derived>::diagonal() const
+{
+  return DiagonalCoeffs<Derived>(derived());
+}
+
+#endif // EIGEN_DIAGONALCOEFFS_H
diff --git a/extern/Eigen2/Eigen/src/Core/DiagonalMatrix.h b/extern/Eigen2/Eigen/src/Core/DiagonalMatrix.h
new file mode 100644
index 00000000000..01f01fdf259
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/DiagonalMatrix.h
@@ -0,0 +1,144 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DIAGONALMATRIX_H
+#define EIGEN_DIAGONALMATRIX_H
+
+/** \class DiagonalMatrix
+  * \nonstableyet 
+  *
+  * \brief Expression of a diagonal matrix
+  *
+  * \param CoeffsVectorType the type of the vector of diagonal coefficients
+  *
+  * This class is an expression of a diagonal matrix with given vector of diagonal
+  * coefficients. It is the return
+  * type of MatrixBase::diagonal(const OtherDerived&) and most of the time this is
+  * the only way it is used.
+  *
+  * \sa MatrixBase::diagonal(const OtherDerived&)
+  */
+template<typename CoeffsVectorType>
+struct ei_traits<DiagonalMatrix<CoeffsVectorType> >
+{
+  typedef typename CoeffsVectorType::Scalar Scalar;
+  typedef typename ei_nested<CoeffsVectorType>::type CoeffsVectorTypeNested;
+  typedef typename ei_unref<CoeffsVectorTypeNested>::type _CoeffsVectorTypeNested;
+  enum {
+    RowsAtCompileTime = CoeffsVectorType::SizeAtCompileTime,
+    ColsAtCompileTime = CoeffsVectorType::SizeAtCompileTime,
+    MaxRowsAtCompileTime = CoeffsVectorType::MaxSizeAtCompileTime,
+    MaxColsAtCompileTime = CoeffsVectorType::MaxSizeAtCompileTime,
+    Flags = (_CoeffsVectorTypeNested::Flags & HereditaryBits) | Diagonal,
+    CoeffReadCost = _CoeffsVectorTypeNested::CoeffReadCost
+  };
+};
+
+template<typename CoeffsVectorType>
+class DiagonalMatrix : ei_no_assignment_operator,
+   public MatrixBase<DiagonalMatrix<CoeffsVectorType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(DiagonalMatrix)
+    typedef CoeffsVectorType _CoeffsVectorType;
+
+    // needed to evaluate a DiagonalMatrix<Xpr> to a DiagonalMatrix<NestByValue<Vector> >
+    template<typename OtherCoeffsVectorType>
+    inline DiagonalMatrix(const DiagonalMatrix<OtherCoeffsVectorType>& other) : m_coeffs(other.diagonal())
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(CoeffsVectorType);
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherCoeffsVectorType);
+      ei_assert(m_coeffs.size() > 0);
+    }
+
+    inline DiagonalMatrix(const CoeffsVectorType& coeffs) : m_coeffs(coeffs)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(CoeffsVectorType);
+      ei_assert(coeffs.size() > 0);
+    }
+
+    inline int rows() const { return m_coeffs.size(); }
+    inline int cols() const { return m_coeffs.size(); }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return row == col ? m_coeffs.coeff(row) : static_cast<Scalar>(0);
+    }
+
+    inline const CoeffsVectorType& diagonal() const { return m_coeffs; }
+
+  protected:
+    const typename CoeffsVectorType::Nested m_coeffs;
+};
+
+/** \nonstableyet 
+  * \returns an expression of a diagonal matrix with *this as vector of diagonal coefficients
+  *
+  * \only_for_vectors
+  *
+  * \addexample AsDiagonalExample \label How to build a diagonal matrix from a vector
+  *
+  * Example: \include MatrixBase_asDiagonal.cpp
+  * Output: \verbinclude MatrixBase_asDiagonal.out
+  *
+  * \sa class DiagonalMatrix, isDiagonal()
+  **/
+template<typename Derived>
+inline const DiagonalMatrix<Derived>
+MatrixBase<Derived>::asDiagonal() const
+{
+  return derived();
+}
+
+/** \nonstableyet 
+  * \returns true if *this is approximately equal to a diagonal matrix,
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isDiagonal.cpp
+  * Output: \verbinclude MatrixBase_isDiagonal.out
+  *
+  * \sa asDiagonal()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isDiagonal
+(RealScalar prec) const
+{
+  if(cols() != rows()) return false;
+  RealScalar maxAbsOnDiagonal = static_cast<RealScalar>(-1);
+  for(int j = 0; j < cols(); ++j)
+  {
+    RealScalar absOnDiagonal = ei_abs(coeff(j,j));
+    if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal;
+  }
+  for(int j = 0; j < cols(); ++j)
+    for(int i = 0; i < j; ++i)
+    {
+      if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false;
+      if(!ei_isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false;
+    }
+  return true;
+}
+
+#endif // EIGEN_DIAGONALMATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Core/DiagonalProduct.h b/extern/Eigen2/Eigen/src/Core/DiagonalProduct.h
new file mode 100644
index 00000000000..f33a26f98b0
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/DiagonalProduct.h
@@ -0,0 +1,130 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DIAGONALPRODUCT_H
+#define EIGEN_DIAGONALPRODUCT_H
+
+/** \internal Specialization of ei_nested for DiagonalMatrix.
+ *  Unlike ei_nested, if the argument is a DiagonalMatrix and if it must be evaluated,
+ *  then it evaluated to a DiagonalMatrix having its own argument evaluated.
+ */
+template<typename T, int N> struct ei_nested_diagonal : ei_nested<T,N> {};
+template<typename T, int N> struct ei_nested_diagonal<DiagonalMatrix<T>,N >
+ : ei_nested<DiagonalMatrix<T>, N, DiagonalMatrix<NestByValue<typename ei_plain_matrix_type<T>::type> > >
+{};
+
+// specialization of ProductReturnType
+template<typename Lhs, typename Rhs>
+struct ProductReturnType<Lhs,Rhs,DiagonalProduct>
+{
+  typedef typename ei_nested_diagonal<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
+  typedef typename ei_nested_diagonal<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+
+  typedef Product<LhsNested, RhsNested, DiagonalProduct> Type;
+};
+
+template<typename LhsNested, typename RhsNested>
+struct ei_traits<Product<LhsNested, RhsNested, DiagonalProduct> >
+{
+  // clean the nested types:
+  typedef typename ei_cleantype<LhsNested>::type _LhsNested;
+  typedef typename ei_cleantype<RhsNested>::type _RhsNested;
+  typedef typename _LhsNested::Scalar Scalar;
+
+  enum {
+    LhsFlags = _LhsNested::Flags,
+    RhsFlags = _RhsNested::Flags,
+    RowsAtCompileTime = _LhsNested::RowsAtCompileTime,
+    ColsAtCompileTime = _RhsNested::ColsAtCompileTime,
+    MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime,
+
+    LhsIsDiagonal = (_LhsNested::Flags&Diagonal)==Diagonal,
+    RhsIsDiagonal = (_RhsNested::Flags&Diagonal)==Diagonal,
+
+    CanVectorizeRhs =  (!RhsIsDiagonal) && (RhsFlags & RowMajorBit) && (RhsFlags & PacketAccessBit)
+                     && (ColsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
+
+    CanVectorizeLhs =  (!LhsIsDiagonal) && (!(LhsFlags & RowMajorBit)) && (LhsFlags & PacketAccessBit)
+                     && (RowsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
+
+    RemovedBits = ~((RhsFlags & RowMajorBit) && (!CanVectorizeLhs) ? 0 : RowMajorBit),
+
+    Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits)
+          | (((CanVectorizeLhs&&RhsIsDiagonal) || (CanVectorizeRhs&&LhsIsDiagonal)) ? PacketAccessBit : 0),
+
+    CoeffReadCost = NumTraits<Scalar>::MulCost + _LhsNested::CoeffReadCost + _RhsNested::CoeffReadCost
+  };
+};
+
+template<typename LhsNested, typename RhsNested> class Product<LhsNested, RhsNested, DiagonalProduct> : ei_no_assignment_operator,
+  public MatrixBase<Product<LhsNested, RhsNested, DiagonalProduct> >
+{
+    typedef typename ei_traits<Product>::_LhsNested _LhsNested;
+    typedef typename ei_traits<Product>::_RhsNested _RhsNested;
+
+    enum {
+      RhsIsDiagonal = (_RhsNested::Flags&Diagonal)==Diagonal
+    };
+
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
+
+    template<typename Lhs, typename Rhs>
+    inline Product(const Lhs& lhs, const Rhs& rhs)
+      : m_lhs(lhs), m_rhs(rhs)
+    {
+      ei_assert(lhs.cols() == rhs.rows());
+    }
+
+    inline int rows() const { return m_lhs.rows(); }
+    inline int cols() const { return m_rhs.cols(); }
+
+    const Scalar coeff(int row, int col) const
+    {
+      const int unique = RhsIsDiagonal ? col : row;
+      return m_lhs.coeff(row, unique) * m_rhs.coeff(unique, col);
+    }
+
+    template<int LoadMode>
+    const PacketScalar packet(int row, int col) const
+    {
+      if (RhsIsDiagonal)
+      {
+        return ei_pmul(m_lhs.template packet<LoadMode>(row, col), ei_pset1(m_rhs.coeff(col, col)));
+      }
+      else
+      {
+        return ei_pmul(ei_pset1(m_lhs.coeff(row, row)), m_rhs.template packet<LoadMode>(row, col));
+      }
+    }
+
+  protected:
+    const LhsNested m_lhs;
+    const RhsNested m_rhs;
+};
+
+#endif // EIGEN_DIAGONALPRODUCT_H
diff --git a/extern/Eigen2/Eigen/src/Core/Dot.h b/extern/Eigen2/Eigen/src/Core/Dot.h
new file mode 100644
index 00000000000..5838af70d4a
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Dot.h
@@ -0,0 +1,361 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DOT_H
+#define EIGEN_DOT_H
+
+/***************************************************************************
+* Part 1 : the logic deciding a strategy for vectorization and unrolling
+***************************************************************************/
+
+template<typename Derived1, typename Derived2>
+struct ei_dot_traits
+{
+public:
+  enum {
+    Vectorization = (int(Derived1::Flags)&int(Derived2::Flags)&ActualPacketAccessBit)
+                 && (int(Derived1::Flags)&int(Derived2::Flags)&LinearAccessBit)
+                  ? LinearVectorization
+                  : NoVectorization
+  };
+
+private:
+  typedef typename Derived1::Scalar Scalar;
+  enum {
+    PacketSize = ei_packet_traits<Scalar>::size,
+    Cost = Derived1::SizeAtCompileTime * (Derived1::CoeffReadCost + Derived2::CoeffReadCost + NumTraits<Scalar>::MulCost)
+           + (Derived1::SizeAtCompileTime-1) * NumTraits<Scalar>::AddCost,
+    UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize))
+  };
+
+public:
+  enum {
+    Unrolling = Cost <= UnrollingLimit
+              ? CompleteUnrolling
+              : NoUnrolling
+  };
+};
+
+/***************************************************************************
+* Part 2 : unrollers
+***************************************************************************/
+
+/*** no vectorization ***/
+
+template<typename Derived1, typename Derived2, int Start, int Length>
+struct ei_dot_novec_unroller
+{
+  enum {
+    HalfLength = Length/2
+  };
+
+  typedef typename Derived1::Scalar Scalar;
+
+  inline static Scalar run(const Derived1& v1, const Derived2& v2)
+  {
+    return ei_dot_novec_unroller<Derived1, Derived2, Start, HalfLength>::run(v1, v2)
+         + ei_dot_novec_unroller<Derived1, Derived2, Start+HalfLength, Length-HalfLength>::run(v1, v2);
+  }
+};
+
+template<typename Derived1, typename Derived2, int Start>
+struct ei_dot_novec_unroller<Derived1, Derived2, Start, 1>
+{
+  typedef typename Derived1::Scalar Scalar;
+
+  inline static Scalar run(const Derived1& v1, const Derived2& v2)
+  {
+    return v1.coeff(Start) * ei_conj(v2.coeff(Start));
+  }
+};
+
+/*** vectorization ***/
+
+template<typename Derived1, typename Derived2, int Index, int Stop,
+         bool LastPacket = (Stop-Index == ei_packet_traits<typename Derived1::Scalar>::size)>
+struct ei_dot_vec_unroller
+{
+  typedef typename Derived1::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  enum {
+    row1 = Derived1::RowsAtCompileTime == 1 ? 0 : Index,
+    col1 = Derived1::RowsAtCompileTime == 1 ? Index : 0,
+    row2 = Derived2::RowsAtCompileTime == 1 ? 0 : Index,
+    col2 = Derived2::RowsAtCompileTime == 1 ? Index : 0
+  };
+
+  inline static PacketScalar run(const Derived1& v1, const Derived2& v2)
+  {
+    return ei_pmadd(
+      v1.template packet<Aligned>(row1, col1),
+      v2.template packet<Aligned>(row2, col2),
+      ei_dot_vec_unroller<Derived1, Derived2, Index+ei_packet_traits<Scalar>::size, Stop>::run(v1, v2)
+    );
+  }
+};
+
+template<typename Derived1, typename Derived2, int Index, int Stop>
+struct ei_dot_vec_unroller<Derived1, Derived2, Index, Stop, true>
+{
+  enum {
+    row1 = Derived1::RowsAtCompileTime == 1 ? 0 : Index,
+    col1 = Derived1::RowsAtCompileTime == 1 ? Index : 0,
+    row2 = Derived2::RowsAtCompileTime == 1 ? 0 : Index,
+    col2 = Derived2::RowsAtCompileTime == 1 ? Index : 0,
+    alignment1 = (Derived1::Flags & AlignedBit) ? Aligned : Unaligned,
+    alignment2 = (Derived2::Flags & AlignedBit) ? Aligned : Unaligned
+  };
+
+  typedef typename Derived1::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  inline static PacketScalar run(const Derived1& v1, const Derived2& v2)
+  {
+    return ei_pmul(v1.template packet<alignment1>(row1, col1), v2.template packet<alignment2>(row2, col2));
+  }
+};
+
+/***************************************************************************
+* Part 3 : implementation of all cases
+***************************************************************************/
+
+template<typename Derived1, typename Derived2,
+         int Vectorization = ei_dot_traits<Derived1, Derived2>::Vectorization,
+         int Unrolling = ei_dot_traits<Derived1, Derived2>::Unrolling
+>
+struct ei_dot_impl;
+
+template<typename Derived1, typename Derived2>
+struct ei_dot_impl<Derived1, Derived2, NoVectorization, NoUnrolling>
+{
+  typedef typename Derived1::Scalar Scalar;
+  static Scalar run(const Derived1& v1, const Derived2& v2)
+  {
+    ei_assert(v1.size()>0 && "you are using a non initialized vector");
+    Scalar res;
+    res = v1.coeff(0) * ei_conj(v2.coeff(0));
+    for(int i = 1; i < v1.size(); ++i)
+      res += v1.coeff(i) * ei_conj(v2.coeff(i));
+    return res;
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_dot_impl<Derived1, Derived2, NoVectorization, CompleteUnrolling>
+  : public ei_dot_novec_unroller<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
+{};
+
+template<typename Derived1, typename Derived2>
+struct ei_dot_impl<Derived1, Derived2, LinearVectorization, NoUnrolling>
+{
+  typedef typename Derived1::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  static Scalar run(const Derived1& v1, const Derived2& v2)
+  {
+    const int size = v1.size();
+    const int packetSize = ei_packet_traits<Scalar>::size;
+    const int alignedSize = (size/packetSize)*packetSize;
+    enum {
+      alignment1 = (Derived1::Flags & AlignedBit) ? Aligned : Unaligned,
+      alignment2 = (Derived2::Flags & AlignedBit) ? Aligned : Unaligned
+    };
+    Scalar res;
+
+    // do the vectorizable part of the sum
+    if(size >= packetSize)
+    {
+      PacketScalar packet_res = ei_pmul(
+                                  v1.template packet<alignment1>(0),
+                                  v2.template packet<alignment2>(0)
+                                );
+      for(int index = packetSize; index<alignedSize; index += packetSize)
+      {
+        packet_res = ei_pmadd(
+                       v1.template packet<alignment1>(index),
+                       v2.template packet<alignment2>(index),
+                       packet_res
+                     );
+      }
+      res = ei_predux(packet_res);
+
+      // now we must do the rest without vectorization.
+      if(alignedSize == size) return res;
+    }
+    else // too small to vectorize anything.
+         // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
+    {
+      res = Scalar(0);
+    }
+
+    // do the remainder of the vector
+    for(int index = alignedSize; index < size; ++index)
+    {
+      res += v1.coeff(index) * v2.coeff(index);
+    }
+
+    return res;
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_dot_impl<Derived1, Derived2, LinearVectorization, CompleteUnrolling>
+{
+  typedef typename Derived1::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  enum {
+    PacketSize = ei_packet_traits<Scalar>::size,
+    Size = Derived1::SizeAtCompileTime,
+    VectorizationSize = (Size / PacketSize) * PacketSize
+  };
+  static Scalar run(const Derived1& v1, const Derived2& v2)
+  {
+    Scalar res = ei_predux(ei_dot_vec_unroller<Derived1, Derived2, 0, VectorizationSize>::run(v1, v2));
+    if (VectorizationSize != Size)
+      res += ei_dot_novec_unroller<Derived1, Derived2, VectorizationSize, Size-VectorizationSize>::run(v1, v2);
+    return res;
+  }
+};
+
+/***************************************************************************
+* Part 4 : implementation of MatrixBase methods
+***************************************************************************/
+
+/** \returns the dot product of *this with other.
+  *
+  * \only_for_vectors
+  *
+  * \note If the scalar type is complex numbers, then this function returns the hermitian
+  * (sesquilinear) dot product, linear in the first variable and conjugate-linear in the
+  * second variable.
+  *
+  * \sa squaredNorm(), norm()
+  */
+template<typename Derived>
+template<typename OtherDerived>
+typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  ei_assert(size() == other.size());
+
+  return ei_dot_impl<Derived, OtherDerived>::run(derived(), other.derived());
+}
+
+/** \returns the squared \em l2 norm of *this, i.e., for vectors, the dot product of *this with itself.
+  *
+  * \sa dot(), norm()
+  */
+template<typename Derived>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::squaredNorm() const
+{
+  return ei_real((*this).cwise().abs2().sum());
+}
+
+/** \returns the \em l2 norm of *this, i.e., for vectors, the square root of the dot product of *this with itself.
+  *
+  * \sa dot(), squaredNorm()
+  */
+template<typename Derived>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
+{
+  return ei_sqrt(squaredNorm());
+}
+
+/** \returns an expression of the quotient of *this by its own norm.
+  *
+  * \only_for_vectors
+  *
+  * \sa norm(), normalize()
+  */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::PlainMatrixType
+MatrixBase<Derived>::normalized() const
+{
+  typedef typename ei_nested<Derived>::type Nested;
+  typedef typename ei_unref<Nested>::type _Nested;
+  _Nested n(derived());
+  return n / n.norm();
+}
+
+/** Normalizes the vector, i.e. divides it by its own norm.
+  *
+  * \only_for_vectors
+  *
+  * \sa norm(), normalized()
+  */
+template<typename Derived>
+inline void MatrixBase<Derived>::normalize()
+{
+  *this /= norm();
+}
+
+/** \returns true if *this is approximately orthogonal to \a other,
+  *          within the precision given by \a prec.
+  *
+  * Example: \include MatrixBase_isOrthogonal.cpp
+  * Output: \verbinclude MatrixBase_isOrthogonal.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+bool MatrixBase<Derived>::isOrthogonal
+(const MatrixBase<OtherDerived>& other, RealScalar prec) const
+{
+  typename ei_nested<Derived,2>::type nested(derived());
+  typename ei_nested<OtherDerived,2>::type otherNested(other.derived());
+  return ei_abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm();
+}
+
+/** \returns true if *this is approximately an unitary matrix,
+  *          within the precision given by \a prec. In the case where the \a Scalar
+  *          type is real numbers, a unitary matrix is an orthogonal matrix, whence the name.
+  *
+  * \note This can be used to check whether a family of vectors forms an orthonormal basis.
+  *       Indeed, \c m.isUnitary() returns true if and only if the columns (equivalently, the rows) of m form an
+  *       orthonormal basis.
+  *
+  * Example: \include MatrixBase_isUnitary.cpp
+  * Output: \verbinclude MatrixBase_isUnitary.out
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isUnitary(RealScalar prec) const
+{
+  typename Derived::Nested nested(derived());
+  for(int i = 0; i < cols(); ++i)
+  {
+    if(!ei_isApprox(nested.col(i).squaredNorm(), static_cast<Scalar>(1), prec))
+      return false;
+    for(int j = 0; j < i; ++j)
+      if(!ei_isMuchSmallerThan(nested.col(i).dot(nested.col(j)), static_cast<Scalar>(1), prec))
+        return false;
+  }
+  return true;
+}
+#endif // EIGEN_DOT_H
diff --git a/extern/Eigen2/Eigen/src/Core/Flagged.h b/extern/Eigen2/Eigen/src/Core/Flagged.h
new file mode 100644
index 00000000000..ce50246cb67
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Flagged.h
@@ -0,0 +1,146 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_FLAGGED_H
+#define EIGEN_FLAGGED_H
+
+/** \class Flagged
+  *
+  * \brief Expression with modified flags
+  *
+  * \param ExpressionType the type of the object of which we are modifying the flags
+  * \param Added the flags added to the expression
+  * \param Removed the flags removed from the expression (has priority over Added).
+  *
+  * This class represents an expression whose flags have been modified.
+  * It is the return type of MatrixBase::flagged()
+  * and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::flagged()
+  */
+template<typename ExpressionType, unsigned int Added, unsigned int Removed>
+struct ei_traits<Flagged<ExpressionType, Added, Removed> > : ei_traits<ExpressionType>
+{
+  enum { Flags = (ExpressionType::Flags | Added) & ~Removed };
+};
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed> class Flagged
+  : public MatrixBase<Flagged<ExpressionType, Added, Removed> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Flagged)
+    typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
+        ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
+    typedef typename ExpressionType::InnerIterator InnerIterator;
+
+    inline Flagged(const ExpressionType& matrix) : m_matrix(matrix) {}
+
+    inline int rows() const { return m_matrix.rows(); }
+    inline int cols() const { return m_matrix.cols(); }
+    inline int stride() const { return m_matrix.stride(); }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row, col);
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived().coeffRef(row, col);
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(index);
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_matrix.const_cast_derived().coeffRef(index);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int row, int col) const
+    {
+      return m_matrix.template packet<LoadMode>(row, col);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int row, int col, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<LoadMode>(row, col, x);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int index) const
+    {
+      return m_matrix.template packet<LoadMode>(index);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int index, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<LoadMode>(index, x);
+    }
+
+    const ExpressionType& _expression() const { return m_matrix; }
+
+  protected:
+    ExpressionTypeNested m_matrix;
+};
+
+/** \returns an expression of *this with added flags
+  *
+  * \addexample MarkExample \label How to mark a triangular matrix as triangular
+  *
+  * Example: \include MatrixBase_marked.cpp
+  * Output: \verbinclude MatrixBase_marked.out
+  *
+  * \sa class Flagged, extract(), part()
+  */
+template<typename Derived>
+template<unsigned int Added>
+inline const Flagged<Derived, Added, 0>
+MatrixBase<Derived>::marked() const
+{
+  return derived();
+}
+
+/** \returns an expression of *this with the following flags removed:
+  * EvalBeforeNestingBit and EvalBeforeAssigningBit.
+  *
+  * Example: \include MatrixBase_lazy.cpp
+  * Output: \verbinclude MatrixBase_lazy.out
+  *
+  * \sa class Flagged, marked()
+  */
+template<typename Derived>
+inline const Flagged<Derived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>
+MatrixBase<Derived>::lazy() const
+{
+  return derived();
+}
+
+#endif // EIGEN_FLAGGED_H
diff --git a/extern/Eigen2/Eigen/src/Core/Functors.h b/extern/Eigen2/Eigen/src/Core/Functors.h
new file mode 100644
index 00000000000..c8ca3dac1cf
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Functors.h
@@ -0,0 +1,368 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_FUNCTORS_H
+#define EIGEN_FUNCTORS_H
+
+// associative functors:
+
+/** \internal
+  * \brief Template functor to compute the sum of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator+, class PartialRedux, MatrixBase::sum()
+  */
+template<typename Scalar> struct ei_scalar_sum_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a + b; }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_padd(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_sum_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the product of two scalars
+  *
+  * \sa class CwiseBinaryOp, Cwise::operator*(), class PartialRedux, MatrixBase::redux()
+  */
+template<typename Scalar> struct ei_scalar_product_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a * b; }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_pmul(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_product_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::MulCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the min of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class PartialRedux, MatrixBase::minCoeff()
+  */
+template<typename Scalar> struct ei_scalar_min_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return std::min(a, b); }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_pmin(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_min_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the max of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class PartialRedux, MatrixBase::maxCoeff()
+  */
+template<typename Scalar> struct ei_scalar_max_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return std::max(a, b); }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_pmax(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_max_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+  };
+};
+
+
+// other binary functors:
+
+/** \internal
+  * \brief Template functor to compute the difference of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::operator-
+  */
+template<typename Scalar> struct ei_scalar_difference_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a - b; }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_psub(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_difference_op<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the quotient of two scalars
+  *
+  * \sa class CwiseBinaryOp, Cwise::operator/()
+  */
+template<typename Scalar> struct ei_scalar_quotient_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a / b; }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
+  { return ei_pdiv(a,b); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_quotient_op<Scalar> > {
+  enum {
+    Cost = 2 * NumTraits<Scalar>::MulCost,
+    PacketAccess = ei_packet_traits<Scalar>::size>1
+                  #if (defined EIGEN_VECTORIZE_SSE)
+                  && NumTraits<Scalar>::HasFloatingPoint
+                  #endif
+  };
+};
+
+// unary functors:
+
+/** \internal
+  * \brief Template functor to compute the opposite of a scalar
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator-
+  */
+template<typename Scalar> struct ei_scalar_opposite_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return -a; }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_opposite_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to compute the absolute value of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::abs
+  */
+template<typename Scalar> struct ei_scalar_abs_op EIGEN_EMPTY_STRUCT {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return ei_abs(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_abs_op<Scalar> >
+{
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = false // this could actually be vectorized with SSSE3.
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the squared absolute value of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::abs2
+  */
+template<typename Scalar> struct ei_scalar_abs2_op EIGEN_EMPTY_STRUCT {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return ei_abs2(a); }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pmul(a,a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_abs2_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
+
+/** \internal
+  * \brief Template functor to compute the conjugate of a complex value
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::conjugate()
+  */
+template<typename Scalar> struct ei_scalar_conjugate_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return ei_conj(a); }
+  template<typename PacketScalar>
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const { return a; }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_conjugate_op<Scalar> >
+{
+  enum {
+    Cost = NumTraits<Scalar>::IsComplex ? NumTraits<Scalar>::AddCost : 0,
+    PacketAccess = int(ei_packet_traits<Scalar>::size)>1
+  };
+};
+
+/** \internal
+  * \brief Template functor to cast a scalar to another type
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::cast()
+  */
+template<typename Scalar, typename NewType>
+struct ei_scalar_cast_op EIGEN_EMPTY_STRUCT {
+  typedef NewType result_type;
+  EIGEN_STRONG_INLINE const NewType operator() (const Scalar& a) const { return static_cast<NewType>(a); }
+};
+template<typename Scalar, typename NewType>
+struct ei_functor_traits<ei_scalar_cast_op<Scalar,NewType> >
+{ enum { Cost = ei_is_same_type<Scalar, NewType>::ret ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the real part of a complex
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::real()
+  */
+template<typename Scalar>
+struct ei_scalar_real_op EIGEN_EMPTY_STRUCT {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return ei_real(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_real_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to extract the imaginary part of a complex
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::imag()
+  */
+template<typename Scalar>
+struct ei_scalar_imag_op EIGEN_EMPTY_STRUCT {
+  typedef typename NumTraits<Scalar>::Real result_type;
+  EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return ei_imag(a); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_imag_op<Scalar> >
+{ enum { Cost = 0, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to multiply a scalar by a fixed other one
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator*, MatrixBase::operator/
+  */
+/* NOTE why doing the ei_pset1() in packetOp *is* an optimization ?
+ * indeed it seems better to declare m_other as a PacketScalar and do the ei_pset1() once
+ * in the constructor. However, in practice:
+ *  - GCC does not like m_other as a PacketScalar and generate a load every time it needs it
+ *  - one the other hand GCC is able to moves the ei_pset1() away the loop :)
+ *  - simpler code ;)
+ * (ICC and gcc 4.4 seems to perform well in both cases, the issue is visible with y = a*x + b*y)
+ */
+template<typename Scalar>
+struct ei_scalar_multiple_op {
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE ei_scalar_multiple_op(const ei_scalar_multiple_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE ei_scalar_multiple_op(const Scalar& other) : m_other(other) { }
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pmul(a, ei_pset1(m_other)); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_multiple_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
+
+template<typename Scalar, bool HasFloatingPoint>
+struct ei_scalar_quotient1_impl {
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const ei_scalar_quotient1_impl& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const Scalar& other) : m_other(static_cast<Scalar>(1) / other) {}
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
+  EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
+  { return ei_pmul(a, ei_pset1(m_other)); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_quotient1_impl<Scalar,true> >
+{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
+
+template<typename Scalar>
+struct ei_scalar_quotient1_impl<Scalar,false> {
+  // FIXME default copy constructors seems bugged with std::complex<>
+  EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const ei_scalar_quotient1_impl& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const Scalar& other) : m_other(other) {}
+  EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_quotient1_impl<Scalar,false> >
+{ enum { Cost = 2 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
+
+/** \internal
+  * \brief Template functor to divide a scalar by a fixed other one
+  *
+  * This functor is used to implement the quotient of a matrix by
+  * a scalar where the scalar type is not necessarily a floating point type.
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::operator/
+  */
+template<typename Scalar>
+struct ei_scalar_quotient1_op : ei_scalar_quotient1_impl<Scalar, NumTraits<Scalar>::HasFloatingPoint > {
+  EIGEN_STRONG_INLINE ei_scalar_quotient1_op(const Scalar& other)
+    : ei_scalar_quotient1_impl<Scalar, NumTraits<Scalar>::HasFloatingPoint >(other) {}
+};
+
+// nullary functors
+
+template<typename Scalar>
+struct ei_scalar_constant_op {
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  EIGEN_STRONG_INLINE ei_scalar_constant_op(const ei_scalar_constant_op& other) : m_other(other.m_other) { }
+  EIGEN_STRONG_INLINE ei_scalar_constant_op(const Scalar& other) : m_other(other) { }
+  EIGEN_STRONG_INLINE const Scalar operator() (int, int = 0) const { return m_other; }
+  EIGEN_STRONG_INLINE const PacketScalar packetOp() const { return ei_pset1(m_other); }
+  const Scalar m_other;
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_constant_op<Scalar> >
+{ enum { Cost = 1, PacketAccess = ei_packet_traits<Scalar>::size>1, IsRepeatable = true }; };
+
+template<typename Scalar> struct ei_scalar_identity_op EIGEN_EMPTY_STRUCT {
+  EIGEN_STRONG_INLINE ei_scalar_identity_op(void) {}
+  EIGEN_STRONG_INLINE const Scalar operator() (int row, int col) const { return row==col ? Scalar(1) : Scalar(0); }
+};
+template<typename Scalar>
+struct ei_functor_traits<ei_scalar_identity_op<Scalar> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true }; };
+
+// allow to add new functors and specializations of ei_functor_traits from outside Eigen.
+// this macro is really needed because ei_functor_traits must be specialized after it is declared but before it is used...
+#ifdef EIGEN_FUNCTORS_PLUGIN
+#include EIGEN_FUNCTORS_PLUGIN
+#endif
+
+// all functors allow linear access, except ei_scalar_identity_op. So we fix here a quick meta
+// to indicate whether a functor allows linear access, just always answering 'yes' except for
+// ei_scalar_identity_op.
+template<typename Functor> struct ei_functor_has_linear_access { enum { ret = 1 }; };
+template<typename Scalar> struct ei_functor_has_linear_access<ei_scalar_identity_op<Scalar> > { enum { ret = 0 }; };
+
+// in CwiseBinaryOp, we require the Lhs and Rhs to have the same scalar type, except for multiplication
+// where we only require them to have the same _real_ scalar type so one may multiply, say, float by complex<float>.
+template<typename Functor> struct ei_functor_allows_mixing_real_and_complex { enum { ret = 0 }; };
+template<typename Scalar> struct ei_functor_allows_mixing_real_and_complex<ei_scalar_product_op<Scalar> > { enum { ret = 1 }; };
+
+#endif // EIGEN_FUNCTORS_H
diff --git a/extern/Eigen2/Eigen/src/Core/Fuzzy.h b/extern/Eigen2/Eigen/src/Core/Fuzzy.h
new file mode 100644
index 00000000000..1285542966c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Fuzzy.h
@@ -0,0 +1,234 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_FUZZY_H
+#define EIGEN_FUZZY_H
+
+#ifndef EIGEN_LEGACY_COMPARES
+
+/** \returns \c true if \c *this is approximately equal to \a other, within the precision
+  * determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
+  * are considered to be approximately equal within precision \f$ p \f$ if
+  * \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
+  * For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm
+  * L2 norm).
+  *
+  * \note Because of the multiplicativeness of this comparison, one can't use this function
+  * to check whether \c *this is approximately equal to the zero matrix or vector.
+  * Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
+  * or vector. If you want to test whether \c *this is zero, use ei_isMuchSmallerThan(const
+  * RealScalar&, RealScalar) instead.
+  *
+  * \sa ei_isMuchSmallerThan(const RealScalar&, RealScalar) const
+  */
+template<typename Derived>
+template<typename OtherDerived>
+bool MatrixBase<Derived>::isApprox(
+  const MatrixBase<OtherDerived>& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  const typename ei_nested<Derived,2>::type nested(derived());
+  const typename ei_nested<OtherDerived,2>::type otherNested(other.derived());
+  return (nested - otherNested).cwise().abs2().sum() <= prec * prec * std::min(nested.cwise().abs2().sum(), otherNested.cwise().abs2().sum());
+}
+
+/** \returns \c true if the norm of \c *this is much smaller than \a other,
+  * within the precision determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+  * considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
+  * \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
+  *
+  * For matrices, the comparison is done using the Hilbert-Schmidt norm. For this reason,
+  * the value of the reference scalar \a other should come from the Hilbert-Schmidt norm
+  * of a reference matrix of same dimensions.
+  *
+  * \sa isApprox(), isMuchSmallerThan(const MatrixBase<OtherDerived>&, RealScalar) const
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isMuchSmallerThan(
+  const typename NumTraits<Scalar>::Real& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  return cwise().abs2().sum() <= prec * prec * other * other;
+}
+
+/** \returns \c true if the norm of \c *this is much smaller than the norm of \a other,
+  * within the precision determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+  * considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
+  * \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
+  * For matrices, the comparison is done using the Hilbert-Schmidt norm.
+  *
+  * \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
+  */
+template<typename Derived>
+template<typename OtherDerived>
+bool MatrixBase<Derived>::isMuchSmallerThan(
+  const MatrixBase<OtherDerived>& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  return this->cwise().abs2().sum() <= prec * prec * other.cwise().abs2().sum();
+}
+
+#else
+
+template<typename Derived, typename OtherDerived=Derived, bool IsVector=Derived::IsVectorAtCompileTime>
+struct ei_fuzzy_selector;
+
+/** \returns \c true if \c *this is approximately equal to \a other, within the precision
+  * determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
+  * are considered to be approximately equal within precision \f$ p \f$ if
+  * \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
+  * For matrices, the comparison is done on all columns.
+  *
+  * \note Because of the multiplicativeness of this comparison, one can't use this function
+  * to check whether \c *this is approximately equal to the zero matrix or vector.
+  * Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
+  * or vector. If you want to test whether \c *this is zero, use ei_isMuchSmallerThan(const
+  * RealScalar&, RealScalar) instead.
+  *
+  * \sa ei_isMuchSmallerThan(const RealScalar&, RealScalar) const
+  */
+template<typename Derived>
+template<typename OtherDerived>
+bool MatrixBase<Derived>::isApprox(
+  const MatrixBase<OtherDerived>& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  return ei_fuzzy_selector<Derived,OtherDerived>::isApprox(derived(), other.derived(), prec);
+}
+
+/** \returns \c true if the norm of \c *this is much smaller than \a other,
+  * within the precision determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+  * considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
+  * \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
+  * For matrices, the comparison is done on all columns.
+  *
+  * \sa isApprox(), isMuchSmallerThan(const MatrixBase<OtherDerived>&, RealScalar) const
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isMuchSmallerThan(
+  const typename NumTraits<Scalar>::Real& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  return ei_fuzzy_selector<Derived>::isMuchSmallerThan(derived(), other, prec);
+}
+
+/** \returns \c true if the norm of \c *this is much smaller than the norm of \a other,
+  * within the precision determined by \a prec.
+  *
+  * \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
+  * considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
+  * \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
+  * For matrices, the comparison is done on all columns.
+  *
+  * \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
+  */
+template<typename Derived>
+template<typename OtherDerived>
+bool MatrixBase<Derived>::isMuchSmallerThan(
+  const MatrixBase<OtherDerived>& other,
+  typename NumTraits<Scalar>::Real prec
+) const
+{
+  return ei_fuzzy_selector<Derived,OtherDerived>::isMuchSmallerThan(derived(), other.derived(), prec);
+}
+
+
+template<typename Derived, typename OtherDerived>
+struct ei_fuzzy_selector<Derived,OtherDerived,true>
+{
+  typedef typename Derived::RealScalar RealScalar;
+  static bool isApprox(const Derived& self, const OtherDerived& other, RealScalar prec)
+  {
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+    ei_assert(self.size() == other.size());
+    return((self - other).squaredNorm() <= std::min(self.squaredNorm(), other.squaredNorm()) * prec * prec);
+  }
+  static bool isMuchSmallerThan(const Derived& self, const RealScalar& other, RealScalar prec)
+  {
+    return(self.squaredNorm() <= ei_abs2(other * prec));
+  }
+  static bool isMuchSmallerThan(const Derived& self, const OtherDerived& other, RealScalar prec)
+  {
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+    ei_assert(self.size() == other.size());
+    return(self.squaredNorm() <= other.squaredNorm() * prec * prec);
+  }
+};
+
+template<typename Derived, typename OtherDerived>
+struct ei_fuzzy_selector<Derived,OtherDerived,false>
+{
+  typedef typename Derived::RealScalar RealScalar;
+  static bool isApprox(const Derived& self, const OtherDerived& other, RealScalar prec)
+  {
+    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
+    ei_assert(self.rows() == other.rows() && self.cols() == other.cols());
+    typename Derived::Nested nested(self);
+    typename OtherDerived::Nested otherNested(other);
+    for(int i = 0; i < self.cols(); ++i)
+      if((nested.col(i) - otherNested.col(i)).squaredNorm()
+          > std::min(nested.col(i).squaredNorm(), otherNested.col(i).squaredNorm()) * prec * prec)
+        return false;
+    return true;
+  }
+  static bool isMuchSmallerThan(const Derived& self, const RealScalar& other, RealScalar prec)
+  {
+    typename Derived::Nested nested(self);
+    for(int i = 0; i < self.cols(); ++i)
+      if(nested.col(i).squaredNorm() > ei_abs2(other * prec))
+        return false;
+    return true;
+  }
+  static bool isMuchSmallerThan(const Derived& self, const OtherDerived& other, RealScalar prec)
+  {
+    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
+    ei_assert(self.rows() == other.rows() && self.cols() == other.cols());
+    typename Derived::Nested nested(self);
+    typename OtherDerived::Nested otherNested(other);
+    for(int i = 0; i < self.cols(); ++i)
+      if(nested.col(i).squaredNorm() > otherNested.col(i).squaredNorm() * prec * prec)
+        return false;
+    return true;
+  }
+};
+
+#endif
+
+#endif // EIGEN_FUZZY_H
diff --git a/extern/Eigen2/Eigen/src/Core/GenericPacketMath.h b/extern/Eigen2/Eigen/src/Core/GenericPacketMath.h
new file mode 100644
index 00000000000..b0eee29f70f
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/GenericPacketMath.h
@@ -0,0 +1,150 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_GENERIC_PACKET_MATH_H
+#define EIGEN_GENERIC_PACKET_MATH_H
+
+/** \internal
+  * \file GenericPacketMath.h
+  *
+  * Default implementation for types not supported by the vectorization.
+  * In practice these functions are provided to make easier the writing
+  * of generic vectorized code.
+  */
+
+/** \internal \returns a + b (coeff-wise) */
+template<typename Packet> inline Packet
+ei_padd(const Packet& a,
+        const Packet& b) { return a+b; }
+
+/** \internal \returns a - b (coeff-wise) */
+template<typename Packet> inline Packet
+ei_psub(const Packet& a,
+        const Packet& b) { return a-b; }
+
+/** \internal \returns a * b (coeff-wise) */
+template<typename Packet> inline Packet
+ei_pmul(const Packet& a,
+        const Packet& b) { return a*b; }
+
+/** \internal \returns a / b (coeff-wise) */
+template<typename Packet> inline Packet
+ei_pdiv(const Packet& a,
+        const Packet& b) { return a/b; }
+
+/** \internal \returns the min of \a a and \a b  (coeff-wise) */
+template<typename Packet> inline Packet
+ei_pmin(const Packet& a,
+        const Packet& b) { return std::min(a, b); }
+
+/** \internal \returns the max of \a a and \a b  (coeff-wise) */
+template<typename Packet> inline Packet
+ei_pmax(const Packet& a,
+        const Packet& b) { return std::max(a, b); }
+
+/** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */
+template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
+ei_pload(const Scalar* from) { return *from; }
+
+/** \internal \returns a packet version of \a *from, (un-aligned load) */
+template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
+ei_ploadu(const Scalar* from) { return *from; }
+
+/** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */
+template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
+ei_pset1(const Scalar& a) { return a; }
+
+/** \internal copy the packet \a from to \a *to, \a to must be 16 bytes aligned */
+template<typename Scalar, typename Packet> inline void ei_pstore(Scalar* to, const Packet& from)
+{ (*to) = from; }
+
+/** \internal copy the packet \a from to \a *to, (un-aligned store) */
+template<typename Scalar, typename Packet> inline void ei_pstoreu(Scalar* to, const Packet& from)
+{ (*to) = from; }
+
+/** \internal \returns the first element of a packet */
+template<typename Packet> inline typename ei_unpacket_traits<Packet>::type ei_pfirst(const Packet& a)
+{ return a; }
+
+/** \internal \returns a packet where the element i contains the sum of the packet of \a vec[i] */
+template<typename Packet> inline Packet
+ei_preduxp(const Packet* vecs) { return vecs[0]; }
+
+/** \internal \returns the sum of the elements of \a a*/
+template<typename Packet> inline typename ei_unpacket_traits<Packet>::type ei_predux(const Packet& a)
+{ return a; }
+
+
+/***************************************************************************
+* The following functions might not have to be overwritten for vectorized types
+***************************************************************************/
+
+/** \internal \returns a * b + c (coeff-wise) */
+template<typename Packet> inline Packet
+ei_pmadd(const Packet&  a,
+         const Packet&  b,
+         const Packet&  c)
+{ return ei_padd(ei_pmul(a, b),c); }
+
+/** \internal \returns a packet version of \a *from.
+  * \If LoadMode equals Aligned, \a from must be 16 bytes aligned */
+template<typename Scalar, int LoadMode>
+inline typename ei_packet_traits<Scalar>::type ei_ploadt(const Scalar* from)
+{
+  if(LoadMode == Aligned)
+    return ei_pload(from);
+  else
+    return ei_ploadu(from);
+}
+
+/** \internal copy the packet \a from to \a *to.
+  * If StoreMode equals Aligned, \a to must be 16 bytes aligned */
+template<typename Scalar, typename Packet, int LoadMode>
+inline void ei_pstoret(Scalar* to, const Packet& from)
+{
+  if(LoadMode == Aligned)
+    ei_pstore(to, from);
+  else
+    ei_pstoreu(to, from);
+}
+
+/** \internal default implementation of ei_palign() allowing partial specialization */
+template<int Offset,typename PacketType>
+struct ei_palign_impl
+{
+  // by default data are aligned, so there is nothing to be done :)
+  inline static void run(PacketType&, const PacketType&) {}
+};
+
+/** \internal update \a first using the concatenation of the \a Offset last elements
+  * of \a first and packet_size minus \a Offset first elements of \a second */
+template<int Offset,typename PacketType>
+inline void ei_palign(PacketType& first, const PacketType& second)
+{
+  ei_palign_impl<Offset,PacketType>::run(first,second);
+}
+
+#endif // EIGEN_GENERIC_PACKET_MATH_H
+
diff --git a/extern/Eigen2/Eigen/src/Core/IO.h b/extern/Eigen2/Eigen/src/Core/IO.h
new file mode 100644
index 00000000000..2b00d5bc509
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/IO.h
@@ -0,0 +1,184 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_IO_H
+#define EIGEN_IO_H
+
+enum { Raw, AlignCols };
+
+/** \class IOFormat
+  *
+  * \brief Stores a set of parameters controlling the way matrices are printed
+  *
+  * List of available parameters:
+  *  - \b precision number of digits for floating point values
+  *  - \b flags can be either Raw (default) or AlignCols which aligns all the columns
+  *  - \b coeffSeparator string printed between two coefficients of the same row
+  *  - \b rowSeparator string printed between two rows
+  *  - \b rowPrefix string printed at the beginning of each row
+  *  - \b rowSuffix string printed at the end of each row
+  *  - \b matPrefix string printed at the beginning of the matrix
+  *  - \b matSuffix string printed at the end of the matrix
+  *
+  * Example: \include IOFormat.cpp
+  * Output: \verbinclude IOFormat.out
+  *
+  * \sa MatrixBase::format(), class WithFormat
+  */
+struct IOFormat
+{
+  /** Default contructor, see class IOFormat for the meaning of the parameters */
+  IOFormat(int _precision=4, int _flags=Raw,
+    const std::string& _coeffSeparator = " ",
+    const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="",
+    const std::string& _matPrefix="", const std::string& _matSuffix="")
+  : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator),
+    coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags)
+  {
+    rowSpacer = "";
+    int i = int(matSuffix.length())-1;
+    while (i>=0 && matSuffix[i]!='\n')
+    {
+      rowSpacer += ' ';
+      i--;
+    }
+  }
+  std::string matPrefix, matSuffix;
+  std::string rowPrefix, rowSuffix, rowSeparator, rowSpacer;
+  std::string coeffSeparator;
+  int precision;
+  int flags;
+};
+
+/** \class WithFormat
+  *
+  * \brief Pseudo expression providing matrix output with given format
+  *
+  * \param ExpressionType the type of the object on which IO stream operations are performed
+  *
+  * This class represents an expression with stream operators controlled by a given IOFormat.
+  * It is the return type of MatrixBase::format()
+  * and most of the time this is the only way it is used.
+  *
+  * See class IOFormat for some examples.
+  *
+  * \sa MatrixBase::format(), class IOFormat
+  */
+template<typename ExpressionType>
+class WithFormat
+{
+  public:
+
+    WithFormat(const ExpressionType& matrix, const IOFormat& format)
+      : m_matrix(matrix), m_format(format)
+    {}
+
+    friend std::ostream & operator << (std::ostream & s, const WithFormat& wf)
+    {
+      return ei_print_matrix(s, wf.m_matrix.eval(), wf.m_format);
+    }
+
+  protected:
+    const typename ExpressionType::Nested m_matrix;
+    IOFormat m_format;
+};
+
+/** \returns a WithFormat proxy object allowing to print a matrix the with given
+  * format \a fmt.
+  *
+  * See class IOFormat for some examples.
+  *
+  * \sa class IOFormat, class WithFormat
+  */
+template<typename Derived>
+inline const WithFormat<Derived>
+MatrixBase<Derived>::format(const IOFormat& fmt) const
+{
+  return WithFormat<Derived>(derived(), fmt);
+}
+
+/** \internal
+  * print the matrix \a _m to the output stream \a s using the output format \a fmt */
+template<typename Derived>
+std::ostream & ei_print_matrix(std::ostream & s, const Derived& _m, const IOFormat& fmt)
+{
+  const typename Derived::Nested m = _m;
+
+  int width = 0;
+  if (fmt.flags & AlignCols)
+  {
+    // compute the largest width
+    for(int j = 1; j < m.cols(); ++j)
+      for(int i = 0; i < m.rows(); ++i)
+      {
+        std::stringstream sstr;
+        sstr.precision(fmt.precision);
+        sstr << m.coeff(i,j);
+        width = std::max<int>(width, int(sstr.str().length()));
+      }
+  }
+  s.precision(fmt.precision);
+  s << fmt.matPrefix;
+  for(int i = 0; i < m.rows(); ++i)
+  {
+    if (i)
+      s << fmt.rowSpacer;
+    s << fmt.rowPrefix;
+    if(width) s.width(width);
+    s << m.coeff(i, 0);
+    for(int j = 1; j < m.cols(); ++j)
+    {
+      s << fmt.coeffSeparator;
+      if (width) s.width(width);
+      s << m.coeff(i, j);
+    }
+    s << fmt.rowSuffix;
+    if( i < m.rows() - 1)
+      s << fmt.rowSeparator;
+  }
+  s << fmt.matSuffix;
+  return s;
+}
+
+/** \relates MatrixBase
+  *
+  * Outputs the matrix, to the given stream.
+  *
+  * If you wish to print the matrix with a format different than the default, use MatrixBase::format().
+  *
+  * It is also possible to change the default format by defining EIGEN_DEFAULT_IO_FORMAT before including Eigen headers.
+  * If not defined, this will automatically be defined to Eigen::IOFormat(), that is the Eigen::IOFormat with default parameters.
+  *
+  * \sa MatrixBase::format()
+  */
+template<typename Derived>
+std::ostream & operator <<
+(std::ostream & s,
+ const MatrixBase<Derived> & m)
+{
+  return ei_print_matrix(s, m.eval(), EIGEN_DEFAULT_IO_FORMAT);
+}
+
+#endif // EIGEN_IO_H
diff --git a/extern/Eigen2/Eigen/src/Core/Map.h b/extern/Eigen2/Eigen/src/Core/Map.h
new file mode 100644
index 00000000000..5f44a87e685
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Map.h
@@ -0,0 +1,111 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MAP_H
+#define EIGEN_MAP_H
+
+/** \class Map
+  *
+  * \brief A matrix or vector expression mapping an existing array of data.
+  *
+  * \param MatrixType the equivalent matrix type of the mapped data
+  * \param _PacketAccess allows to enforce aligned loads and stores if set to ForceAligned.
+  *                      The default is AsRequested. This parameter is internaly used by Eigen
+  *                      in expressions such as \code Map<...>(...) += other; \endcode and most
+  *                      of the time this is the only way it is used.
+  *
+  * This class represents a matrix or vector expression mapping an existing array of data.
+  * It can be used to let Eigen interface without any overhead with non-Eigen data structures,
+  * such as plain C arrays or structures from other libraries.
+  *
+  * This class is the return type of Matrix::Map() but can also be used directly.
+  *
+  * \sa Matrix::Map()
+  */
+template<typename MatrixType, int _PacketAccess>
+struct ei_traits<Map<MatrixType, _PacketAccess> > : public ei_traits<MatrixType>
+{
+  enum {
+    PacketAccess = _PacketAccess,
+    Flags = ei_traits<MatrixType>::Flags & ~AlignedBit
+  };
+  typedef typename ei_meta_if<int(PacketAccess)==ForceAligned,
+                              Map<MatrixType, _PacketAccess>&,
+                              Map<MatrixType, ForceAligned> >::ret AlignedDerivedType;
+};
+
+template<typename MatrixType, int PacketAccess> class Map
+  : public MapBase<Map<MatrixType, PacketAccess> >
+{
+  public:
+
+    _EIGEN_GENERIC_PUBLIC_INTERFACE(Map, MapBase<Map>)
+    typedef typename ei_traits<Map>::AlignedDerivedType AlignedDerivedType;
+
+    inline int stride() const { return this->innerSize(); }
+
+    AlignedDerivedType _convertToForceAligned()
+    {
+      return Map<MatrixType,ForceAligned>(Base::m_data, Base::m_rows.value(), Base::m_cols.value());
+    }
+
+    inline Map(const Scalar* data) : Base(data) {}
+
+    inline Map(const Scalar* data, int size) : Base(data, size) {}
+
+    inline Map(const Scalar* data, int rows, int cols) : Base(data, rows, cols) {}
+
+    inline void resize(int rows, int cols)
+    {
+      EIGEN_ONLY_USED_FOR_DEBUG(rows);
+      EIGEN_ONLY_USED_FOR_DEBUG(cols);
+      ei_assert(rows == this->rows());
+      ei_assert(cols == this->cols());
+    }
+
+    inline void resize(int size)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType)
+      EIGEN_ONLY_USED_FOR_DEBUG(size);
+      ei_assert(size == this->size());
+    }
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
+};
+
+/** Constructor copying an existing array of data.
+  * Only for fixed-size matrices and vectors.
+  * \param data The array of data to copy
+  *
+  * \sa Matrix::Map(const Scalar *)
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _StorageOrder, int _MaxRows, int _MaxCols>
+inline Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols>
+  ::Matrix(const Scalar *data)
+{
+  _set_noalias(Eigen::Map<Matrix>(data));
+}
+
+#endif // EIGEN_MAP_H
diff --git a/extern/Eigen2/Eigen/src/Core/MapBase.h b/extern/Eigen2/Eigen/src/Core/MapBase.h
new file mode 100644
index 00000000000..c923bc34034
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/MapBase.h
@@ -0,0 +1,202 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MAPBASE_H
+#define EIGEN_MAPBASE_H
+
+/** \class MapBase
+  *
+  * \brief Base class for Map and Block expression with direct access
+  *
+  * Expression classes inheriting MapBase must define the constant \c PacketAccess,
+  * and type \c AlignedDerivedType in their respective ei_traits<> specialization structure.
+  * The value of \c PacketAccess can be either:
+  *  - \b ForceAligned which enforces both aligned loads and stores
+  *  - \b AsRequested which is the default behavior
+  * The type \c AlignedDerivedType should correspond to the equivalent expression type
+  * with \c PacketAccess being \c ForceAligned.
+  *
+  * \sa class Map, class Block
+  */
+template<typename Derived> class MapBase
+  : public MatrixBase<Derived>
+{
+  public:
+
+    typedef MatrixBase<Derived> Base;
+    enum {
+      IsRowMajor = (int(ei_traits<Derived>::Flags) & RowMajorBit) ? 1 : 0,
+      PacketAccess = ei_traits<Derived>::PacketAccess,
+      RowsAtCompileTime = ei_traits<Derived>::RowsAtCompileTime,
+      ColsAtCompileTime = ei_traits<Derived>::ColsAtCompileTime,
+      SizeAtCompileTime = Base::SizeAtCompileTime
+    };
+
+    typedef typename ei_traits<Derived>::AlignedDerivedType AlignedDerivedType;
+    typedef typename ei_traits<Derived>::Scalar Scalar;
+    typedef typename Base::PacketScalar PacketScalar;
+    using Base::derived;
+
+    inline int rows() const { return m_rows.value(); }
+    inline int cols() const { return m_cols.value(); }
+
+    inline int stride() const { return derived().stride(); }
+    inline const Scalar* data() const { return m_data; }
+
+    template<bool IsForceAligned,typename Dummy> struct force_aligned_impl {
+      AlignedDerivedType static run(MapBase& a) { return a.derived(); }
+    };
+
+    template<typename Dummy> struct force_aligned_impl<false,Dummy> {
+      AlignedDerivedType static run(MapBase& a) { return a.derived()._convertToForceAligned(); }
+    };
+
+    /** \returns an expression equivalent to \c *this but having the \c PacketAccess constant
+      * set to \c ForceAligned. Must be reimplemented by the derived class. */
+    AlignedDerivedType forceAligned()
+    {
+      return force_aligned_impl<int(PacketAccess)==int(ForceAligned),Derived>::run(*this);
+    }
+
+    inline const Scalar& coeff(int row, int col) const
+    {
+      if(IsRowMajor)
+        return m_data[col + row * stride()];
+      else // column-major
+        return m_data[row + col * stride()];
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      if(IsRowMajor)
+        return const_cast<Scalar*>(m_data)[col + row * stride()];
+      else // column-major
+        return const_cast<Scalar*>(m_data)[row + col * stride()];
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      ei_assert(Derived::IsVectorAtCompileTime || (ei_traits<Derived>::Flags & LinearAccessBit));
+      if ( ((RowsAtCompileTime == 1) == IsRowMajor) )
+        return m_data[index];
+      else
+        return m_data[index*stride()];
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      ei_assert(Derived::IsVectorAtCompileTime || (ei_traits<Derived>::Flags & LinearAccessBit));
+      if ( ((RowsAtCompileTime == 1) == IsRowMajor) )
+        return const_cast<Scalar*>(m_data)[index];
+      else
+        return const_cast<Scalar*>(m_data)[index*stride()];
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(int row, int col) const
+    {
+      return ei_ploadt<Scalar, int(PacketAccess) == ForceAligned ? Aligned : LoadMode>
+               (m_data + (IsRowMajor ? col + row * stride()
+                                     : row + col * stride()));
+    }
+
+    template<int LoadMode>
+    inline PacketScalar packet(int index) const
+    {
+      return ei_ploadt<Scalar, int(PacketAccess) == ForceAligned ? Aligned : LoadMode>(m_data + index);
+    }
+
+    template<int StoreMode>
+    inline void writePacket(int row, int col, const PacketScalar& x)
+    {
+      ei_pstoret<Scalar, PacketScalar, int(PacketAccess) == ForceAligned ? Aligned : StoreMode>
+               (const_cast<Scalar*>(m_data) + (IsRowMajor ? col + row * stride()
+                                                          : row + col * stride()), x);
+    }
+
+    template<int StoreMode>
+    inline void writePacket(int index, const PacketScalar& x)
+    {
+      ei_pstoret<Scalar, PacketScalar, int(PacketAccess) == ForceAligned ? Aligned : StoreMode>
+        (const_cast<Scalar*>(m_data) + index, x);
+    }
+
+    inline MapBase(const Scalar* data) : m_data(data), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
+    {
+      EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
+    }
+
+    inline MapBase(const Scalar* data, int size)
+            : m_data(data),
+              m_rows(RowsAtCompileTime == Dynamic ? size : RowsAtCompileTime),
+              m_cols(ColsAtCompileTime == Dynamic ? size : ColsAtCompileTime)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+      ei_assert(size > 0 || data == 0);
+      ei_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == size);
+    }
+
+    inline MapBase(const Scalar* data, int rows, int cols)
+            : m_data(data), m_rows(rows), m_cols(cols)
+    {
+      ei_assert( (data == 0)
+              || (   rows > 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
+                  && cols > 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
+    }
+
+    Derived& operator=(const MapBase& other)
+    {
+      return Base::operator=(other);
+    }
+
+    template<typename OtherDerived>
+    Derived& operator=(const MatrixBase<OtherDerived>& other)
+    {
+      return Base::operator=(other);
+    }
+    
+    using Base::operator*=;
+
+    template<typename OtherDerived>
+    Derived& operator+=(const MatrixBase<OtherDerived>& other)
+    { return derived() = forceAligned() + other; }
+
+    template<typename OtherDerived>
+    Derived& operator-=(const MatrixBase<OtherDerived>& other)
+    { return derived() = forceAligned() - other; }
+
+    Derived& operator*=(const Scalar& other)
+    { return derived() = forceAligned() * other; }
+
+    Derived& operator/=(const Scalar& other)
+    { return derived() = forceAligned() / other; }
+
+  protected:
+    const Scalar* EIGEN_RESTRICT m_data;
+    const ei_int_if_dynamic<RowsAtCompileTime> m_rows;
+    const ei_int_if_dynamic<ColsAtCompileTime> m_cols;
+};
+
+#endif // EIGEN_MAPBASE_H
diff --git a/extern/Eigen2/Eigen/src/Core/MathFunctions.h b/extern/Eigen2/Eigen/src/Core/MathFunctions.h
new file mode 100644
index 00000000000..1ee64af02c6
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/MathFunctions.h
@@ -0,0 +1,295 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MATHFUNCTIONS_H
+#define EIGEN_MATHFUNCTIONS_H
+
+template<typename T> inline typename NumTraits<T>::Real precision();
+template<typename T> inline typename NumTraits<T>::Real machine_epsilon();
+template<typename T> inline T ei_random(T a, T b);
+template<typename T> inline T ei_random();
+template<typename T> inline T ei_random_amplitude()
+{
+  if(NumTraits<T>::HasFloatingPoint) return static_cast<T>(1);
+  else return static_cast<T>(10);
+}
+
+template<typename T> inline T ei_hypot(T x, T y)
+{
+  T _x = ei_abs(x);
+  T _y = ei_abs(y);
+  T p = std::max(_x, _y);
+  T q = std::min(_x, _y);
+  T qp = q/p;
+  return p * ei_sqrt(T(1) + qp*qp);
+}
+
+/**************
+***   int   ***
+**************/
+
+template<> inline int precision<int>() { return 0; }
+template<> inline int machine_epsilon<int>() { return 0; }
+inline int ei_real(int x)  { return x; }
+inline int ei_imag(int)    { return 0; }
+inline int ei_conj(int x)  { return x; }
+inline int ei_abs(int x)   { return abs(x); }
+inline int ei_abs2(int x)  { return x*x; }
+inline int ei_sqrt(int)  { ei_assert(false); return 0; }
+inline int ei_exp(int)  { ei_assert(false); return 0; }
+inline int ei_log(int)  { ei_assert(false); return 0; }
+inline int ei_sin(int)  { ei_assert(false); return 0; }
+inline int ei_cos(int)  { ei_assert(false); return 0; }
+inline int ei_atan2(int, int)  { ei_assert(false); return 0; }
+inline int ei_pow(int x, int y) { return int(std::pow(double(x), y)); }
+
+template<> inline int ei_random(int a, int b)
+{
+  // We can't just do rand()%n as only the high-order bits are really random
+  return a + static_cast<int>((b-a+1) * (rand() / (RAND_MAX + 1.0)));
+}
+template<> inline int ei_random()
+{
+  return ei_random<int>(-ei_random_amplitude<int>(), ei_random_amplitude<int>());
+}
+inline bool ei_isMuchSmallerThan(int a, int, int = precision<int>())
+{
+  return a == 0;
+}
+inline bool ei_isApprox(int a, int b, int = precision<int>())
+{
+  return a == b;
+}
+inline bool ei_isApproxOrLessThan(int a, int b, int = precision<int>())
+{
+  return a <= b;
+}
+
+/**************
+*** float   ***
+**************/
+
+template<> inline float precision<float>() { return 1e-5f; }
+template<> inline float machine_epsilon<float>() { return 1.192e-07f; }
+inline float ei_real(float x)  { return x; }
+inline float ei_imag(float)    { return 0.f; }
+inline float ei_conj(float x)  { return x; }
+inline float ei_abs(float x)   { return std::abs(x); }
+inline float ei_abs2(float x)  { return x*x; }
+inline float ei_sqrt(float x)  { return std::sqrt(x); }
+inline float ei_exp(float x)   { return std::exp(x); }
+inline float ei_log(float x)   { return std::log(x); }
+inline float ei_sin(float x)   { return std::sin(x); }
+inline float ei_cos(float x)   { return std::cos(x); }
+inline float ei_atan2(float y, float x) { return std::atan2(y,x); }
+inline float ei_pow(float x, float y)  { return std::pow(x, y); }
+
+template<> inline float ei_random(float a, float b)
+{
+#ifdef EIGEN_NICE_RANDOM
+  int i;
+  do { i = ei_random<int>(256*int(a),256*int(b));
+  } while(i==0);
+  return float(i)/256.f;
+#else
+  return a + (b-a) * float(std::rand()) / float(RAND_MAX);
+#endif
+}
+template<> inline float ei_random()
+{
+  return ei_random<float>(-ei_random_amplitude<float>(), ei_random_amplitude<float>());
+}
+inline bool ei_isMuchSmallerThan(float a, float b, float prec = precision<float>())
+{
+  return ei_abs(a) <= ei_abs(b) * prec;
+}
+inline bool ei_isApprox(float a, float b, float prec = precision<float>())
+{
+  return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
+}
+inline bool ei_isApproxOrLessThan(float a, float b, float prec = precision<float>())
+{
+  return a <= b || ei_isApprox(a, b, prec);
+}
+
+/**************
+*** double  ***
+**************/
+
+template<> inline double precision<double>() { return 1e-11; }
+template<> inline double machine_epsilon<double>() { return 2.220e-16; }
+
+inline double ei_real(double x)  { return x; }
+inline double ei_imag(double)    { return 0.; }
+inline double ei_conj(double x)  { return x; }
+inline double ei_abs(double x)   { return std::abs(x); }
+inline double ei_abs2(double x)  { return x*x; }
+inline double ei_sqrt(double x)  { return std::sqrt(x); }
+inline double ei_exp(double x)   { return std::exp(x); }
+inline double ei_log(double x)   { return std::log(x); }
+inline double ei_sin(double x)   { return std::sin(x); }
+inline double ei_cos(double x)   { return std::cos(x); }
+inline double ei_atan2(double y, double x) { return std::atan2(y,x); }
+inline double ei_pow(double x, double y) { return std::pow(x, y); }
+
+template<> inline double ei_random(double a, double b)
+{
+#ifdef EIGEN_NICE_RANDOM
+  int i;
+  do { i= ei_random<int>(256*int(a),256*int(b));
+  } while(i==0);
+  return i/256.;
+#else
+  return a + (b-a) * std::rand() / RAND_MAX;
+#endif
+}
+template<> inline double ei_random()
+{
+  return ei_random<double>(-ei_random_amplitude<double>(), ei_random_amplitude<double>());
+}
+inline bool ei_isMuchSmallerThan(double a, double b, double prec = precision<double>())
+{
+  return ei_abs(a) <= ei_abs(b) * prec;
+}
+inline bool ei_isApprox(double a, double b, double prec = precision<double>())
+{
+  return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
+}
+inline bool ei_isApproxOrLessThan(double a, double b, double prec = precision<double>())
+{
+  return a <= b || ei_isApprox(a, b, prec);
+}
+
+/*********************
+*** complex<float> ***
+*********************/
+
+template<> inline float precision<std::complex<float> >() { return precision<float>(); }
+template<> inline float machine_epsilon<std::complex<float> >() { return machine_epsilon<float>(); }
+inline float ei_real(const std::complex<float>& x) { return std::real(x); }
+inline float ei_imag(const std::complex<float>& x) { return std::imag(x); }
+inline std::complex<float> ei_conj(const std::complex<float>& x) { return std::conj(x); }
+inline float ei_abs(const std::complex<float>& x) { return std::abs(x); }
+inline float ei_abs2(const std::complex<float>& x) { return std::norm(x); }
+inline std::complex<float> ei_exp(std::complex<float> x)  { return std::exp(x); }
+inline std::complex<float> ei_sin(std::complex<float> x)  { return std::sin(x); }
+inline std::complex<float> ei_cos(std::complex<float> x)  { return std::cos(x); }
+inline std::complex<float> ei_atan2(std::complex<float>, std::complex<float> )  { ei_assert(false); return 0; }
+
+template<> inline std::complex<float> ei_random()
+{
+  return std::complex<float>(ei_random<float>(), ei_random<float>());
+}
+inline bool ei_isMuchSmallerThan(const std::complex<float>& a, const std::complex<float>& b, float prec = precision<float>())
+{
+  return ei_abs2(a) <= ei_abs2(b) * prec * prec;
+}
+inline bool ei_isMuchSmallerThan(const std::complex<float>& a, float b, float prec = precision<float>())
+{
+  return ei_abs2(a) <= ei_abs2(b) * prec * prec;
+}
+inline bool ei_isApprox(const std::complex<float>& a, const std::complex<float>& b, float prec = precision<float>())
+{
+  return ei_isApprox(ei_real(a), ei_real(b), prec)
+      && ei_isApprox(ei_imag(a), ei_imag(b), prec);
+}
+// ei_isApproxOrLessThan wouldn't make sense for complex numbers
+
+/**********************
+*** complex<double> ***
+**********************/
+
+template<> inline double precision<std::complex<double> >() { return precision<double>(); }
+template<> inline double machine_epsilon<std::complex<double> >() { return machine_epsilon<double>(); }
+inline double ei_real(const std::complex<double>& x) { return std::real(x); }
+inline double ei_imag(const std::complex<double>& x) { return std::imag(x); }
+inline std::complex<double> ei_conj(const std::complex<double>& x) { return std::conj(x); }
+inline double ei_abs(const std::complex<double>& x) { return std::abs(x); }
+inline double ei_abs2(const std::complex<double>& x) { return std::norm(x); }
+inline std::complex<double> ei_exp(std::complex<double> x)  { return std::exp(x); }
+inline std::complex<double> ei_sin(std::complex<double> x)  { return std::sin(x); }
+inline std::complex<double> ei_cos(std::complex<double> x)  { return std::cos(x); }
+inline std::complex<double> ei_atan2(std::complex<double>, std::complex<double>)  { ei_assert(false); return 0; }
+
+template<> inline std::complex<double> ei_random()
+{
+  return std::complex<double>(ei_random<double>(), ei_random<double>());
+}
+inline bool ei_isMuchSmallerThan(const std::complex<double>& a, const std::complex<double>& b, double prec = precision<double>())
+{
+  return ei_abs2(a) <= ei_abs2(b) * prec * prec;
+}
+inline bool ei_isMuchSmallerThan(const std::complex<double>& a, double b, double prec = precision<double>())
+{
+  return ei_abs2(a) <= ei_abs2(b) * prec * prec;
+}
+inline bool ei_isApprox(const std::complex<double>& a, const std::complex<double>& b, double prec = precision<double>())
+{
+  return ei_isApprox(ei_real(a), ei_real(b), prec)
+      && ei_isApprox(ei_imag(a), ei_imag(b), prec);
+}
+// ei_isApproxOrLessThan wouldn't make sense for complex numbers
+
+
+/******************
+*** long double ***
+******************/
+
+template<> inline long double precision<long double>() { return precision<double>(); }
+template<> inline long double machine_epsilon<long double>() { return 1.084e-19l; }
+inline long double ei_real(long double x)  { return x; }
+inline long double ei_imag(long double)    { return 0.; }
+inline long double ei_conj(long double x)  { return x; }
+inline long double ei_abs(long double x)   { return std::abs(x); }
+inline long double ei_abs2(long double x)  { return x*x; }
+inline long double ei_sqrt(long double x)  { return std::sqrt(x); }
+inline long double ei_exp(long double x)   { return std::exp(x); }
+inline long double ei_log(long double x)   { return std::log(x); }
+inline long double ei_sin(long double x)   { return std::sin(x); }
+inline long double ei_cos(long double x)   { return std::cos(x); }
+inline long double ei_atan2(long double y, long double x) { return std::atan2(y,x); }
+inline long double ei_pow(long double x, long double y)  { return std::pow(x, y); }
+
+template<> inline long double ei_random(long double a, long double b)
+{
+  return ei_random<double>(static_cast<double>(a),static_cast<double>(b));
+}
+template<> inline long double ei_random()
+{
+  return ei_random<double>(-ei_random_amplitude<double>(), ei_random_amplitude<double>());
+}
+inline bool ei_isMuchSmallerThan(long double a, long double b, long double prec = precision<long double>())
+{
+  return ei_abs(a) <= ei_abs(b) * prec;
+}
+inline bool ei_isApprox(long double a, long double b, long double prec = precision<long double>())
+{
+  return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
+}
+inline bool ei_isApproxOrLessThan(long double a, long double b, long double prec = precision<long double>())
+{
+  return a <= b || ei_isApprox(a, b, prec);
+}
+
+#endif // EIGEN_MATHFUNCTIONS_H
diff --git a/extern/Eigen2/Eigen/src/Core/Matrix.h b/extern/Eigen2/Eigen/src/Core/Matrix.h
new file mode 100644
index 00000000000..ffd16d37606
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Matrix.h
@@ -0,0 +1,637 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MATRIX_H
+#define EIGEN_MATRIX_H
+
+
+/** \class Matrix
+  *
+  * \brief The matrix class, also used for vectors and row-vectors
+  *
+  * The %Matrix class is the work-horse for all \em dense (\ref dense "note") matrices and vectors within Eigen.
+  * Vectors are matrices with one column, and row-vectors are matrices with one row.
+  *
+  * The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note").
+  *
+  * The first three template parameters are required:
+  * \param _Scalar Numeric type, i.e. float, double, int
+  * \param _Rows Number of rows, or \b Dynamic
+  * \param _Cols Number of columns, or \b Dynamic
+  *
+  * The remaining template parameters are optional -- in most cases you don't have to worry about them.
+  * \param _Options A combination of either \b RowMajor or \b ColMajor, and of either
+  *                 \b AutoAlign or \b DontAlign.
+  *                 The former controls storage order, and defaults to column-major. The latter controls alignment, which is required
+  *                 for vectorization. It defaults to aligning matrices except for fixed sizes that aren't a multiple of the packet size.
+  * \param _MaxRows Maximum number of rows. Defaults to \a _Rows (\ref maxrows "note").
+  * \param _MaxCols Maximum number of columns. Defaults to \a _Cols (\ref maxrows "note").
+  *
+  * Eigen provides a number of typedefs covering the usual cases. Here are some examples:
+  *
+  * \li \c Matrix2d is a 2x2 square matrix of doubles (\c Matrix<double, 2, 2>)
+  * \li \c Vector4f is a vector of 4 floats (\c Matrix<float, 4, 1>)
+  * \li \c RowVector3i is a row-vector of 3 ints (\c Matrix<int, 1, 3>)
+  *
+  * \li \c MatrixXf is a dynamic-size matrix of floats (\c Matrix<float, Dynamic, Dynamic>)
+  * \li \c VectorXf is a dynamic-size vector of floats (\c Matrix<float, Dynamic, 1>)
+  *
+  * See \link matrixtypedefs this page \endlink for a complete list of predefined \em %Matrix and \em Vector typedefs.
+  *
+  * You can access elements of vectors and matrices using normal subscripting:
+  *
+  * \code
+  * Eigen::VectorXd v(10);
+  * v[0] = 0.1;
+  * v[1] = 0.2;
+  * v(0) = 0.3;
+  * v(1) = 0.4;
+  *
+  * Eigen::MatrixXi m(10, 10);
+  * m(0, 1) = 1;
+  * m(0, 2) = 2;
+  * m(0, 3) = 3;
+  * \endcode
+  *
+  * <i><b>Some notes:</b></i>
+  *
+  * <dl>
+  * <dt><b>\anchor dense Dense versus sparse:</b></dt>
+  * <dd>This %Matrix class handles dense, not sparse matrices and vectors. For sparse matrices and vectors, see the Sparse module.
+  *
+  * Dense matrices and vectors are plain usual arrays of coefficients. All the coefficients are stored, in an ordinary contiguous array.
+  * This is unlike Sparse matrices and vectors where the coefficients are stored as a list of nonzero coefficients.</dd>
+  *
+  * <dt><b>\anchor fixedsize Fixed-size versus dynamic-size:</b></dt>
+  * <dd>Fixed-size means that the numbers of rows and columns are known are compile-time. In this case, Eigen allocates the array
+  * of coefficients as a fixed-size array, as a class member. This makes sense for very small matrices, typically up to 4x4, sometimes up
+  * to 16x16. Larger matrices should be declared as dynamic-size even if one happens to know their size at compile-time.
+  *
+  * Dynamic-size means that the numbers of rows or columns are not necessarily known at compile-time. In this case they are runtime
+  * variables, and the array of coefficients is allocated dynamically on the heap.
+  *
+  * Note that \em dense matrices, be they Fixed-size or Dynamic-size, <em>do not</em> expand dynamically in the sense of a std::map.
+  * If you want this behavior, see the Sparse module.</dd>
+  *
+  * <dt><b>\anchor maxrows _MaxRows and _MaxCols:</b></dt>
+  * <dd>In most cases, one just leaves these parameters to the default values.
+  * These parameters mean the maximum size of rows and columns that the matrix may have. They are useful in cases
+  * when the exact numbers of rows and columns are not known are compile-time, but it is known at compile-time that they cannot
+  * exceed a certain value. This happens when taking dynamic-size blocks inside fixed-size matrices: in this case _MaxRows and _MaxCols
+  * are the dimensions of the original matrix, while _Rows and _Cols are Dynamic.</dd>
+  * </dl>
+  *
+  * \see MatrixBase for the majority of the API methods for matrices
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+struct ei_traits<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
+{
+  typedef _Scalar Scalar;
+  enum {
+    RowsAtCompileTime = _Rows,
+    ColsAtCompileTime = _Cols,
+    MaxRowsAtCompileTime = _MaxRows,
+    MaxColsAtCompileTime = _MaxCols,
+    Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
+    CoeffReadCost = NumTraits<Scalar>::ReadCost
+  };
+};
+
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+class Matrix
+  : public MatrixBase<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
+{
+  public:
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix)
+    enum { Options = _Options };
+    friend class Eigen::Map<Matrix, Unaligned>;
+    typedef class Eigen::Map<Matrix, Unaligned> UnalignedMapType;
+    friend class Eigen::Map<Matrix, Aligned>;
+    typedef class Eigen::Map<Matrix, Aligned> AlignedMapType;
+
+  protected:
+    ei_matrix_storage<Scalar, MaxSizeAtCompileTime, RowsAtCompileTime, ColsAtCompileTime, Options> m_storage;
+
+  public:
+    enum { NeedsToAlign = (Options&AutoAlign) == AutoAlign
+                          && SizeAtCompileTime!=Dynamic && ((sizeof(Scalar)*SizeAtCompileTime)%16)==0 };
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+    
+    Base& base() { return *static_cast<Base*>(this); }
+    const Base& base() const { return *static_cast<const Base*>(this); }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_storage.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_storage.cols(); }
+
+    EIGEN_STRONG_INLINE int stride(void) const
+    {
+      if(Flags & RowMajorBit)
+        return m_storage.cols();
+      else
+        return m_storage.rows();
+    }
+
+    EIGEN_STRONG_INLINE const Scalar& coeff(int row, int col) const
+    {
+      if(Flags & RowMajorBit)
+        return m_storage.data()[col + row * m_storage.cols()];
+      else // column-major
+        return m_storage.data()[row + col * m_storage.rows()];
+    }
+
+    EIGEN_STRONG_INLINE const Scalar& coeff(int index) const
+    {
+      return m_storage.data()[index];
+    }
+
+    EIGEN_STRONG_INLINE Scalar& coeffRef(int row, int col)
+    {
+      if(Flags & RowMajorBit)
+        return m_storage.data()[col + row * m_storage.cols()];
+      else // column-major
+        return m_storage.data()[row + col * m_storage.rows()];
+    }
+
+    EIGEN_STRONG_INLINE Scalar& coeffRef(int index)
+    {
+      return m_storage.data()[index];
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
+    {
+      return ei_ploadt<Scalar, LoadMode>
+               (m_storage.data() + (Flags & RowMajorBit
+                                   ? col + row * m_storage.cols()
+                                   : row + col * m_storage.rows()));
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE PacketScalar packet(int index) const
+    {
+      return ei_ploadt<Scalar, LoadMode>(m_storage.data() + index);
+    }
+
+    template<int StoreMode>
+    EIGEN_STRONG_INLINE void writePacket(int row, int col, const PacketScalar& x)
+    {
+      ei_pstoret<Scalar, PacketScalar, StoreMode>
+              (m_storage.data() + (Flags & RowMajorBit
+                                   ? col + row * m_storage.cols()
+                                   : row + col * m_storage.rows()), x);
+    }
+
+    template<int StoreMode>
+    EIGEN_STRONG_INLINE void writePacket(int index, const PacketScalar& x)
+    {
+      ei_pstoret<Scalar, PacketScalar, StoreMode>(m_storage.data() + index, x);
+    }
+
+    /** \returns a const pointer to the data array of this matrix */
+    EIGEN_STRONG_INLINE const Scalar *data() const
+    { return m_storage.data(); }
+
+    /** \returns a pointer to the data array of this matrix */
+    EIGEN_STRONG_INLINE Scalar *data()
+    { return m_storage.data(); }
+
+    /** Resizes \c *this to a \a rows x \a cols matrix.
+      *
+      * Makes sense for dynamic-size matrices only.
+      *
+      * If the current number of coefficients of \c *this exactly matches the
+      * product \a rows * \a cols, then no memory allocation is performed and
+      * the current values are left unchanged. In all other cases, including
+      * shrinking, the data is reallocated and all previous values are lost.
+      *
+      * \sa resize(int) for vectors.
+      */
+    inline void resize(int rows, int cols)
+    {
+      ei_assert((MaxRowsAtCompileTime == Dynamic || MaxRowsAtCompileTime >= rows)
+             && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
+             && (MaxColsAtCompileTime == Dynamic || MaxColsAtCompileTime >= cols)
+             && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
+      m_storage.resize(rows * cols, rows, cols);
+    }
+
+    /** Resizes \c *this to a vector of length \a size
+      *
+      * \sa resize(int,int) for the details.
+      */
+    inline void resize(int size)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix)
+      if(RowsAtCompileTime == 1)
+        m_storage.resize(size, 1, size);
+      else
+        m_storage.resize(size, size, 1);
+    }
+
+    /** Copies the value of the expression \a other into \c *this with automatic resizing.
+      *
+      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+      * it will be initialized.
+      *
+      * Note that copying a row-vector into a vector (and conversely) is allowed.
+      * The resizing, if any, is then done in the appropriate way so that row-vectors
+      * remain row-vectors and vectors remain vectors.
+      */
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE Matrix& operator=(const MatrixBase<OtherDerived>& other)
+    {
+      return _set(other);
+    }
+
+    /** This is a special case of the templated operator=. Its purpose is to
+      * prevent a default operator= from hiding the templated operator=.
+      */
+    EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other)
+    {
+      return _set(other);
+    }
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Matrix, +=)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Matrix, -=)
+    EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Matrix, *=)
+    EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Matrix, /=)
+
+    /** Default constructor.
+      *
+      * For fixed-size matrices, does nothing.
+      *
+      * For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
+      * is called a null matrix. This constructor is the unique way to create null matrices: resizing
+      * a matrix to 0 is not supported.
+      *
+      * \sa resize(int,int)
+      */
+    EIGEN_STRONG_INLINE explicit Matrix() : m_storage()
+    {
+      _check_template_params();
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal */
+    Matrix(ei_constructor_without_unaligned_array_assert)
+      : m_storage(ei_constructor_without_unaligned_array_assert())
+    {}
+#endif
+
+    /** Constructs a vector or row-vector with given dimension. \only_for_vectors
+      *
+      * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,
+      * it is redundant to pass the dimension here, so it makes more sense to use the default
+      * constructor Matrix() instead.
+      */
+    EIGEN_STRONG_INLINE explicit Matrix(int dim)
+      : m_storage(dim, RowsAtCompileTime == 1 ? 1 : dim, ColsAtCompileTime == 1 ? 1 : dim)
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix)
+      ei_assert(dim > 0);
+      ei_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim);
+    }
+
+    /** This constructor has two very different behaviors, depending on the type of *this.
+      *
+      * \li When Matrix is a fixed-size vector type of size 2, this constructor constructs
+      *     an initialized vector. The parameters \a x, \a y are copied into the first and second
+      *     coords of the vector respectively.
+      * \li Otherwise, this constructor constructs an uninitialized matrix with \a x rows and
+      *     \a y columns. This is useful for dynamic-size matrices. For fixed-size matrices,
+      *     it is redundant to pass these parameters, so one should use the default constructor
+      *     Matrix() instead.
+      */
+    EIGEN_STRONG_INLINE Matrix(int x, int y) : m_storage(x*y, x, y)
+    {
+      _check_template_params();
+      if((RowsAtCompileTime == 1 && ColsAtCompileTime == 2)
+      || (RowsAtCompileTime == 2 && ColsAtCompileTime == 1))
+      {
+        m_storage.data()[0] = Scalar(x);
+        m_storage.data()[1] = Scalar(y);
+      }
+      else
+      {
+        ei_assert(x > 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == x)
+               && y > 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == y));
+      }
+    }
+    /** constructs an initialized 2D vector with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(const float& x, const float& y)
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 2)
+      m_storage.data()[0] = x;
+      m_storage.data()[1] = y;
+    }
+    /** constructs an initialized 2D vector with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(const double& x, const double& y)
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 2)
+      m_storage.data()[0] = x;
+      m_storage.data()[1] = y;
+    }
+    /** constructs an initialized 3D vector with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z)
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 3)
+      m_storage.data()[0] = x;
+      m_storage.data()[1] = y;
+      m_storage.data()[2] = z;
+    }
+    /** constructs an initialized 4D vector with given coefficients */
+    EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 4)
+      m_storage.data()[0] = x;
+      m_storage.data()[1] = y;
+      m_storage.data()[2] = z;
+      m_storage.data()[3] = w;
+    }
+
+    explicit Matrix(const Scalar *data);
+
+    /** Constructor copying the value of the expression \a other */
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE Matrix(const MatrixBase<OtherDerived>& other)
+             : m_storage(other.rows() * other.cols(), other.rows(), other.cols())
+    {
+      _check_template_params();
+      _set_noalias(other);
+    }
+    /** Copy constructor */
+    EIGEN_STRONG_INLINE Matrix(const Matrix& other)
+            : Base(), m_storage(other.rows() * other.cols(), other.rows(), other.cols())
+    {
+      _check_template_params();
+      _set_noalias(other);
+    }
+    /** Destructor */
+    inline ~Matrix() {}
+
+    /** Override MatrixBase::swap() since for dynamic-sized matrices of same type it is enough to swap the
+      * data pointers.
+      */
+    template<typename OtherDerived>
+    void swap(const MatrixBase<OtherDerived>& other);
+
+    /** \name Map
+      * These are convenience functions returning Map objects. The Map() static functions return unaligned Map objects,
+      * while the AlignedMap() functions return aligned Map objects and thus should be called only with 16-byte-aligned
+      * \a data pointers.
+      *
+      * \see class Map
+      */
+    //@{
+    inline static const UnalignedMapType Map(const Scalar* data)
+    { return UnalignedMapType(data); }
+    inline static UnalignedMapType Map(Scalar* data)
+    { return UnalignedMapType(data); }
+    inline static const UnalignedMapType Map(const Scalar* data, int size)
+    { return UnalignedMapType(data, size); }
+    inline static UnalignedMapType Map(Scalar* data, int size)
+    { return UnalignedMapType(data, size); }
+    inline static const UnalignedMapType Map(const Scalar* data, int rows, int cols)
+    { return UnalignedMapType(data, rows, cols); }
+    inline static UnalignedMapType Map(Scalar* data, int rows, int cols)
+    { return UnalignedMapType(data, rows, cols); }
+
+    inline static const AlignedMapType MapAligned(const Scalar* data)
+    { return AlignedMapType(data); }
+    inline static AlignedMapType MapAligned(Scalar* data)
+    { return AlignedMapType(data); }
+    inline static const AlignedMapType MapAligned(const Scalar* data, int size)
+    { return AlignedMapType(data, size); }
+    inline static AlignedMapType MapAligned(Scalar* data, int size)
+    { return AlignedMapType(data, size); }
+    inline static const AlignedMapType MapAligned(const Scalar* data, int rows, int cols)
+    { return AlignedMapType(data, rows, cols); }
+    inline static AlignedMapType MapAligned(Scalar* data, int rows, int cols)
+    { return AlignedMapType(data, rows, cols); }
+    //@}
+
+    using Base::setConstant;
+    Matrix& setConstant(int size, const Scalar& value);
+    Matrix& setConstant(int rows, int cols, const Scalar& value);
+
+    using Base::setZero;
+    Matrix& setZero(int size);
+    Matrix& setZero(int rows, int cols);
+
+    using Base::setOnes;
+    Matrix& setOnes(int size);
+    Matrix& setOnes(int rows, int cols);
+
+    using Base::setRandom;
+    Matrix& setRandom(int size);
+    Matrix& setRandom(int rows, int cols);
+
+    using Base::setIdentity;
+    Matrix& setIdentity(int rows, int cols);
+
+/////////// Geometry module ///////////
+
+    template<typename OtherDerived>
+    explicit Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
+    template<typename OtherDerived>
+    Matrix& operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
+
+    // allow to extend Matrix outside Eigen
+    #ifdef EIGEN_MATRIX_PLUGIN
+    #include EIGEN_MATRIX_PLUGIN
+    #endif
+
+  private:
+    /** \internal Resizes *this in preparation for assigning \a other to it.
+      * Takes care of doing all the checking that's needed.
+      *
+      * Note that copying a row-vector into a vector (and conversely) is allowed.
+      * The resizing, if any, is then done in the appropriate way so that row-vectors
+      * remain row-vectors and vectors remain vectors.
+      */
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE void _resize_to_match(const MatrixBase<OtherDerived>& other)
+    {
+      if(RowsAtCompileTime == 1)
+      {
+        ei_assert(other.isVector());
+        resize(1, other.size());
+      }
+      else if(ColsAtCompileTime == 1)
+      {
+        ei_assert(other.isVector());
+        resize(other.size(), 1);
+      }
+      else resize(other.rows(), other.cols());
+    }
+
+    /** \internal Copies the value of the expression \a other into \c *this with automatic resizing.
+      *
+      * *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
+      * it will be initialized.
+      *
+      * Note that copying a row-vector into a vector (and conversely) is allowed.
+      * The resizing, if any, is then done in the appropriate way so that row-vectors
+      * remain row-vectors and vectors remain vectors.
+      *
+      * \sa operator=(const MatrixBase<OtherDerived>&), _set_noalias()
+      */
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE Matrix& _set(const MatrixBase<OtherDerived>& other)
+    {
+      _set_selector(other.derived(), typename ei_meta_if<bool(int(OtherDerived::Flags) & EvalBeforeAssigningBit), ei_meta_true, ei_meta_false>::ret());
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const ei_meta_true&) { _set_noalias(other.eval()); }
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const ei_meta_false&) { _set_noalias(other); }
+
+    /** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which
+      * is the case when creating a new matrix) so one can enforce lazy evaluation.
+      *
+      * \sa operator=(const MatrixBase<OtherDerived>&), _set()
+      */
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE Matrix& _set_noalias(const MatrixBase<OtherDerived>& other)
+    {
+      _resize_to_match(other);
+      // the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because
+      // it wouldn't allow to copy a row-vector into a column-vector.
+      return ei_assign_selector<Matrix,OtherDerived,false>::run(*this, other.derived());
+    }
+
+    static EIGEN_STRONG_INLINE void _check_template_params()
+    {
+        EIGEN_STATIC_ASSERT((_Rows > 0
+                        && _Cols > 0
+                        && _MaxRows <= _Rows
+                        && _MaxCols <= _Cols
+                        && (_Options & (AutoAlign|RowMajor)) == _Options),
+          INVALID_MATRIX_TEMPLATE_PARAMETERS)
+    }
+    
+    template<typename MatrixType, typename OtherDerived, bool IsSameType, bool IsDynamicSize>
+    friend struct ei_matrix_swap_impl;
+};
+
+template<typename MatrixType, typename OtherDerived,
+         bool IsSameType = ei_is_same_type<MatrixType, OtherDerived>::ret,
+         bool IsDynamicSize = MatrixType::SizeAtCompileTime==Dynamic>
+struct ei_matrix_swap_impl
+{
+  static inline void run(MatrixType& matrix, MatrixBase<OtherDerived>& other)
+  {
+    matrix.base().swap(other);
+  }
+};
+
+template<typename MatrixType, typename OtherDerived>
+struct ei_matrix_swap_impl<MatrixType, OtherDerived, true, true>
+{
+  static inline void run(MatrixType& matrix, MatrixBase<OtherDerived>& other)
+  {
+    matrix.m_storage.swap(other.derived().m_storage);
+  }
+};
+
+template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
+template<typename OtherDerived>
+inline void Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::swap(const MatrixBase<OtherDerived>& other)
+{
+  ei_matrix_swap_impl<Matrix, OtherDerived>::run(*this, *const_cast<MatrixBase<OtherDerived>*>(&other));
+}
+
+
+/** \defgroup matrixtypedefs Global matrix typedefs
+  *
+  * \ingroup Core_Module
+  *
+  * Eigen defines several typedef shortcuts for most common matrix and vector types.
+  *
+  * The general patterns are the following:
+  *
+  * \c MatrixSizeType where \c Size can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
+  * and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
+  * for complex double.
+  *
+  * For example, \c Matrix3d is a fixed-size 3x3 matrix type of doubles, and \c MatrixXf is a dynamic-size matrix of floats.
+  *
+  * There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is
+  * a fixed-size vector of 4 complex floats.
+  *
+  * \sa class Matrix
+  */
+
+#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)   \
+/** \ingroup matrixtypedefs */                                    \
+typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix;  \
+/** \ingroup matrixtypedefs */                                    \
+typedef Matrix<Type, Size, 1>    Vector##SizeSuffix##TypeSuffix;  \
+/** \ingroup matrixtypedefs */                                    \
+typedef Matrix<Type, 1, Size>    RowVector##SizeSuffix##TypeSuffix;
+
+#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
+EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
+EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
+EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
+EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
+
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int,                  i)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float,                f)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double,               d)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>,  cf)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
+
+#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
+#undef EIGEN_MAKE_TYPEDEFS
+
+#undef EIGEN_MAKE_TYPEDEFS_LARGE
+
+#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
+using Eigen::Matrix##SizeSuffix##TypeSuffix; \
+using Eigen::Vector##SizeSuffix##TypeSuffix; \
+using Eigen::RowVector##SizeSuffix##TypeSuffix;
+
+#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
+
+#define EIGEN_USING_MATRIX_TYPEDEFS \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \
+EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd)
+
+#endif // EIGEN_MATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Core/MatrixBase.h b/extern/Eigen2/Eigen/src/Core/MatrixBase.h
new file mode 100644
index 00000000000..7935a7554ea
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/MatrixBase.h
@@ -0,0 +1,632 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MATRIXBASE_H
+#define EIGEN_MATRIXBASE_H
+
+/** \class MatrixBase
+  *
+  * \brief Base class for all matrices, vectors, and expressions
+  *
+  * This class is the base that is inherited by all matrix, vector, and expression
+  * types. Most of the Eigen API is contained in this class. Other important classes for
+  * the Eigen API are Matrix, Cwise, and PartialRedux.
+  *
+  * Note that some methods are defined in the \ref Array module.
+  *
+  * \param Derived is the derived type, e.g. a matrix type, or an expression, etc.
+  *
+  * When writing a function taking Eigen objects as argument, if you want your function
+  * to take as argument any matrix, vector, or expression, just let it take a
+  * MatrixBase argument. As an example, here is a function printFirstRow which, given
+  * a matrix, vector, or expression \a x, prints the first row of \a x.
+  *
+  * \code
+    template<typename Derived>
+    void printFirstRow(const Eigen::MatrixBase<Derived>& x)
+    {
+      cout << x.row(0) << endl;
+    }
+  * \endcode
+  *
+  */
+template<typename Derived> class MatrixBase
+{
+  public:
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    class InnerIterator;
+
+    typedef typename ei_traits<Derived>::Scalar Scalar;
+    typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    enum {
+
+      RowsAtCompileTime = ei_traits<Derived>::RowsAtCompileTime,
+        /**< The number of rows at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+      ColsAtCompileTime = ei_traits<Derived>::ColsAtCompileTime,
+        /**< The number of columns at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+
+      SizeAtCompileTime = (ei_size_at_compile_time<ei_traits<Derived>::RowsAtCompileTime,
+                                                   ei_traits<Derived>::ColsAtCompileTime>::ret),
+        /**< This is equal to the number of coefficients, i.e. the number of
+          * rows times the number of columns, or to \a Dynamic if this is not
+          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+      MaxRowsAtCompileTime = ei_traits<Derived>::MaxRowsAtCompileTime,
+        /**< This value is equal to the maximum possible number of rows that this expression
+          * might have. If this expression might have an arbitrarily high number of rows,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
+          */
+
+      MaxColsAtCompileTime = ei_traits<Derived>::MaxColsAtCompileTime,
+        /**< This value is equal to the maximum possible number of columns that this expression
+          * might have. If this expression might have an arbitrarily high number of columns,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
+          */
+
+      MaxSizeAtCompileTime = (ei_size_at_compile_time<ei_traits<Derived>::MaxRowsAtCompileTime,
+                                                      ei_traits<Derived>::MaxColsAtCompileTime>::ret),
+        /**< This value is equal to the maximum possible number of coefficients that this expression
+          * might have. If this expression might have an arbitrarily high number of coefficients,
+          * this value is set to \a Dynamic.
+          *
+          * This value is useful to know when evaluating an expression, in order to determine
+          * whether it is possible to avoid doing a dynamic memory allocation.
+          *
+          * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
+          */
+
+      IsVectorAtCompileTime = ei_traits<Derived>::RowsAtCompileTime == 1
+                           || ei_traits<Derived>::ColsAtCompileTime == 1,
+        /**< This is set to true if either the number of rows or the number of
+          * columns is known at compile-time to be equal to 1. Indeed, in that case,
+          * we are dealing with a column-vector (if there is only one column) or with
+          * a row-vector (if there is only one row). */
+
+      Flags = ei_traits<Derived>::Flags,
+        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+          * constructed from this one. See the \ref flags "list of flags".
+          */
+
+      CoeffReadCost = ei_traits<Derived>::CoeffReadCost
+        /**< This is a rough measure of how expensive it is to read one coefficient from
+          * this expression.
+          */
+    };
+
+    /** Default constructor. Just checks at compile-time for self-consistency of the flags. */
+    MatrixBase()
+    {
+      ei_assert(ei_are_flags_consistent<Flags>::ret);
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** This is the "real scalar" type; if the \a Scalar type is already real numbers
+      * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
+      * \a Scalar is \a std::complex<T> then RealScalar is \a T.
+      *
+      * \sa class NumTraits
+      */
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    /** type of the equivalent square matrix */
+    typedef Matrix<Scalar,EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime),
+                          EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    /** \returns the number of rows. \sa cols(), RowsAtCompileTime */
+    inline int rows() const { return derived().rows(); }
+    /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/
+    inline int cols() const { return derived().cols(); }
+    /** \returns the number of coefficients, which is \a rows()*cols().
+      * \sa rows(), cols(), SizeAtCompileTime. */
+    inline int size() const { return rows() * cols(); }
+    /** \returns the number of nonzero coefficients which is in practice the number
+      * of stored coefficients. */
+    inline int nonZeros() const { return derived.nonZeros(); }
+    /** \returns true if either the number of rows or the number of columns is equal to 1.
+      * In other words, this function returns
+      * \code rows()==1 || cols()==1 \endcode
+      * \sa rows(), cols(), IsVectorAtCompileTime. */
+    inline bool isVector() const { return rows()==1 || cols()==1; }
+    /** \returns the size of the storage major dimension,
+      * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
+    int outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
+    /** \returns the size of the inner dimension according to the storage order,
+      * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
+    int innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal the plain matrix type corresponding to this expression. Note that is not necessarily
+      * exactly the return type of eval(): in the case of plain matrices, the return type of eval() is a const
+      * reference to a matrix, not a matrix! It guaranteed however, that the return type of eval() is either
+      * PlainMatrixType or const PlainMatrixType&.
+      */
+    typedef typename ei_plain_matrix_type<Derived>::type PlainMatrixType;
+    /** \internal the column-major plain matrix type corresponding to this expression. Note that is not necessarily
+      * exactly the return type of eval(): in the case of plain matrices, the return type of eval() is a const
+      * reference to a matrix, not a matrix!
+      * The only difference from PlainMatrixType is that PlainMatrixType_ColMajor is guaranteed to be column-major.
+      */
+    typedef typename ei_plain_matrix_type<Derived>::type PlainMatrixType_ColMajor;
+
+    /** \internal Represents a matrix with all coefficients equal to one another*/
+    typedef CwiseNullaryOp<ei_scalar_constant_op<Scalar>,Derived> ConstantReturnType;
+    /** \internal Represents a scalar multiple of a matrix */
+    typedef CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, Derived> ScalarMultipleReturnType;
+    /** \internal Represents a quotient of a matrix by a scalar*/
+    typedef CwiseUnaryOp<ei_scalar_quotient1_op<Scalar>, Derived> ScalarQuotient1ReturnType;
+    /** \internal the return type of MatrixBase::conjugate() */
+    typedef typename ei_meta_if<NumTraits<Scalar>::IsComplex,
+                        const CwiseUnaryOp<ei_scalar_conjugate_op<Scalar>, Derived>,
+                        const Derived&
+                     >::ret ConjugateReturnType;
+    /** \internal the return type of MatrixBase::real() */
+    typedef CwiseUnaryOp<ei_scalar_real_op<Scalar>, Derived> RealReturnType;
+    /** \internal the return type of MatrixBase::imag() */
+    typedef CwiseUnaryOp<ei_scalar_imag_op<Scalar>, Derived> ImagReturnType;
+    /** \internal the return type of MatrixBase::adjoint() */
+    typedef Eigen::Transpose<NestByValue<typename ei_cleantype<ConjugateReturnType>::type> >
+            AdjointReturnType;
+    /** \internal the return type of MatrixBase::eigenvalues() */
+    typedef Matrix<typename NumTraits<typename ei_traits<Derived>::Scalar>::Real, ei_traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
+    /** \internal expression tyepe of a column */
+    typedef Block<Derived, ei_traits<Derived>::RowsAtCompileTime, 1> ColXpr;
+    /** \internal expression tyepe of a column */
+    typedef Block<Derived, 1, ei_traits<Derived>::ColsAtCompileTime> RowXpr;
+    /** \internal the return type of identity */
+    typedef CwiseNullaryOp<ei_scalar_identity_op<Scalar>,Derived> IdentityReturnType;
+    /** \internal the return type of unit vectors */
+    typedef Block<CwiseNullaryOp<ei_scalar_identity_op<Scalar>, SquareMatrixType>,
+                  ei_traits<Derived>::RowsAtCompileTime,
+                  ei_traits<Derived>::ColsAtCompileTime> BasisReturnType;
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+
+    /** Copies \a other into *this. \returns a reference to *this. */
+    template<typename OtherDerived>
+    Derived& operator=(const MatrixBase<OtherDerived>& other);
+
+    /** Special case of the template operator=, in order to prevent the compiler
+      * from generating a default operator= (issue hit with g++ 4.1)
+      */
+    inline Derived& operator=(const MatrixBase& other)
+    {
+      return this->operator=<Derived>(other);
+    }
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** Copies \a other into *this without evaluating other. \returns a reference to *this. */
+    template<typename OtherDerived>
+    Derived& lazyAssign(const MatrixBase<OtherDerived>& other);
+
+    /** Overloaded for cache friendly product evaluation */
+    template<typename Lhs, typename Rhs>
+    Derived& lazyAssign(const Product<Lhs,Rhs,CacheFriendlyProduct>& product);
+
+    /** Overloaded for cache friendly product evaluation */
+    template<typename OtherDerived>
+    Derived& lazyAssign(const Flagged<OtherDerived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other)
+    { return lazyAssign(other._expression()); }
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    CommaInitializer<Derived> operator<< (const Scalar& s);
+
+    template<typename OtherDerived>
+    CommaInitializer<Derived> operator<< (const MatrixBase<OtherDerived>& other);
+
+    const Scalar coeff(int row, int col) const;
+    const Scalar operator()(int row, int col) const;
+
+    Scalar& coeffRef(int row, int col);
+    Scalar& operator()(int row, int col);
+
+    const Scalar coeff(int index) const;
+    const Scalar operator[](int index) const;
+    const Scalar operator()(int index) const;
+
+    Scalar& coeffRef(int index);
+    Scalar& operator[](int index);
+    Scalar& operator()(int index);
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    template<typename OtherDerived>
+    void copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    void copyCoeff(int index, const MatrixBase<OtherDerived>& other);
+    template<typename OtherDerived, int StoreMode, int LoadMode>
+    void copyPacket(int row, int col, const MatrixBase<OtherDerived>& other);
+    template<typename OtherDerived, int StoreMode, int LoadMode>
+    void copyPacket(int index, const MatrixBase<OtherDerived>& other);
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    template<int LoadMode>
+    PacketScalar packet(int row, int col) const;
+    template<int StoreMode>
+    void writePacket(int row, int col, const PacketScalar& x);
+
+    template<int LoadMode>
+    PacketScalar packet(int index) const;
+    template<int StoreMode>
+    void writePacket(int index, const PacketScalar& x);
+
+    const Scalar x() const;
+    const Scalar y() const;
+    const Scalar z() const;
+    const Scalar w() const;
+    Scalar& x();
+    Scalar& y();
+    Scalar& z();
+    Scalar& w();
+
+
+    const CwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived> operator-() const;
+
+    template<typename OtherDerived>
+    const CwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+    operator+(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const CwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+    operator-(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    Derived& operator+=(const MatrixBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    Derived& operator-=(const MatrixBase<OtherDerived>& other);
+
+    template<typename Lhs,typename Rhs>
+    Derived& operator+=(const Flagged<Product<Lhs,Rhs,CacheFriendlyProduct>, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other);
+
+    Derived& operator*=(const Scalar& other);
+    Derived& operator/=(const Scalar& other);
+
+    const ScalarMultipleReturnType operator*(const Scalar& scalar) const;
+    const CwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
+    operator/(const Scalar& scalar) const;
+
+    inline friend const CwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<Derived>::Scalar>, Derived>
+    operator*(const Scalar& scalar, const MatrixBase& matrix)
+    { return matrix*scalar; }
+
+
+    template<typename OtherDerived>
+    const typename ProductReturnType<Derived,OtherDerived>::Type
+    operator*(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    Derived& operator*=(const MatrixBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    typename ei_plain_matrix_type_column_major<OtherDerived>::type
+		solveTriangular(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived>
+    void solveTriangularInPlace(const MatrixBase<OtherDerived>& other) const;
+
+
+    template<typename OtherDerived>
+    Scalar dot(const MatrixBase<OtherDerived>& other) const;
+    RealScalar squaredNorm() const;
+    RealScalar norm()  const;
+    const PlainMatrixType normalized() const;
+    void normalize();
+
+    Eigen::Transpose<Derived> transpose();
+    const Eigen::Transpose<Derived> transpose() const;
+    void transposeInPlace();
+    const AdjointReturnType adjoint() const;
+
+
+    RowXpr row(int i);
+    const RowXpr row(int i) const;
+
+    ColXpr col(int i);
+    const ColXpr col(int i) const;
+
+    Minor<Derived> minor(int row, int col);
+    const Minor<Derived> minor(int row, int col) const;
+
+    typename BlockReturnType<Derived>::Type block(int startRow, int startCol, int blockRows, int blockCols);
+    const typename BlockReturnType<Derived>::Type
+    block(int startRow, int startCol, int blockRows, int blockCols) const;
+
+    typename BlockReturnType<Derived>::SubVectorType segment(int start, int size);
+    const typename BlockReturnType<Derived>::SubVectorType segment(int start, int size) const;
+
+    typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size);
+    const typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size) const;
+
+    typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size);
+    const typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size) const;
+
+    typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols);
+    const typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols) const;
+
+    template<int BlockRows, int BlockCols>
+    typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol);
+    template<int BlockRows, int BlockCols>
+    const typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol) const;
+
+    template<int CRows, int CCols>
+    typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type);
+    template<int CRows, int CCols>
+    const typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type) const;
+
+    template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType start(void);
+    template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType start() const;
+
+    template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType end();
+    template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType end() const;
+
+    template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType segment(int start);
+    template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType segment(int start) const;
+
+    DiagonalCoeffs<Derived> diagonal();
+    const DiagonalCoeffs<Derived> diagonal() const;
+
+    template<unsigned int Mode> Part<Derived, Mode> part();
+    template<unsigned int Mode> const Part<Derived, Mode> part() const;
+
+
+    static const ConstantReturnType
+    Constant(int rows, int cols, const Scalar& value);
+    static const ConstantReturnType
+    Constant(int size, const Scalar& value);
+    static const ConstantReturnType
+    Constant(const Scalar& value);
+
+    template<typename CustomNullaryOp>
+    static const CwiseNullaryOp<CustomNullaryOp, Derived>
+    NullaryExpr(int rows, int cols, const CustomNullaryOp& func);
+    template<typename CustomNullaryOp>
+    static const CwiseNullaryOp<CustomNullaryOp, Derived>
+    NullaryExpr(int size, const CustomNullaryOp& func);
+    template<typename CustomNullaryOp>
+    static const CwiseNullaryOp<CustomNullaryOp, Derived>
+    NullaryExpr(const CustomNullaryOp& func);
+
+    static const ConstantReturnType Zero(int rows, int cols);
+    static const ConstantReturnType Zero(int size);
+    static const ConstantReturnType Zero();
+    static const ConstantReturnType Ones(int rows, int cols);
+    static const ConstantReturnType Ones(int size);
+    static const ConstantReturnType Ones();
+    static const IdentityReturnType Identity();
+    static const IdentityReturnType Identity(int rows, int cols);
+    static const BasisReturnType Unit(int size, int i);
+    static const BasisReturnType Unit(int i);
+    static const BasisReturnType UnitX();
+    static const BasisReturnType UnitY();
+    static const BasisReturnType UnitZ();
+    static const BasisReturnType UnitW();
+
+    const DiagonalMatrix<Derived> asDiagonal() const;
+
+    void fill(const Scalar& value);
+    Derived& setConstant(const Scalar& value);
+    Derived& setZero();
+    Derived& setOnes();
+    Derived& setRandom();
+    Derived& setIdentity();
+
+
+    template<typename OtherDerived>
+    bool isApprox(const MatrixBase<OtherDerived>& other,
+                  RealScalar prec = precision<Scalar>()) const;
+    bool isMuchSmallerThan(const RealScalar& other,
+                           RealScalar prec = precision<Scalar>()) const;
+    template<typename OtherDerived>
+    bool isMuchSmallerThan(const MatrixBase<OtherDerived>& other,
+                           RealScalar prec = precision<Scalar>()) const;
+
+    bool isApproxToConstant(const Scalar& value, RealScalar prec = precision<Scalar>()) const;
+    bool isConstant(const Scalar& value, RealScalar prec = precision<Scalar>()) const;
+    bool isZero(RealScalar prec = precision<Scalar>()) const;
+    bool isOnes(RealScalar prec = precision<Scalar>()) const;
+    bool isIdentity(RealScalar prec = precision<Scalar>()) const;
+    bool isDiagonal(RealScalar prec = precision<Scalar>()) const;
+
+    bool isUpperTriangular(RealScalar prec = precision<Scalar>()) const;
+    bool isLowerTriangular(RealScalar prec = precision<Scalar>()) const;
+
+    template<typename OtherDerived>
+    bool isOrthogonal(const MatrixBase<OtherDerived>& other,
+                      RealScalar prec = precision<Scalar>()) const;
+    bool isUnitary(RealScalar prec = precision<Scalar>()) const;
+
+    template<typename OtherDerived>
+    inline bool operator==(const MatrixBase<OtherDerived>& other) const
+    { return (cwise() == other).all(); }
+
+    template<typename OtherDerived>
+    inline bool operator!=(const MatrixBase<OtherDerived>& other) const
+    { return (cwise() != other).any(); }
+
+
+    template<typename NewType>
+    const CwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived> cast() const;
+
+    /** \returns the matrix or vector obtained by evaluating this expression.
+      *
+      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+      * a const reference, in order to avoid a useless copy.
+      */
+    EIGEN_STRONG_INLINE const typename ei_eval<Derived>::type eval() const
+    { return typename ei_eval<Derived>::type(derived()); }
+
+    template<typename OtherDerived>
+    void swap(const MatrixBase<OtherDerived>& other);
+
+    template<unsigned int Added>
+    const Flagged<Derived, Added, 0> marked() const;
+    const Flagged<Derived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit> lazy() const;
+
+    /** \returns number of elements to skip to pass from one row (resp. column) to another
+      * for a row-major (resp. column-major) matrix.
+      * Combined with coeffRef() and the \ref flags flags, it allows a direct access to the data
+      * of the underlying matrix.
+      */
+    inline int stride(void) const { return derived().stride(); }
+
+    inline const NestByValue<Derived> nestByValue() const;
+
+
+    ConjugateReturnType conjugate() const;
+    const RealReturnType real() const;
+    const ImagReturnType imag() const;
+
+    template<typename CustomUnaryOp>
+    const CwiseUnaryOp<CustomUnaryOp, Derived> unaryExpr(const CustomUnaryOp& func = CustomUnaryOp()) const;
+
+    template<typename CustomBinaryOp, typename OtherDerived>
+    const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
+    binaryExpr(const MatrixBase<OtherDerived> &other, const CustomBinaryOp& func = CustomBinaryOp()) const;
+
+
+    Scalar sum() const;
+    Scalar trace() const;
+
+    typename ei_traits<Derived>::Scalar minCoeff() const;
+    typename ei_traits<Derived>::Scalar maxCoeff() const;
+
+    typename ei_traits<Derived>::Scalar minCoeff(int* row, int* col) const;
+    typename ei_traits<Derived>::Scalar maxCoeff(int* row, int* col) const;
+
+    typename ei_traits<Derived>::Scalar minCoeff(int* index) const;
+    typename ei_traits<Derived>::Scalar maxCoeff(int* index) const;
+
+    template<typename BinaryOp>
+    typename ei_result_of<BinaryOp(typename ei_traits<Derived>::Scalar)>::type
+    redux(const BinaryOp& func) const;
+
+    template<typename Visitor>
+    void visit(Visitor& func) const;
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
+    inline Derived& const_cast_derived() const
+    { return *static_cast<Derived*>(const_cast<MatrixBase*>(this)); }
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    const Cwise<Derived> cwise() const;
+    Cwise<Derived> cwise();
+
+    inline const WithFormat<Derived> format(const IOFormat& fmt) const;
+
+/////////// Array module ///////////
+
+    bool all(void) const;
+    bool any(void) const;
+    int count() const;
+
+    const PartialRedux<Derived,Horizontal> rowwise() const;
+    const PartialRedux<Derived,Vertical> colwise() const;
+
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int rows, int cols);
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int size);
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random();
+
+    template<typename ThenDerived,typename ElseDerived>
+    const Select<Derived,ThenDerived,ElseDerived>
+    select(const MatrixBase<ThenDerived>& thenMatrix,
+           const MatrixBase<ElseDerived>& elseMatrix) const;
+
+    template<typename ThenDerived>
+    inline const Select<Derived,ThenDerived, NestByValue<typename ThenDerived::ConstantReturnType> >
+    select(const MatrixBase<ThenDerived>& thenMatrix, typename ThenDerived::Scalar elseScalar) const;
+
+    template<typename ElseDerived>
+    inline const Select<Derived, NestByValue<typename ElseDerived::ConstantReturnType>, ElseDerived >
+    select(typename ElseDerived::Scalar thenScalar, const MatrixBase<ElseDerived>& elseMatrix) const;
+
+    template<int p> RealScalar lpNorm() const;
+
+/////////// LU module ///////////
+
+    const LU<PlainMatrixType> lu() const;
+    const PlainMatrixType inverse() const;
+    void computeInverse(PlainMatrixType *result) const;
+    Scalar determinant() const;
+
+/////////// Cholesky module ///////////
+
+    const LLT<PlainMatrixType>  llt() const;
+    const LDLT<PlainMatrixType> ldlt() const;
+
+/////////// QR module ///////////
+
+    const QR<PlainMatrixType> qr() const;
+
+    EigenvaluesReturnType eigenvalues() const;
+    RealScalar operatorNorm() const;
+
+/////////// SVD module ///////////
+
+    SVD<PlainMatrixType> svd() const;
+
+/////////// Geometry module ///////////
+
+    template<typename OtherDerived>
+    PlainMatrixType cross(const MatrixBase<OtherDerived>& other) const;
+    PlainMatrixType unitOrthogonal(void) const;
+    Matrix<Scalar,3,1> eulerAngles(int a0, int a1, int a2) const;
+
+/////////// Sparse module ///////////
+
+    // dense = spasre * dense
+    template<typename Derived1, typename Derived2>
+    Derived& lazyAssign(const SparseProduct<Derived1,Derived2,SparseTimeDenseProduct>& product);
+    // dense = dense * spasre
+    template<typename Derived1, typename Derived2>
+    Derived& lazyAssign(const SparseProduct<Derived1,Derived2,DenseTimeSparseProduct>& product);
+
+    #ifdef EIGEN_MATRIXBASE_PLUGIN
+    #include EIGEN_MATRIXBASE_PLUGIN
+    #endif
+};
+
+#endif // EIGEN_MATRIXBASE_H
diff --git a/extern/Eigen2/Eigen/src/Core/MatrixStorage.h b/extern/Eigen2/Eigen/src/Core/MatrixStorage.h
new file mode 100644
index 00000000000..ba2355b8e60
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/MatrixStorage.h
@@ -0,0 +1,249 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MATRIXSTORAGE_H
+#define EIGEN_MATRIXSTORAGE_H
+
+struct ei_constructor_without_unaligned_array_assert {};
+
+/** \internal
+  * Static array automatically aligned if the total byte size is a multiple of 16 and the matrix options require auto alignment
+  */
+template <typename T, int Size, int MatrixOptions,
+          bool Align = (MatrixOptions&AutoAlign) && (((Size*sizeof(T))&0xf)==0)
+> struct ei_matrix_array
+{
+  EIGEN_ALIGN_128 T array[Size];
+
+  ei_matrix_array()
+  {
+    #ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+    ei_assert((reinterpret_cast<size_t>(array) & 0xf) == 0
+              && "this assertion is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html  **** READ THIS WEB PAGE !!! ****");
+    #endif
+  }
+
+  ei_matrix_array(ei_constructor_without_unaligned_array_assert) {}
+};
+
+template <typename T, int Size, int MatrixOptions> struct ei_matrix_array<T,Size,MatrixOptions,false>
+{
+  T array[Size];
+  ei_matrix_array() {}
+  ei_matrix_array(ei_constructor_without_unaligned_array_assert) {}
+};
+
+/** \internal
+  *
+  * \class ei_matrix_storage
+  *
+  * \brief Stores the data of a matrix
+  *
+  * This class stores the data of fixed-size, dynamic-size or mixed matrices
+  * in a way as compact as possible.
+  *
+  * \sa Matrix
+  */
+template<typename T, int Size, int _Rows, int _Cols, int _Options> class ei_matrix_storage;
+
+// purely fixed-size matrix
+template<typename T, int Size, int _Rows, int _Cols, int _Options> class ei_matrix_storage
+{
+    ei_matrix_array<T,Size,_Options> m_data;
+  public:
+    inline explicit ei_matrix_storage() {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
+      : m_data(ei_constructor_without_unaligned_array_assert()) {}
+    inline ei_matrix_storage(int,int,int) {}
+    inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); }
+    inline static int rows(void) {return _Rows;}
+    inline static int cols(void) {return _Cols;}
+    inline void resize(int,int,int) {}
+    inline const T *data() const { return m_data.array; }
+    inline T *data() { return m_data.array; }
+};
+
+// dynamic-size matrix with fixed-size storage
+template<typename T, int Size, int _Options> class ei_matrix_storage<T, Size, Dynamic, Dynamic, _Options>
+{
+    ei_matrix_array<T,Size,_Options> m_data;
+    int m_rows;
+    int m_cols;
+  public:
+    inline explicit ei_matrix_storage() : m_rows(0), m_cols(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
+      : m_data(ei_constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {}
+    inline ei_matrix_storage(int, int rows, int cols) : m_rows(rows), m_cols(cols) {}
+    inline ~ei_matrix_storage() {}
+    inline void swap(ei_matrix_storage& other)
+    { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
+    inline int rows(void) const {return m_rows;}
+    inline int cols(void) const {return m_cols;}
+    inline void resize(int, int rows, int cols)
+    {
+      m_rows = rows;
+      m_cols = cols;
+    }
+    inline const T *data() const { return m_data.array; }
+    inline T *data() { return m_data.array; }
+};
+
+// dynamic-size matrix with fixed-size storage and fixed width
+template<typename T, int Size, int _Cols, int _Options> class ei_matrix_storage<T, Size, Dynamic, _Cols, _Options>
+{
+    ei_matrix_array<T,Size,_Options> m_data;
+    int m_rows;
+  public:
+    inline explicit ei_matrix_storage() : m_rows(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
+      : m_data(ei_constructor_without_unaligned_array_assert()), m_rows(0) {}
+    inline ei_matrix_storage(int, int rows, int) : m_rows(rows) {}
+    inline ~ei_matrix_storage() {}
+    inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    inline int rows(void) const {return m_rows;}
+    inline int cols(void) const {return _Cols;}
+    inline void resize(int /*size*/, int rows, int)
+    {
+      m_rows = rows;
+    }
+    inline const T *data() const { return m_data.array; }
+    inline T *data() { return m_data.array; }
+};
+
+// dynamic-size matrix with fixed-size storage and fixed height
+template<typename T, int Size, int _Rows, int _Options> class ei_matrix_storage<T, Size, _Rows, Dynamic, _Options>
+{
+    ei_matrix_array<T,Size,_Options> m_data;
+    int m_cols;
+  public:
+    inline explicit ei_matrix_storage() : m_cols(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
+      : m_data(ei_constructor_without_unaligned_array_assert()), m_cols(0) {}
+    inline ei_matrix_storage(int, int, int cols) : m_cols(cols) {}
+    inline ~ei_matrix_storage() {}
+    inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    inline int rows(void) const {return _Rows;}
+    inline int cols(void) const {return m_cols;}
+    inline void resize(int, int, int cols)
+    {
+      m_cols = cols;
+    }
+    inline const T *data() const { return m_data.array; }
+    inline T *data() { return m_data.array; }
+};
+
+// purely dynamic matrix.
+template<typename T, int _Options> class ei_matrix_storage<T, Dynamic, Dynamic, Dynamic, _Options>
+{
+    T *m_data;
+    int m_rows;
+    int m_cols;
+  public:
+    inline explicit ei_matrix_storage() : m_data(0), m_rows(0), m_cols(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
+       : m_data(0), m_rows(0), m_cols(0) {}
+    inline ei_matrix_storage(int size, int rows, int cols)
+      : m_data(ei_aligned_new<T>(size)), m_rows(rows), m_cols(cols) {}
+    inline ~ei_matrix_storage() { ei_aligned_delete(m_data, m_rows*m_cols); }
+    inline void swap(ei_matrix_storage& other)
+    { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
+    inline int rows(void) const {return m_rows;}
+    inline int cols(void) const {return m_cols;}
+    void resize(int size, int rows, int cols)
+    {
+      if(size != m_rows*m_cols)
+      {
+        ei_aligned_delete(m_data, m_rows*m_cols);
+        if (size)
+          m_data = ei_aligned_new<T>(size);
+        else
+          m_data = 0;
+      }
+      m_rows = rows;
+      m_cols = cols;
+    }
+    inline const T *data() const { return m_data; }
+    inline T *data() { return m_data; }
+};
+
+// matrix with dynamic width and fixed height (so that matrix has dynamic size).
+template<typename T, int _Rows, int _Options> class ei_matrix_storage<T, Dynamic, _Rows, Dynamic, _Options>
+{
+    T *m_data;
+    int m_cols;
+  public:
+    inline explicit ei_matrix_storage() : m_data(0), m_cols(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
+    inline ei_matrix_storage(int size, int, int cols) : m_data(ei_aligned_new<T>(size)), m_cols(cols) {}
+    inline ~ei_matrix_storage() { ei_aligned_delete(m_data, _Rows*m_cols); }
+    inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    inline static int rows(void) {return _Rows;}
+    inline int cols(void) const {return m_cols;}
+    void resize(int size, int, int cols)
+    {
+      if(size != _Rows*m_cols)
+      {
+        ei_aligned_delete(m_data, _Rows*m_cols);
+        if (size)
+          m_data = ei_aligned_new<T>(size);
+        else
+          m_data = 0;
+      }
+      m_cols = cols;
+    }
+    inline const T *data() const { return m_data; }
+    inline T *data() { return m_data; }
+};
+
+// matrix with dynamic height and fixed width (so that matrix has dynamic size).
+template<typename T, int _Cols, int _Options> class ei_matrix_storage<T, Dynamic, Dynamic, _Cols, _Options>
+{
+    T *m_data;
+    int m_rows;
+  public:
+    inline explicit ei_matrix_storage() : m_data(0), m_rows(0) {}
+    inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
+    inline ei_matrix_storage(int size, int rows, int) : m_data(ei_aligned_new<T>(size)), m_rows(rows) {}
+    inline ~ei_matrix_storage() { ei_aligned_delete(m_data, _Cols*m_rows); }
+    inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    inline int rows(void) const {return m_rows;}
+    inline static int cols(void) {return _Cols;}
+    void resize(int size, int rows, int)
+    {
+      if(size != m_rows*_Cols)
+      {
+        ei_aligned_delete(m_data, _Cols*m_rows);
+        if (size)
+          m_data = ei_aligned_new<T>(size);
+        else
+          m_data = 0;
+      }
+      m_rows = rows;
+    }
+    inline const T *data() const { return m_data; }
+    inline T *data() { return m_data; }
+};
+
+#endif // EIGEN_MATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Core/Minor.h b/extern/Eigen2/Eigen/src/Core/Minor.h
new file mode 100644
index 00000000000..e2d47da79c2
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Minor.h
@@ -0,0 +1,122 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MINOR_H
+#define EIGEN_MINOR_H
+
+/** \nonstableyet 
+  * \class Minor
+  *
+  * \brief Expression of a minor
+  *
+  * \param MatrixType the type of the object in which we are taking a minor
+  *
+  * This class represents an expression of a minor. It is the return
+  * type of MatrixBase::minor() and most of the time this is the only way it
+  * is used.
+  *
+  * \sa MatrixBase::minor()
+  */
+template<typename MatrixType>
+struct ei_traits<Minor<MatrixType> >
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    RowsAtCompileTime = (MatrixType::RowsAtCompileTime != Dynamic) ?
+                          int(MatrixType::RowsAtCompileTime) - 1 : Dynamic,
+    ColsAtCompileTime = (MatrixType::ColsAtCompileTime != Dynamic) ?
+                          int(MatrixType::ColsAtCompileTime) - 1 : Dynamic,
+    MaxRowsAtCompileTime = (MatrixType::MaxRowsAtCompileTime != Dynamic) ?
+                             int(MatrixType::MaxRowsAtCompileTime) - 1 : Dynamic,
+    MaxColsAtCompileTime = (MatrixType::MaxColsAtCompileTime != Dynamic) ?
+                             int(MatrixType::MaxColsAtCompileTime) - 1 : Dynamic,
+    Flags = _MatrixTypeNested::Flags & HereditaryBits,
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost
+  };
+};
+
+template<typename MatrixType> class Minor
+  : public MatrixBase<Minor<MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Minor)
+
+    inline Minor(const MatrixType& matrix,
+                       int row, int col)
+      : m_matrix(matrix), m_row(row), m_col(col)
+    {
+      ei_assert(row >= 0 && row < matrix.rows()
+          && col >= 0 && col < matrix.cols());
+    }
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Minor)
+
+    inline int rows() const { return m_matrix.rows() - 1; }
+    inline int cols() const { return m_matrix.cols() - 1; }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived().coeffRef(row + (row >= m_row), col + (col >= m_col));
+    }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + (row >= m_row), col + (col >= m_col));
+    }
+
+  protected:
+    const typename MatrixType::Nested m_matrix;
+    const int m_row, m_col;
+};
+
+/** \nonstableyet 
+  * \return an expression of the (\a row, \a col)-minor of *this,
+  * i.e. an expression constructed from *this by removing the specified
+  * row and column.
+  *
+  * Example: \include MatrixBase_minor.cpp
+  * Output: \verbinclude MatrixBase_minor.out
+  *
+  * \sa class Minor
+  */
+template<typename Derived>
+inline Minor<Derived>
+MatrixBase<Derived>::minor(int row, int col)
+{
+  return Minor<Derived>(derived(), row, col);
+}
+
+/** \nonstableyet 
+  * This is the const version of minor(). */
+template<typename Derived>
+inline const Minor<Derived>
+MatrixBase<Derived>::minor(int row, int col) const
+{
+  return Minor<Derived>(derived(), row, col);
+}
+
+#endif // EIGEN_MINOR_H
diff --git a/extern/Eigen2/Eigen/src/Core/NestByValue.h b/extern/Eigen2/Eigen/src/Core/NestByValue.h
new file mode 100644
index 00000000000..da79315bffe
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/NestByValue.h
@@ -0,0 +1,114 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_NESTBYVALUE_H
+#define EIGEN_NESTBYVALUE_H
+
+/** \class NestByValue
+  *
+  * \brief Expression which must be nested by value
+  *
+  * \param ExpressionType the type of the object of which we are requiring nesting-by-value
+  *
+  * This class is the return type of MatrixBase::nestByValue()
+  * and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::nestByValue()
+  */
+template<typename ExpressionType>
+struct ei_traits<NestByValue<ExpressionType> > : public ei_traits<ExpressionType>
+{};
+
+template<typename ExpressionType> class NestByValue
+  : public MatrixBase<NestByValue<ExpressionType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(NestByValue)
+
+    inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {}
+
+    inline int rows() const { return m_expression.rows(); }
+    inline int cols() const { return m_expression.cols(); }
+    inline int stride() const { return m_expression.stride(); }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_expression.coeff(row, col);
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_expression.const_cast_derived().coeffRef(row, col);
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_expression.coeff(index);
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_expression.const_cast_derived().coeffRef(index);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int row, int col) const
+    {
+      return m_expression.template packet<LoadMode>(row, col);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int row, int col, const PacketScalar& x)
+    {
+      m_expression.const_cast_derived().template writePacket<LoadMode>(row, col, x);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int index) const
+    {
+      return m_expression.template packet<LoadMode>(index);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int index, const PacketScalar& x)
+    {
+      m_expression.const_cast_derived().template writePacket<LoadMode>(index, x);
+    }
+
+  protected:
+    const ExpressionType m_expression;
+};
+
+/** \returns an expression of the temporary version of *this.
+  */
+template<typename Derived>
+inline const NestByValue<Derived>
+MatrixBase<Derived>::nestByValue() const
+{
+  return NestByValue<Derived>(derived());
+}
+
+#endif // EIGEN_NESTBYVALUE_H
diff --git a/extern/Eigen2/Eigen/src/Core/NumTraits.h b/extern/Eigen2/Eigen/src/Core/NumTraits.h
new file mode 100644
index 00000000000..b27284a78bc
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/NumTraits.h
@@ -0,0 +1,142 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_NUMTRAITS_H
+#define EIGEN_NUMTRAITS_H
+
+/** \class NumTraits
+  *
+  * \brief Holds some data about the various numeric (i.e. scalar) types allowed by Eigen.
+  *
+  * \param T the numeric type about which this class provides data. Recall that Eigen allows
+  *          only the following types for \a T: \c int, \c float, \c double,
+  *          \c std::complex<float>, \c std::complex<double>, and \c long \c double (especially
+  *          useful to enforce x87 arithmetics when SSE is the default).
+  *
+  * The provided data consists of:
+  * \li A typedef \a Real, giving the "real part" type of \a T. If \a T is already real,
+  *     then \a Real is just a typedef to \a T. If \a T is \c std::complex<U> then \a Real
+  *     is a typedef to \a U.
+  * \li A typedef \a FloatingPoint, giving the "floating-point type" of \a T. If \a T is
+  *     \c int, then \a FloatingPoint is a typedef to \c double. Otherwise, \a FloatingPoint
+  *     is a typedef to \a T.
+  * \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c std::complex
+  *     type, and to 0 otherwise.
+  * \li An enum \a HasFloatingPoint. It is equal to \c 0 if \a T is \c int,
+  *     and to \c 1 otherwise.
+  */
+template<typename T> struct NumTraits;
+
+template<> struct NumTraits<int>
+{
+  typedef int Real;
+  typedef double FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 0,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+template<> struct NumTraits<float>
+{
+  typedef float Real;
+  typedef float FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 1,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+template<> struct NumTraits<double>
+{
+  typedef double Real;
+  typedef double FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 1,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+template<typename _Real> struct NumTraits<std::complex<_Real> >
+{
+  typedef _Real Real;
+  typedef std::complex<_Real> FloatingPoint;
+  enum {
+    IsComplex = 1,
+    HasFloatingPoint = NumTraits<Real>::HasFloatingPoint,
+    ReadCost = 2,
+    AddCost = 2 * NumTraits<Real>::AddCost,
+    MulCost = 4 * NumTraits<Real>::MulCost + 2 * NumTraits<Real>::AddCost
+  };
+};
+
+template<> struct NumTraits<long long int>
+{
+  typedef long long int Real;
+  typedef long double FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 0,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+template<> struct NumTraits<long double>
+{
+  typedef long double Real;
+  typedef long double FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 1,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+template<> struct NumTraits<bool>
+{
+  typedef bool Real;
+  typedef float FloatingPoint;
+  enum {
+    IsComplex = 0,
+    HasFloatingPoint = 0,
+    ReadCost = 1,
+    AddCost = 1,
+    MulCost = 1
+  };
+};
+
+#endif // EIGEN_NUMTRAITS_H
diff --git a/extern/Eigen2/Eigen/src/Core/Part.h b/extern/Eigen2/Eigen/src/Core/Part.h
new file mode 100644
index 00000000000..9c273f249ec
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Part.h
@@ -0,0 +1,375 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PART_H
+#define EIGEN_PART_H
+
+/** \nonstableyet
+  * \class Part
+  *
+  * \brief Expression of a triangular matrix extracted from a given matrix
+  *
+  * \param MatrixType the type of the object in which we are taking the triangular part
+  * \param Mode the kind of triangular matrix expression to construct. Can be UpperTriangular, StrictlyUpperTriangular,
+  *             UnitUpperTriangular, LowerTriangular, StrictlyLowerTriangular, UnitLowerTriangular. This is in fact a bit field; it must have either
+  *             UpperTriangularBit or LowerTriangularBit, and additionnaly it may have either ZeroDiagBit or
+  *             UnitDiagBit.
+  *
+  * This class represents an expression of the upper or lower triangular part of
+  * a square matrix, possibly with a further assumption on the diagonal. It is the return type
+  * of MatrixBase::part() and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::part()
+  */
+template<typename MatrixType, unsigned int Mode>
+struct ei_traits<Part<MatrixType, Mode> > : ei_traits<MatrixType>
+{
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    Flags = (_MatrixTypeNested::Flags & (HereditaryBits) & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit))) | Mode,
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost
+  };
+};
+
+template<typename MatrixType, unsigned int Mode> class Part
+  : public MatrixBase<Part<MatrixType, Mode> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Part)
+
+    inline Part(const MatrixType& matrix) : m_matrix(matrix)
+    { ei_assert(ei_are_flags_consistent<Mode>::ret); }
+
+    /** \sa MatrixBase::operator+=() */
+    template<typename Other> Part&  operator+=(const Other& other);
+    /** \sa MatrixBase::operator-=() */
+    template<typename Other> Part&  operator-=(const Other& other);
+    /** \sa MatrixBase::operator*=() */
+    Part&  operator*=(const typename ei_traits<MatrixType>::Scalar& other);
+    /** \sa MatrixBase::operator/=() */
+    Part&  operator/=(const typename ei_traits<MatrixType>::Scalar& other);
+
+    /** \sa operator=(), MatrixBase::lazyAssign() */
+    template<typename Other> void lazyAssign(const Other& other);
+    /** \sa MatrixBase::operator=() */
+    template<typename Other> Part& operator=(const Other& other);
+
+    inline int rows() const { return m_matrix.rows(); }
+    inline int cols() const { return m_matrix.cols(); }
+    inline int stride() const { return m_matrix.stride(); }
+
+    inline Scalar coeff(int row, int col) const
+    {
+      // SelfAdjointBit doesn't play any role here: just because a matrix is selfadjoint doesn't say anything about
+      // each individual coefficient, except for the not-very-useful-here fact that diagonal coefficients are real.
+      if( ((Flags & LowerTriangularBit) && (col>row)) || ((Flags & UpperTriangularBit) && (row>col)) )
+        return (Scalar)0;
+      if(Flags & UnitDiagBit)
+        return col==row ? (Scalar)1 : m_matrix.coeff(row, col);
+      else if(Flags & ZeroDiagBit)
+        return col==row ? (Scalar)0 : m_matrix.coeff(row, col);
+      else
+        return m_matrix.coeff(row, col);
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      EIGEN_STATIC_ASSERT(!(Flags & UnitDiagBit), WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED)
+      EIGEN_STATIC_ASSERT(!(Flags & SelfAdjointBit), COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED)
+      ei_assert(   (Mode==UpperTriangular && col>=row)
+                || (Mode==LowerTriangular && col<=row)
+                || (Mode==StrictlyUpperTriangular && col>row)
+                || (Mode==StrictlyLowerTriangular && col<row));
+      return m_matrix.const_cast_derived().coeffRef(row, col);
+    }
+
+    /** \internal */
+    const MatrixType& _expression() const { return m_matrix; }
+
+    /** discard any writes to a row */
+    const Block<Part, 1, ColsAtCompileTime> row(int i) { return Base::row(i); }
+    const Block<Part, 1, ColsAtCompileTime> row(int i) const { return Base::row(i); }
+    /** discard any writes to a column */
+    const Block<Part, RowsAtCompileTime, 1> col(int i) { return Base::col(i); }
+    const Block<Part, RowsAtCompileTime, 1> col(int i) const { return Base::col(i); }
+
+    template<typename OtherDerived>
+    void swap(const MatrixBase<OtherDerived>& other)
+    {
+      Part<SwapWrapper<MatrixType>,Mode>(const_cast<MatrixType&>(m_matrix)).lazyAssign(other.derived());
+    }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+};
+
+/** \nonstableyet
+  * \returns an expression of a triangular matrix extracted from the current matrix
+  *
+  * The parameter \a Mode can have the following values: \c UpperTriangular, \c StrictlyUpperTriangular, \c UnitUpperTriangular,
+  * \c LowerTriangular, \c StrictlyLowerTriangular, \c UnitLowerTriangular.
+  *
+  * \addexample PartExample \label How to extract a triangular part of an arbitrary matrix
+  *
+  * Example: \include MatrixBase_extract.cpp
+  * Output: \verbinclude MatrixBase_extract.out
+  *
+  * \sa class Part, part(), marked()
+  */
+template<typename Derived>
+template<unsigned int Mode>
+const Part<Derived, Mode> MatrixBase<Derived>::part() const
+{
+  return derived();
+}
+
+template<typename MatrixType, unsigned int Mode>
+template<typename Other>
+inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator=(const Other& other)
+{
+  if(Other::Flags & EvalBeforeAssigningBit)
+  {
+    typename MatrixBase<Other>::PlainMatrixType other_evaluated(other.rows(), other.cols());
+    other_evaluated.template part<Mode>().lazyAssign(other);
+    lazyAssign(other_evaluated);
+  }
+  else
+    lazyAssign(other.derived());
+  return *this;
+}
+
+template<typename Derived1, typename Derived2, unsigned int Mode, int UnrollCount>
+struct ei_part_assignment_impl
+{
+  enum {
+    col = (UnrollCount-1) / Derived1::RowsAtCompileTime,
+    row = (UnrollCount-1) % Derived1::RowsAtCompileTime
+  };
+
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    ei_part_assignment_impl<Derived1, Derived2, Mode, UnrollCount-1>::run(dst, src);
+
+    if(Mode == SelfAdjoint)
+    {
+      if(row == col)
+        dst.coeffRef(row, col) = ei_real(src.coeff(row, col));
+      else if(row < col)
+        dst.coeffRef(col, row) = ei_conj(dst.coeffRef(row, col) = src.coeff(row, col));
+    }
+    else
+    {
+      ei_assert(Mode == UpperTriangular || Mode == LowerTriangular || Mode == StrictlyUpperTriangular || Mode == StrictlyLowerTriangular);
+      if((Mode == UpperTriangular && row <= col)
+      || (Mode == LowerTriangular && row >= col)
+      || (Mode == StrictlyUpperTriangular && row < col)
+      || (Mode == StrictlyLowerTriangular && row > col))
+        dst.copyCoeff(row, col, src);
+    }
+  }
+};
+
+template<typename Derived1, typename Derived2, unsigned int Mode>
+struct ei_part_assignment_impl<Derived1, Derived2, Mode, 1>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    if(!(Mode & ZeroDiagBit))
+      dst.copyCoeff(0, 0, src);
+  }
+};
+
+// prevent buggy user code from causing an infinite recursion
+template<typename Derived1, typename Derived2, unsigned int Mode>
+struct ei_part_assignment_impl<Derived1, Derived2, Mode, 0>
+{
+  inline static void run(Derived1 &, const Derived2 &) {}
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_part_assignment_impl<Derived1, Derived2, UpperTriangular, Dynamic>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    for(int j = 0; j < dst.cols(); ++j)
+      for(int i = 0; i <= j; ++i)
+        dst.copyCoeff(i, j, src);
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_part_assignment_impl<Derived1, Derived2, LowerTriangular, Dynamic>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    for(int j = 0; j < dst.cols(); ++j)
+      for(int i = j; i < dst.rows(); ++i)
+        dst.copyCoeff(i, j, src);
+  }
+};
+
+template<typename Derived1, typename Derived2>
+struct ei_part_assignment_impl<Derived1, Derived2, StrictlyUpperTriangular, Dynamic>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    for(int j = 0; j < dst.cols(); ++j)
+      for(int i = 0; i < j; ++i)
+        dst.copyCoeff(i, j, src);
+  }
+};
+template<typename Derived1, typename Derived2>
+struct ei_part_assignment_impl<Derived1, Derived2, StrictlyLowerTriangular, Dynamic>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    for(int j = 0; j < dst.cols(); ++j)
+      for(int i = j+1; i < dst.rows(); ++i)
+        dst.copyCoeff(i, j, src);
+  }
+};
+template<typename Derived1, typename Derived2>
+struct ei_part_assignment_impl<Derived1, Derived2, SelfAdjoint, Dynamic>
+{
+  inline static void run(Derived1 &dst, const Derived2 &src)
+  {
+    for(int j = 0; j < dst.cols(); ++j)
+    {
+      for(int i = 0; i < j; ++i)
+        dst.coeffRef(j, i) = ei_conj(dst.coeffRef(i, j) = src.coeff(i, j));
+      dst.coeffRef(j, j) = ei_real(src.coeff(j, j));
+    }
+  }
+};
+
+template<typename MatrixType, unsigned int Mode>
+template<typename Other>
+void Part<MatrixType, Mode>::lazyAssign(const Other& other)
+{
+  const bool unroll = MatrixType::SizeAtCompileTime * Other::CoeffReadCost / 2 <= EIGEN_UNROLLING_LIMIT;
+  ei_assert(m_matrix.rows() == other.rows() && m_matrix.cols() == other.cols());
+
+  ei_part_assignment_impl
+    <MatrixType, Other, Mode,
+    unroll ? int(MatrixType::SizeAtCompileTime) : Dynamic
+    >::run(m_matrix.const_cast_derived(), other.derived());
+}
+
+/** \nonstableyet
+  * \returns a lvalue pseudo-expression allowing to perform special operations on \c *this.
+  *
+  * The \a Mode parameter can have the following values: \c UpperTriangular, \c StrictlyUpperTriangular, \c LowerTriangular,
+  * \c StrictlyLowerTriangular, \c SelfAdjoint.
+  *
+  * \addexample PartExample \label How to write to a triangular part of a matrix
+  *
+  * Example: \include MatrixBase_part.cpp
+  * Output: \verbinclude MatrixBase_part.out
+  *
+  * \sa class Part, MatrixBase::extract(), MatrixBase::marked()
+  */
+template<typename Derived>
+template<unsigned int Mode>
+inline Part<Derived, Mode> MatrixBase<Derived>::part()
+{
+  return Part<Derived, Mode>(derived());
+}
+
+/** \returns true if *this is approximately equal to an upper triangular matrix,
+  *          within the precision given by \a prec.
+  *
+  * \sa isLowerTriangular(), extract(), part(), marked()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isUpperTriangular(RealScalar prec) const
+{
+  if(cols() != rows()) return false;
+  RealScalar maxAbsOnUpperTriangularPart = static_cast<RealScalar>(-1);
+  for(int j = 0; j < cols(); ++j)
+    for(int i = 0; i <= j; ++i)
+    {
+      RealScalar absValue = ei_abs(coeff(i,j));
+      if(absValue > maxAbsOnUpperTriangularPart) maxAbsOnUpperTriangularPart = absValue;
+    }
+  for(int j = 0; j < cols()-1; ++j)
+    for(int i = j+1; i < rows(); ++i)
+      if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnUpperTriangularPart, prec)) return false;
+  return true;
+}
+
+/** \returns true if *this is approximately equal to a lower triangular matrix,
+  *          within the precision given by \a prec.
+  *
+  * \sa isUpperTriangular(), extract(), part(), marked()
+  */
+template<typename Derived>
+bool MatrixBase<Derived>::isLowerTriangular(RealScalar prec) const
+{
+  if(cols() != rows()) return false;
+  RealScalar maxAbsOnLowerTriangularPart = static_cast<RealScalar>(-1);
+  for(int j = 0; j < cols(); ++j)
+    for(int i = j; i < rows(); ++i)
+    {
+      RealScalar absValue = ei_abs(coeff(i,j));
+      if(absValue > maxAbsOnLowerTriangularPart) maxAbsOnLowerTriangularPart = absValue;
+    }
+  for(int j = 1; j < cols(); ++j)
+    for(int i = 0; i < j; ++i)
+      if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnLowerTriangularPart, prec)) return false;
+  return true;
+}
+
+template<typename MatrixType, unsigned int Mode>
+template<typename Other>
+inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator+=(const Other& other)
+{
+  return *this = m_matrix + other;
+}
+
+template<typename MatrixType, unsigned int Mode>
+template<typename Other>
+inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator-=(const Other& other)
+{
+  return *this = m_matrix - other;
+}
+
+template<typename MatrixType, unsigned int Mode>
+inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator*=
+(const typename ei_traits<MatrixType>::Scalar& other)
+{
+  return *this = m_matrix * other;
+}
+
+template<typename MatrixType, unsigned int Mode>
+inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator/=
+(const typename ei_traits<MatrixType>::Scalar& other)
+{
+  return *this = m_matrix / other;
+}
+
+#endif // EIGEN_PART_H
diff --git a/extern/Eigen2/Eigen/src/Core/Product.h b/extern/Eigen2/Eigen/src/Core/Product.h
new file mode 100644
index 00000000000..1151b21641c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Product.h
@@ -0,0 +1,769 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PRODUCT_H
+#define EIGEN_PRODUCT_H
+
+/***************************
+*** Forward declarations ***
+***************************/
+
+template<int VectorizationMode, int Index, typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl;
+
+template<int StorageOrder, int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl;
+
+/** \class ProductReturnType
+  *
+  * \brief Helper class to get the correct and optimized returned type of operator*
+  *
+  * \param Lhs the type of the left-hand side
+  * \param Rhs the type of the right-hand side
+  * \param ProductMode the type of the product (determined automatically by ei_product_mode)
+  *
+  * This class defines the typename Type representing the optimized product expression
+  * between two matrix expressions. In practice, using ProductReturnType<Lhs,Rhs>::Type
+  * is the recommended way to define the result type of a function returning an expression
+  * which involve a matrix product. The class Product or DiagonalProduct should never be
+  * used directly.
+  *
+  * \sa class Product, class DiagonalProduct, MatrixBase::operator*(const MatrixBase<OtherDerived>&)
+  */
+template<typename Lhs, typename Rhs, int ProductMode>
+struct ProductReturnType
+{
+  typedef typename ei_nested<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
+  typedef typename ei_nested<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+
+  typedef Product<LhsNested, RhsNested, ProductMode> Type;
+};
+
+// cache friendly specialization
+// note that there is a DiagonalProduct specialization in DiagonalProduct.h
+template<typename Lhs, typename Rhs>
+struct ProductReturnType<Lhs,Rhs,CacheFriendlyProduct>
+{
+  typedef typename ei_nested<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
+
+  typedef typename ei_nested<Rhs,Lhs::RowsAtCompileTime,
+                             typename ei_plain_matrix_type_column_major<Rhs>::type
+                   >::type RhsNested;
+
+  typedef Product<LhsNested, RhsNested, CacheFriendlyProduct> Type;
+};
+
+/*  Helper class to determine the type of the product, can be either:
+ *    - NormalProduct
+ *    - CacheFriendlyProduct
+ *    - DiagonalProduct
+ */
+template<typename Lhs, typename Rhs> struct ei_product_mode
+{
+  enum{
+
+    value = ((Rhs::Flags&Diagonal)==Diagonal) || ((Lhs::Flags&Diagonal)==Diagonal)
+          ? DiagonalProduct
+          : Lhs::MaxColsAtCompileTime == Dynamic
+            && ( Lhs::MaxRowsAtCompileTime == Dynamic
+              || Rhs::MaxColsAtCompileTime == Dynamic )
+            && (!(Rhs::IsVectorAtCompileTime && (Lhs::Flags&RowMajorBit)  && (!(Lhs::Flags&DirectAccessBit))))
+            && (!(Lhs::IsVectorAtCompileTime && (!(Rhs::Flags&RowMajorBit)) && (!(Rhs::Flags&DirectAccessBit))))
+            && (ei_is_same_type<typename Lhs::Scalar, typename Rhs::Scalar>::ret)
+          ? CacheFriendlyProduct
+          : NormalProduct };
+};
+
+/** \class Product
+  *
+  * \brief Expression of the product of two matrices
+  *
+  * \param LhsNested the type used to store the left-hand side
+  * \param RhsNested the type used to store the right-hand side
+  * \param ProductMode the type of the product
+  *
+  * This class represents an expression of the product of two matrices.
+  * It is the return type of the operator* between matrices. Its template
+  * arguments are determined automatically by ProductReturnType. Therefore,
+  * Product should never be used direclty. To determine the result type of a
+  * function which involves a matrix product, use ProductReturnType::Type.
+  *
+  * \sa ProductReturnType, MatrixBase::operator*(const MatrixBase<OtherDerived>&)
+  */
+template<typename LhsNested, typename RhsNested, int ProductMode>
+struct ei_traits<Product<LhsNested, RhsNested, ProductMode> >
+{
+  // clean the nested types:
+  typedef typename ei_cleantype<LhsNested>::type _LhsNested;
+  typedef typename ei_cleantype<RhsNested>::type _RhsNested;
+  typedef typename ei_scalar_product_traits<typename _LhsNested::Scalar, typename _RhsNested::Scalar>::ReturnType Scalar;
+
+  enum {
+    LhsCoeffReadCost = _LhsNested::CoeffReadCost,
+    RhsCoeffReadCost = _RhsNested::CoeffReadCost,
+    LhsFlags = _LhsNested::Flags,
+    RhsFlags = _RhsNested::Flags,
+
+    RowsAtCompileTime = _LhsNested::RowsAtCompileTime,
+    ColsAtCompileTime = _RhsNested::ColsAtCompileTime,
+    InnerSize = EIGEN_ENUM_MIN(_LhsNested::ColsAtCompileTime, _RhsNested::RowsAtCompileTime),
+
+    MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime,
+
+    LhsRowMajor = LhsFlags & RowMajorBit,
+    RhsRowMajor = RhsFlags & RowMajorBit,
+
+    CanVectorizeRhs = RhsRowMajor && (RhsFlags & PacketAccessBit)
+                    && (ColsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
+
+    CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit)
+                    && (RowsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
+
+    EvalToRowMajor = RhsRowMajor && (ProductMode==(int)CacheFriendlyProduct ? LhsRowMajor : (!CanVectorizeLhs)),
+
+    RemovedBits = ~(EvalToRowMajor ? 0 : RowMajorBit),
+
+    Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits)
+          | EvalBeforeAssigningBit
+          | EvalBeforeNestingBit
+          | (CanVectorizeLhs || CanVectorizeRhs ? PacketAccessBit : 0)
+          | (LhsFlags & RhsFlags & AlignedBit),
+
+    CoeffReadCost = InnerSize == Dynamic ? Dynamic
+                  : InnerSize * (NumTraits<Scalar>::MulCost + LhsCoeffReadCost + RhsCoeffReadCost)
+                    + (InnerSize - 1) * NumTraits<Scalar>::AddCost,
+
+    /* CanVectorizeInner deserves special explanation. It does not affect the product flags. It is not used outside
+     * of Product. If the Product itself is not a packet-access expression, there is still a chance that the inner
+     * loop of the product might be vectorized. This is the meaning of CanVectorizeInner. Since it doesn't affect
+     * the Flags, it is safe to make this value depend on ActualPacketAccessBit, that doesn't affect the ABI.
+     */
+    CanVectorizeInner = LhsRowMajor && (!RhsRowMajor) && (LhsFlags & RhsFlags & ActualPacketAccessBit)
+                      && (InnerSize % ei_packet_traits<Scalar>::size == 0)
+  };
+};
+
+template<typename LhsNested, typename RhsNested, int ProductMode> class Product : ei_no_assignment_operator,
+  public MatrixBase<Product<LhsNested, RhsNested, ProductMode> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
+
+  private:
+
+    typedef typename ei_traits<Product>::_LhsNested _LhsNested;
+    typedef typename ei_traits<Product>::_RhsNested _RhsNested;
+
+    enum {
+      PacketSize = ei_packet_traits<Scalar>::size,
+      InnerSize  = ei_traits<Product>::InnerSize,
+      Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT,
+      CanVectorizeInner = ei_traits<Product>::CanVectorizeInner
+    };
+
+    typedef ei_product_coeff_impl<CanVectorizeInner ? InnerVectorization : NoVectorization,
+                                  Unroll ? InnerSize-1 : Dynamic,
+                                  _LhsNested, _RhsNested, Scalar> ScalarCoeffImpl;
+
+  public:
+
+    template<typename Lhs, typename Rhs>
+    inline Product(const Lhs& lhs, const Rhs& rhs)
+      : m_lhs(lhs), m_rhs(rhs)
+    {
+      // we don't allow taking products of matrices of different real types, as that wouldn't be vectorizable.
+      // We still allow to mix T and complex<T>.
+      EIGEN_STATIC_ASSERT((ei_is_same_type<typename Lhs::RealScalar, typename Rhs::RealScalar>::ret),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+      ei_assert(lhs.cols() == rhs.rows()
+        && "invalid matrix product"
+        && "if you wanted a coeff-wise or a dot product use the respective explicit functions");
+    }
+
+    /** \internal
+      * compute \a res += \c *this using the cache friendly product.
+      */
+    template<typename DestDerived>
+    void _cacheFriendlyEvalAndAdd(DestDerived& res) const;
+
+    /** \internal
+      * \returns whether it is worth it to use the cache friendly product.
+      */
+    EIGEN_STRONG_INLINE bool _useCacheFriendlyProduct() const
+    {
+      return  m_lhs.cols()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+              && (  rows()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+                 || cols()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD);
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_rhs.cols(); }
+
+    EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
+    {
+      Scalar res;
+      ScalarCoeffImpl::run(row, col, m_lhs, m_rhs, res);
+      return res;
+    }
+
+    /* Allow index-based non-packet access. It is impossible though to allow index-based packed access,
+     * which is why we don't set the LinearAccessBit.
+     */
+    EIGEN_STRONG_INLINE const Scalar coeff(int index) const
+    {
+      Scalar res;
+      const int row = RowsAtCompileTime == 1 ? 0 : index;
+      const int col = RowsAtCompileTime == 1 ? index : 0;
+      ScalarCoeffImpl::run(row, col, m_lhs, m_rhs, res);
+      return res;
+    }
+
+    template<int LoadMode>
+    EIGEN_STRONG_INLINE const PacketScalar packet(int row, int col) const
+    {
+      PacketScalar res;
+      ei_product_packet_impl<Flags&RowMajorBit ? RowMajor : ColMajor,
+                                   Unroll ? InnerSize-1 : Dynamic,
+                                   _LhsNested, _RhsNested, PacketScalar, LoadMode>
+        ::run(row, col, m_lhs, m_rhs, res);
+      return res;
+    }
+
+    EIGEN_STRONG_INLINE const _LhsNested& lhs() const { return m_lhs; }
+    EIGEN_STRONG_INLINE const _RhsNested& rhs() const { return m_rhs; }
+
+  protected:
+    const LhsNested m_lhs;
+    const RhsNested m_rhs;
+};
+
+/** \returns the matrix product of \c *this and \a other.
+  *
+  * \note If instead of the matrix product you want the coefficient-wise product, see Cwise::operator*().
+  *
+  * \sa lazy(), operator*=(const MatrixBase&), Cwise::operator*()
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline const typename ProductReturnType<Derived,OtherDerived>::Type
+MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
+{
+  enum {
+    ProductIsValid =  Derived::ColsAtCompileTime==Dynamic
+                   || OtherDerived::RowsAtCompileTime==Dynamic
+                   || int(Derived::ColsAtCompileTime)==int(OtherDerived::RowsAtCompileTime),
+    AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime,
+    SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived,OtherDerived)
+  };
+  // note to the lost user:
+  //    * for a dot product use: v1.dot(v2)
+  //    * for a coeff-wise product use: v1.cwise()*v2
+  EIGEN_STATIC_ASSERT(ProductIsValid || !(AreVectors && SameSizes),
+    INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
+  EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors),
+    INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
+  EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT)
+  return typename ProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
+}
+
+/** replaces \c *this by \c *this * \a other.
+  *
+  * \returns a reference to \c *this
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline Derived &
+MatrixBase<Derived>::operator*=(const MatrixBase<OtherDerived> &other)
+{
+  return derived() = derived() * other.derived();
+}
+
+/***************************************************************************
+* Normal product .coeff() implementation (with meta-unrolling)
+***************************************************************************/
+
+/**************************************
+*** Scalar path  - no vectorization ***
+**************************************/
+
+template<int Index, typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<NoVectorization, Index, Lhs, Rhs, RetScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
+  {
+    ei_product_coeff_impl<NoVectorization, Index-1, Lhs, Rhs, RetScalar>::run(row, col, lhs, rhs, res);
+    res += lhs.coeff(row, Index) * rhs.coeff(Index, col);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<NoVectorization, 0, Lhs, Rhs, RetScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
+  {
+    res = lhs.coeff(row, 0) * rhs.coeff(0, col);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<NoVectorization, Dynamic, Lhs, Rhs, RetScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar& res)
+  {
+    ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
+    res = lhs.coeff(row, 0) * rhs.coeff(0, col);
+      for(int i = 1; i < lhs.cols(); ++i)
+        res += lhs.coeff(row, i) * rhs.coeff(i, col);
+  }
+};
+
+// prevent buggy user code from causing an infinite recursion
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<NoVectorization, -1, Lhs, Rhs, RetScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int, int, const Lhs&, const Rhs&, RetScalar&) {}
+};
+
+/*******************************************
+*** Scalar path with inner vectorization ***
+*******************************************/
+
+template<int Index, typename Lhs, typename Rhs, typename PacketScalar>
+struct ei_product_coeff_vectorized_unroller
+{
+  enum { PacketSize = ei_packet_traits<typename Lhs::Scalar>::size };
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::PacketScalar &pres)
+  {
+    ei_product_coeff_vectorized_unroller<Index-PacketSize, Lhs, Rhs, PacketScalar>::run(row, col, lhs, rhs, pres);
+    pres = ei_padd(pres, ei_pmul( lhs.template packet<Aligned>(row, Index) , rhs.template packet<Aligned>(Index, col) ));
+  }
+};
+
+template<typename Lhs, typename Rhs, typename PacketScalar>
+struct ei_product_coeff_vectorized_unroller<0, Lhs, Rhs, PacketScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::PacketScalar &pres)
+  {
+    pres = ei_pmul(lhs.template packet<Aligned>(row, 0) , rhs.template packet<Aligned>(0, col));
+  }
+};
+
+template<int Index, typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<InnerVectorization, Index, Lhs, Rhs, RetScalar>
+{
+  typedef typename Lhs::PacketScalar PacketScalar;
+  enum { PacketSize = ei_packet_traits<typename Lhs::Scalar>::size };
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
+  {
+    PacketScalar pres;
+    ei_product_coeff_vectorized_unroller<Index+1-PacketSize, Lhs, Rhs, PacketScalar>::run(row, col, lhs, rhs, pres);
+    ei_product_coeff_impl<NoVectorization,Index,Lhs,Rhs,RetScalar>::run(row, col, lhs, rhs, res);
+    res = ei_predux(pres);
+  }
+};
+
+template<typename Lhs, typename Rhs, int LhsRows = Lhs::RowsAtCompileTime, int RhsCols = Rhs::ColsAtCompileTime>
+struct ei_product_coeff_vectorized_dyn_selector
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
+  {
+    res = ei_dot_impl<
+      Block<Lhs, 1, ei_traits<Lhs>::ColsAtCompileTime>,
+      Block<Rhs, ei_traits<Rhs>::RowsAtCompileTime, 1>,
+      LinearVectorization, NoUnrolling>::run(lhs.row(row), rhs.col(col));
+  }
+};
+
+// NOTE the 3 following specializations are because taking .col(0) on a vector is a bit slower
+// NOTE maybe they are now useless since we have a specialization for Block<Matrix>
+template<typename Lhs, typename Rhs, int RhsCols>
+struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,1,RhsCols>
+{
+  EIGEN_STRONG_INLINE static void run(int /*row*/, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
+  {
+    res = ei_dot_impl<
+      Lhs,
+      Block<Rhs, ei_traits<Rhs>::RowsAtCompileTime, 1>,
+      LinearVectorization, NoUnrolling>::run(lhs, rhs.col(col));
+  }
+};
+
+template<typename Lhs, typename Rhs, int LhsRows>
+struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,LhsRows,1>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int /*col*/, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
+  {
+    res = ei_dot_impl<
+      Block<Lhs, 1, ei_traits<Lhs>::ColsAtCompileTime>,
+      Rhs,
+      LinearVectorization, NoUnrolling>::run(lhs.row(row), rhs);
+  }
+};
+
+template<typename Lhs, typename Rhs>
+struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,1,1>
+{
+  EIGEN_STRONG_INLINE static void run(int /*row*/, int /*col*/, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
+  {
+    res = ei_dot_impl<
+      Lhs,
+      Rhs,
+      LinearVectorization, NoUnrolling>::run(lhs, rhs);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct ei_product_coeff_impl<InnerVectorization, Dynamic, Lhs, Rhs, RetScalar>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
+  {
+    ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs>::run(row, col, lhs, rhs, res);
+  }
+};
+
+/*******************
+*** Packet path  ***
+*******************/
+
+template<int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<RowMajor, Index, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
+  {
+    ei_product_packet_impl<RowMajor, Index-1, Lhs, Rhs, PacketScalar, LoadMode>::run(row, col, lhs, rhs, res);
+    res =  ei_pmadd(ei_pset1(lhs.coeff(row, Index)), rhs.template packet<LoadMode>(Index, col), res);
+  }
+};
+
+template<int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<ColMajor, Index, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
+  {
+    ei_product_packet_impl<ColMajor, Index-1, Lhs, Rhs, PacketScalar, LoadMode>::run(row, col, lhs, rhs, res);
+    res =  ei_pmadd(lhs.template packet<LoadMode>(row, Index), ei_pset1(rhs.coeff(Index, col)), res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<RowMajor, 0, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
+  {
+    res = ei_pmul(ei_pset1(lhs.coeff(row, 0)),rhs.template packet<LoadMode>(0, col));
+  }
+};
+
+template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<ColMajor, 0, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
+  {
+    res = ei_pmul(lhs.template packet<LoadMode>(row, 0), ei_pset1(rhs.coeff(0, col)));
+  }
+};
+
+template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar& res)
+  {
+    ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
+    res = ei_pmul(ei_pset1(lhs.coeff(row, 0)),rhs.template packet<LoadMode>(0, col));
+      for(int i = 1; i < lhs.cols(); ++i)
+        res =  ei_pmadd(ei_pset1(lhs.coeff(row, i)), rhs.template packet<LoadMode>(i, col), res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
+struct ei_product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, PacketScalar, LoadMode>
+{
+  EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar& res)
+  {
+    ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
+    res = ei_pmul(lhs.template packet<LoadMode>(row, 0), ei_pset1(rhs.coeff(0, col)));
+      for(int i = 1; i < lhs.cols(); ++i)
+        res =  ei_pmadd(lhs.template packet<LoadMode>(row, i), ei_pset1(rhs.coeff(i, col)), res);
+  }
+};
+
+/***************************************************************************
+* Cache friendly product callers and specific nested evaluation strategies
+***************************************************************************/
+
+template<typename Scalar, typename RhsType>
+static void ei_cache_friendly_product_colmajor_times_vector(
+  int size, const Scalar* lhs, int lhsStride, const RhsType& rhs, Scalar* res);
+
+template<typename Scalar, typename ResType>
+static void ei_cache_friendly_product_rowmajor_times_vector(
+  const Scalar* lhs, int lhsStride, const Scalar* rhs, int rhsSize, ResType& res);
+
+template<typename ProductType,
+  int LhsRows  = ei_traits<ProductType>::RowsAtCompileTime,
+  int LhsOrder = int(ei_traits<ProductType>::LhsFlags)&RowMajorBit ? RowMajor : ColMajor,
+  int LhsHasDirectAccess = int(ei_traits<ProductType>::LhsFlags)&DirectAccessBit? HasDirectAccess : NoDirectAccess,
+  int RhsCols  = ei_traits<ProductType>::ColsAtCompileTime,
+  int RhsOrder = int(ei_traits<ProductType>::RhsFlags)&RowMajorBit ? RowMajor : ColMajor,
+  int RhsHasDirectAccess = int(ei_traits<ProductType>::RhsFlags)&DirectAccessBit? HasDirectAccess : NoDirectAccess>
+struct ei_cache_friendly_product_selector
+{
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    product._cacheFriendlyEvalAndAdd(res);
+  }
+};
+
+// optimized colmajor * vector path
+template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
+struct ei_cache_friendly_product_selector<ProductType,LhsRows,ColMajor,NoDirectAccess,1,RhsOrder,RhsAccess>
+{
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    const int size = product.rhs().rows();
+    for (int k=0; k<size; ++k)
+        res += product.rhs().coeff(k) * product.lhs().col(k);
+  }
+};
+
+// optimized cache friendly colmajor * vector path for matrix with direct access flag
+// NOTE this path could also be enabled for expressions if we add runtime align queries
+template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
+struct ei_cache_friendly_product_selector<ProductType,LhsRows,ColMajor,HasDirectAccess,1,RhsOrder,RhsAccess>
+{
+  typedef typename ProductType::Scalar Scalar;
+
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    enum {
+      EvalToRes = (ei_packet_traits<Scalar>::size==1)
+                ||((DestDerived::Flags&ActualPacketAccessBit) && (!(DestDerived::Flags & RowMajorBit))) };
+    Scalar* EIGEN_RESTRICT _res;
+    if (EvalToRes)
+       _res = &res.coeffRef(0);
+    else
+    {
+      _res = ei_aligned_stack_new(Scalar,res.size());
+      Map<Matrix<Scalar,DestDerived::RowsAtCompileTime,1> >(_res, res.size()) = res;
+    }
+    ei_cache_friendly_product_colmajor_times_vector(res.size(),
+      &product.lhs().const_cast_derived().coeffRef(0,0), product.lhs().stride(),
+      product.rhs(), _res);
+
+    if (!EvalToRes)
+    {
+      res = Map<Matrix<Scalar,DestDerived::SizeAtCompileTime,1> >(_res, res.size());
+      ei_aligned_stack_delete(Scalar, _res, res.size());
+    }
+  }
+};
+
+// optimized vector * rowmajor path
+template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
+struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,RowMajor,NoDirectAccess>
+{
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    const int cols = product.lhs().cols();
+    for (int j=0; j<cols; ++j)
+      res += product.lhs().coeff(j) * product.rhs().row(j);
+  }
+};
+
+// optimized cache friendly vector * rowmajor path for matrix with direct access flag
+// NOTE this path coul also be enabled for expressions if we add runtime align queries
+template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
+struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,RowMajor,HasDirectAccess>
+{
+  typedef typename ProductType::Scalar Scalar;
+
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    enum {
+      EvalToRes = (ei_packet_traits<Scalar>::size==1)
+                ||((DestDerived::Flags & ActualPacketAccessBit) && (DestDerived::Flags & RowMajorBit)) };
+    Scalar* EIGEN_RESTRICT _res;
+    if (EvalToRes)
+       _res = &res.coeffRef(0);
+    else
+    {
+      _res = ei_aligned_stack_new(Scalar, res.size());
+      Map<Matrix<Scalar,DestDerived::SizeAtCompileTime,1> >(_res, res.size()) = res;
+    }
+    ei_cache_friendly_product_colmajor_times_vector(res.size(),
+      &product.rhs().const_cast_derived().coeffRef(0,0), product.rhs().stride(),
+      product.lhs().transpose(), _res);
+
+    if (!EvalToRes)
+    {
+      res = Map<Matrix<Scalar,DestDerived::SizeAtCompileTime,1> >(_res, res.size());
+      ei_aligned_stack_delete(Scalar, _res, res.size());
+    }
+  }
+};
+
+// optimized rowmajor - vector product
+template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
+struct ei_cache_friendly_product_selector<ProductType,LhsRows,RowMajor,HasDirectAccess,1,RhsOrder,RhsAccess>
+{
+  typedef typename ProductType::Scalar Scalar;
+  typedef typename ei_traits<ProductType>::_RhsNested Rhs;
+  enum {
+      UseRhsDirectly = ((ei_packet_traits<Scalar>::size==1) || (Rhs::Flags&ActualPacketAccessBit))
+                     && (!(Rhs::Flags & RowMajorBit)) };
+
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    Scalar* EIGEN_RESTRICT _rhs;
+    if (UseRhsDirectly)
+       _rhs = &product.rhs().const_cast_derived().coeffRef(0);
+    else
+    {
+      _rhs = ei_aligned_stack_new(Scalar, product.rhs().size());
+      Map<Matrix<Scalar,Rhs::SizeAtCompileTime,1> >(_rhs, product.rhs().size()) = product.rhs();
+    }
+    ei_cache_friendly_product_rowmajor_times_vector(&product.lhs().const_cast_derived().coeffRef(0,0), product.lhs().stride(),
+                                                    _rhs, product.rhs().size(), res);
+
+    if (!UseRhsDirectly) ei_aligned_stack_delete(Scalar, _rhs, product.rhs().size());
+  }
+};
+
+// optimized vector - colmajor product
+template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
+struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,ColMajor,HasDirectAccess>
+{
+  typedef typename ProductType::Scalar Scalar;
+  typedef typename ei_traits<ProductType>::_LhsNested Lhs;
+  enum {
+      UseLhsDirectly = ((ei_packet_traits<Scalar>::size==1) || (Lhs::Flags&ActualPacketAccessBit))
+                     && (Lhs::Flags & RowMajorBit) };
+
+  template<typename DestDerived>
+  inline static void run(DestDerived& res, const ProductType& product)
+  {
+    Scalar* EIGEN_RESTRICT _lhs;
+    if (UseLhsDirectly)
+       _lhs = &product.lhs().const_cast_derived().coeffRef(0);
+    else
+    {
+      _lhs = ei_aligned_stack_new(Scalar, product.lhs().size());
+      Map<Matrix<Scalar,Lhs::SizeAtCompileTime,1> >(_lhs, product.lhs().size()) = product.lhs();
+    }
+    ei_cache_friendly_product_rowmajor_times_vector(&product.rhs().const_cast_derived().coeffRef(0,0), product.rhs().stride(),
+                                                    _lhs, product.lhs().size(), res);
+
+    if(!UseLhsDirectly) ei_aligned_stack_delete(Scalar, _lhs, product.lhs().size());
+  }
+};
+
+// discard this case which has to be handled by the default path
+// (we keep it to be sure to hit a compilation error if this is not the case)
+template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
+struct ei_cache_friendly_product_selector<ProductType,LhsRows,RowMajor,NoDirectAccess,1,RhsOrder,RhsAccess>
+{};
+
+// discard this case which has to be handled by the default path
+// (we keep it to be sure to hit a compilation error if this is not the case)
+template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
+struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,ColMajor,NoDirectAccess>
+{};
+
+
+/** \internal */
+template<typename Derived>
+template<typename Lhs,typename Rhs>
+inline Derived&
+MatrixBase<Derived>::operator+=(const Flagged<Product<Lhs,Rhs,CacheFriendlyProduct>, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other)
+{
+  if (other._expression()._useCacheFriendlyProduct())
+    ei_cache_friendly_product_selector<Product<Lhs,Rhs,CacheFriendlyProduct> >::run(const_cast_derived(), other._expression());
+  else
+    lazyAssign(derived() + other._expression());
+  return derived();
+}
+
+template<typename Derived>
+template<typename Lhs, typename Rhs>
+inline Derived& MatrixBase<Derived>::lazyAssign(const Product<Lhs,Rhs,CacheFriendlyProduct>& product)
+{
+  if (product._useCacheFriendlyProduct())
+  {
+    setZero();
+    ei_cache_friendly_product_selector<Product<Lhs,Rhs,CacheFriendlyProduct> >::run(const_cast_derived(), product);
+  }
+  else
+  {
+    lazyAssign<Product<Lhs,Rhs,CacheFriendlyProduct> >(product);
+  }
+  return derived();
+}
+
+template<typename T> struct ei_product_copy_rhs
+{
+  typedef typename ei_meta_if<
+         (ei_traits<T>::Flags & RowMajorBit)
+      || (!(ei_traits<T>::Flags & DirectAccessBit)),
+      typename ei_plain_matrix_type_column_major<T>::type,
+      const T&
+    >::ret type;
+};
+
+template<typename T> struct ei_product_copy_lhs
+{
+  typedef typename ei_meta_if<
+      (!(int(ei_traits<T>::Flags) & DirectAccessBit)),
+      typename ei_plain_matrix_type<T>::type,
+      const T&
+    >::ret type;
+};
+
+template<typename Lhs, typename Rhs, int ProductMode>
+template<typename DestDerived>
+inline void Product<Lhs,Rhs,ProductMode>::_cacheFriendlyEvalAndAdd(DestDerived& res) const
+{
+  typedef typename ei_product_copy_lhs<_LhsNested>::type LhsCopy;
+  typedef typename ei_unref<LhsCopy>::type _LhsCopy;
+  typedef typename ei_product_copy_rhs<_RhsNested>::type RhsCopy;
+  typedef typename ei_unref<RhsCopy>::type _RhsCopy;
+  LhsCopy lhs(m_lhs);
+  RhsCopy rhs(m_rhs);
+  ei_cache_friendly_product<Scalar>(
+    rows(), cols(), lhs.cols(),
+    _LhsCopy::Flags&RowMajorBit, (const Scalar*)&(lhs.const_cast_derived().coeffRef(0,0)), lhs.stride(),
+    _RhsCopy::Flags&RowMajorBit, (const Scalar*)&(rhs.const_cast_derived().coeffRef(0,0)), rhs.stride(),
+    DestDerived::Flags&RowMajorBit, (Scalar*)&(res.coeffRef(0,0)), res.stride()
+  );
+}
+
+#endif // EIGEN_PRODUCT_H
diff --git a/extern/Eigen2/Eigen/src/Core/Redux.h b/extern/Eigen2/Eigen/src/Core/Redux.h
new file mode 100644
index 00000000000..734ef1929a4
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Redux.h
@@ -0,0 +1,117 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_REDUX_H
+#define EIGEN_REDUX_H
+
+template<typename BinaryOp, typename Derived, int Start, int Length>
+struct ei_redux_impl
+{
+  enum {
+    HalfLength = Length/2
+  };
+
+  typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
+
+  static Scalar run(const Derived &mat, const BinaryOp& func)
+  {
+    return func(
+      ei_redux_impl<BinaryOp, Derived, Start, HalfLength>::run(mat, func),
+      ei_redux_impl<BinaryOp, Derived, Start+HalfLength, Length - HalfLength>::run(mat, func));
+  }
+};
+
+template<typename BinaryOp, typename Derived, int Start>
+struct ei_redux_impl<BinaryOp, Derived, Start, 1>
+{
+  enum {
+    col = Start / Derived::RowsAtCompileTime,
+    row = Start % Derived::RowsAtCompileTime
+  };
+
+  typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
+
+  static Scalar run(const Derived &mat, const BinaryOp &)
+  {
+    return mat.coeff(row, col);
+  }
+};
+
+template<typename BinaryOp, typename Derived, int Start>
+struct ei_redux_impl<BinaryOp, Derived, Start, Dynamic>
+{
+  typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
+  static Scalar run(const Derived& mat, const BinaryOp& func)
+  {
+    ei_assert(mat.rows()>0 && mat.cols()>0 && "you are using a non initialized matrix");
+    Scalar res;
+    res = mat.coeff(0,0);
+    for(int i = 1; i < mat.rows(); ++i)
+      res = func(res, mat.coeff(i, 0));
+    for(int j = 1; j < mat.cols(); ++j)
+      for(int i = 0; i < mat.rows(); ++i)
+        res = func(res, mat.coeff(i, j));
+    return res;
+  }
+};
+
+/** \returns the result of a full redux operation on the whole matrix or vector using \a func
+  *
+  * The template parameter \a BinaryOp is the type of the functor \a func which must be
+  * an assiociative operator. Both current STL and TR1 functor styles are handled.
+  *
+  * \sa MatrixBase::sum(), MatrixBase::minCoeff(), MatrixBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise()
+  */
+template<typename Derived>
+template<typename BinaryOp>
+typename ei_result_of<BinaryOp(typename ei_traits<Derived>::Scalar)>::type
+MatrixBase<Derived>::redux(const BinaryOp& func) const
+{
+  const bool unroll = SizeAtCompileTime * CoeffReadCost
+                    + (SizeAtCompileTime-1) * ei_functor_traits<BinaryOp>::Cost
+                    <= EIGEN_UNROLLING_LIMIT;
+  return ei_redux_impl<BinaryOp, Derived, 0, unroll ? int(SizeAtCompileTime) : Dynamic>
+            ::run(derived(), func);
+}
+
+/** \returns the minimum of all coefficients of *this
+  */
+template<typename Derived>
+inline typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::minCoeff() const
+{
+  return this->redux(Eigen::ei_scalar_min_op<Scalar>());
+}
+
+/** \returns the maximum of all coefficients of *this
+  */
+template<typename Derived>
+inline typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::maxCoeff() const
+{
+  return this->redux(Eigen::ei_scalar_max_op<Scalar>());
+}
+
+#endif // EIGEN_REDUX_H
diff --git a/extern/Eigen2/Eigen/src/Core/SolveTriangular.h b/extern/Eigen2/Eigen/src/Core/SolveTriangular.h
new file mode 100644
index 00000000000..12fb0e1d159
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/SolveTriangular.h
@@ -0,0 +1,297 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SOLVETRIANGULAR_H
+#define EIGEN_SOLVETRIANGULAR_H
+
+template<typename XprType> struct ei_is_part { enum {value=false}; };
+template<typename XprType, unsigned int Mode> struct ei_is_part<Part<XprType,Mode> > { enum {value=true}; };
+
+template<typename Lhs, typename Rhs,
+  int TriangularPart = (int(Lhs::Flags) & LowerTriangularBit)
+                     ? LowerTriangular
+                     : (int(Lhs::Flags) & UpperTriangularBit)
+                     ? UpperTriangular
+                     : -1,
+  int StorageOrder = ei_is_part<Lhs>::value ? -1  // this is to solve ambiguous specializations
+                   : int(Lhs::Flags) & (RowMajorBit|SparseBit)
+  >
+struct ei_solve_triangular_selector;
+
+// transform a Part xpr to a Flagged xpr
+template<typename Lhs, unsigned int LhsMode, typename Rhs, int UpLo, int StorageOrder>
+struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,UpLo,StorageOrder>
+{
+  static void run(const Part<Lhs,LhsMode>& lhs, Rhs& other)
+  {
+    ei_solve_triangular_selector<Flagged<Lhs,LhsMode,0>,Rhs>::run(lhs._expression(), other);
+  }
+};
+
+// forward substitution, row-major
+template<typename Lhs, typename Rhs, int UpLo>
+struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,RowMajor|IsDense>
+{
+  typedef typename Rhs::Scalar Scalar;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    const bool IsLowerTriangular = (UpLo==LowerTriangular);
+    const int size = lhs.cols();
+    /* We perform the inverse product per block of 4 rows such that we perfectly match
+     * our optimized matrix * vector product. blockyStart represents the number of rows
+     * we have process first using the non-block version.
+     */
+    int blockyStart = (std::max(size-5,0)/4)*4;
+    if (IsLowerTriangular)
+      blockyStart = size - blockyStart;
+    else
+      blockyStart -= 1;
+    for(int c=0 ; c<other.cols() ; ++c)
+    {
+      // process first rows using the non block version
+      if(!(Lhs::Flags & UnitDiagBit))
+      {
+        if (IsLowerTriangular)
+          other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0);
+        else
+          other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1);
+      }
+      for(int i=(IsLowerTriangular ? 1 : size-2); IsLowerTriangular ? i<blockyStart : i>blockyStart; i += (IsLowerTriangular ? 1 : -1) )
+      {
+        Scalar tmp = other.coeff(i,c)
+          - (IsLowerTriangular ? ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0)
+                     : ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0));
+        if (Lhs::Flags & UnitDiagBit)
+          other.coeffRef(i,c) = tmp;
+        else
+          other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
+      }
+
+      // now let's process the remaining rows 4 at once
+      for(int i=blockyStart; IsLowerTriangular ? i<size : i>0; )
+      {
+        int startBlock = i;
+        int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
+
+        /* Process the i cols times 4 rows block, and keep the result in a temporary vector */
+        // FIXME use fixed size block but take care to small fixed size matrices...
+        Matrix<Scalar,Dynamic,1> btmp(4);
+        if (IsLowerTriangular)
+          btmp = lhs.block(startBlock,0,4,i) * other.col(c).start(i);
+        else
+          btmp = lhs.block(i-3,i+1,4,size-1-i) * other.col(c).end(size-1-i);
+
+        /* Let's process the 4x4 sub-matrix as usual.
+         * btmp stores the diagonal coefficients used to update the remaining part of the result.
+         */
+        {
+          Scalar tmp = other.coeff(startBlock,c)-btmp.coeff(IsLowerTriangular?0:3);
+          if (Lhs::Flags & UnitDiagBit)
+            other.coeffRef(i,c) = tmp;
+          else
+            other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
+        }
+
+        i += IsLowerTriangular ? 1 : -1;
+        for (;IsLowerTriangular ? i<endBlock : i>endBlock; i += IsLowerTriangular ? 1 : -1)
+        {
+          int remainingSize = IsLowerTriangular ? i-startBlock : startBlock-i;
+          Scalar tmp = other.coeff(i,c)
+            - btmp.coeff(IsLowerTriangular ? remainingSize : 3-remainingSize)
+            - (   lhs.row(i).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)
+              * other.col(c).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)).coeff(0,0);
+
+          if (Lhs::Flags & UnitDiagBit)
+            other.coeffRef(i,c) = tmp;
+          else
+            other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
+        }
+      }
+    }
+  }
+};
+
+// Implements the following configurations:
+//  - inv(LowerTriangular,         ColMajor) * Column vector
+//  - inv(LowerTriangular,UnitDiag,ColMajor) * Column vector
+//  - inv(UpperTriangular,         ColMajor) * Column vector
+//  - inv(UpperTriangular,UnitDiag,ColMajor) * Column vector
+template<typename Lhs, typename Rhs, int UpLo>
+struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,ColMajor|IsDense>
+{
+  typedef typename Rhs::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type Packet;
+  enum { PacketSize =  ei_packet_traits<Scalar>::size };
+
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    static const bool IsLowerTriangular = (UpLo==LowerTriangular);
+    const int size = lhs.cols();
+    for(int c=0 ; c<other.cols() ; ++c)
+    {
+      /* let's perform the inverse product per block of 4 columns such that we perfectly match
+       * our optimized matrix * vector product. blockyEnd represents the number of rows
+       * we can process using the block version.
+       */
+      int blockyEnd = (std::max(size-5,0)/4)*4;
+      if (!IsLowerTriangular)
+        blockyEnd = size-1 - blockyEnd;
+      for(int i=IsLowerTriangular ? 0 : size-1; IsLowerTriangular ? i<blockyEnd : i>blockyEnd;)
+      {
+        /* Let's process the 4x4 sub-matrix as usual.
+         * btmp stores the diagonal coefficients used to update the remaining part of the result.
+         */
+        int startBlock = i;
+        int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
+        Matrix<Scalar,4,1> btmp;
+        for (;IsLowerTriangular ? i<endBlock : i>endBlock;
+             i += IsLowerTriangular ? 1 : -1)
+        {
+          if(!(Lhs::Flags & UnitDiagBit))
+            other.coeffRef(i,c) /= lhs.coeff(i,i);
+          int remainingSize = IsLowerTriangular ? endBlock-i-1 : i-endBlock-1;
+          if (remainingSize>0)
+            other.col(c).segment((IsLowerTriangular ? i : endBlock) + 1, remainingSize) -=
+                other.coeffRef(i,c)
+              * Block<Lhs,Dynamic,1>(lhs, (IsLowerTriangular ? i : endBlock) + 1, i, remainingSize, 1);
+          btmp.coeffRef(IsLowerTriangular ? i-startBlock : remainingSize) = -other.coeffRef(i,c);
+        }
+
+        /* Now we can efficiently update the remaining part of the result as a matrix * vector product.
+         * NOTE in order to reduce both compilation time and binary size, let's directly call
+         * the fast product implementation. It is equivalent to the following code:
+         *   other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
+         *                                       * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
+         */
+        // FIXME this is cool but what about conjugate/adjoint expressions ? do we want to evaluate them ?
+        // this is a more general problem though.
+        ei_cache_friendly_product_colmajor_times_vector(
+          IsLowerTriangular ? size-endBlock : endBlock+1,
+          &(lhs.const_cast_derived().coeffRef(IsLowerTriangular ? endBlock : 0, IsLowerTriangular ? startBlock : endBlock+1)),
+          lhs.stride(),
+          btmp, &(other.coeffRef(IsLowerTriangular ? endBlock : 0, c)));
+// 				if (IsLowerTriangular)
+//           other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
+//                                           * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
+// 				else
+//           other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
+//                                           * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
+      }
+
+      /* Now we have to process the remaining part as usual */
+      int i;
+      for(i=blockyEnd; IsLowerTriangular ? i<size-1 : i>0; i += (IsLowerTriangular ? 1 : -1) )
+      {
+        if(!(Lhs::Flags & UnitDiagBit))
+          other.coeffRef(i,c) /= lhs.coeff(i,i);
+
+        /* NOTE we cannot use lhs.col(i).end(size-i-1) because Part::coeffRef gets called by .col() to
+         * get the address of the start of the row
+         */
+        if(IsLowerTriangular)
+          other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1,i, size-i-1,1);
+        else
+          other.col(c).start(i) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, 0,i, i, 1);
+      }
+      if(!(Lhs::Flags & UnitDiagBit))
+        other.coeffRef(i,c) /= lhs.coeff(i,i);
+    }
+  }
+};
+
+/** "in-place" version of MatrixBase::solveTriangular() where the result is written in \a other
+  *
+  * \nonstableyet
+  *
+  * The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
+  * This function will const_cast it, so constness isn't honored here.
+  *
+  * See MatrixBase:solveTriangular() for the details.
+  */
+template<typename Derived>
+template<typename OtherDerived>
+void MatrixBase<Derived>::solveTriangularInPlace(const MatrixBase<OtherDerived>& _other) const
+{
+  MatrixBase<OtherDerived>& other = _other.const_cast_derived();
+  ei_assert(derived().cols() == derived().rows());
+  ei_assert(derived().cols() == other.rows());
+  ei_assert(!(Flags & ZeroDiagBit));
+  ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit));
+
+  enum { copy = ei_traits<OtherDerived>::Flags & RowMajorBit };
+
+  typedef typename ei_meta_if<copy,
+    typename ei_plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::ret OtherCopy;
+  OtherCopy otherCopy(other.derived());
+
+  ei_solve_triangular_selector<Derived, typename ei_unref<OtherCopy>::type>::run(derived(), otherCopy);
+
+  if (copy)
+    other = otherCopy;
+}
+
+/** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
+  *
+  * \nonstableyet
+  *
+  * This function computes the inverse-matrix matrix product inverse(\c *this) * \a other.
+  * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
+  * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
+  * is an upper (resp. lower) triangular matrix.
+  *
+  * It is required that \c *this be marked as either an upper or a lower triangular matrix, which
+  * can be done by marked(), and that is automatically the case with expressions such as those returned
+  * by extract().
+  *
+  * \addexample SolveTriangular \label How to solve a triangular system (aka. how to multiply the inverse of a triangular matrix by another one)
+  *
+  * Example: \include MatrixBase_marked.cpp
+  * Output: \verbinclude MatrixBase_marked.out
+  *
+  * This function is essentially a wrapper to the faster solveTriangularInPlace() function creating
+  * a temporary copy of \a other, calling solveTriangularInPlace() on the copy and returning it.
+  * Therefore, if \a other is not needed anymore, it is quite faster to call solveTriangularInPlace()
+  * instead of solveTriangular().
+  *
+  * For users coming from BLAS, this function (and more specifically solveTriangularInPlace()) offer
+  * all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
+  *
+  * \b Tips: to perform a \em "right-inverse-multiply" you can simply transpose the operation, e.g.:
+  * \code
+  * M * T^1  <=>  T.transpose().solveTriangularInPlace(M.transpose());
+  * \endcode
+  *
+  * \sa solveTriangularInPlace(), marked(), extract()
+  */
+template<typename Derived>
+template<typename OtherDerived>
+typename ei_plain_matrix_type_column_major<OtherDerived>::type
+MatrixBase<Derived>::solveTriangular(const MatrixBase<OtherDerived>& other) const
+{
+  typename ei_plain_matrix_type_column_major<OtherDerived>::type res(other);
+  solveTriangularInPlace(res);
+  return res;
+}
+
+#endif // EIGEN_SOLVETRIANGULAR_H
diff --git a/extern/Eigen2/Eigen/src/Core/Sum.h b/extern/Eigen2/Eigen/src/Core/Sum.h
new file mode 100644
index 00000000000..6d7e9959fa5
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Sum.h
@@ -0,0 +1,271 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SUM_H
+#define EIGEN_SUM_H
+
+/***************************************************************************
+* Part 1 : the logic deciding a strategy for vectorization and unrolling
+***************************************************************************/
+
+template<typename Derived>
+struct ei_sum_traits
+{
+private:
+  enum {
+    PacketSize = ei_packet_traits<typename Derived::Scalar>::size
+  };
+
+public:
+  enum {
+    Vectorization = (int(Derived::Flags)&ActualPacketAccessBit)
+                 && (int(Derived::Flags)&LinearAccessBit)
+                  ? LinearVectorization
+                  : NoVectorization
+  };
+
+private:
+  enum {
+    Cost = Derived::SizeAtCompileTime * Derived::CoeffReadCost
+           + (Derived::SizeAtCompileTime-1) * NumTraits<typename Derived::Scalar>::AddCost,
+    UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize))
+  };
+
+public:
+  enum {
+    Unrolling = Cost <= UnrollingLimit
+              ? CompleteUnrolling
+              : NoUnrolling
+  };
+};
+
+/***************************************************************************
+* Part 2 : unrollers
+***************************************************************************/
+
+/*** no vectorization ***/
+
+template<typename Derived, int Start, int Length>
+struct ei_sum_novec_unroller
+{
+  enum {
+    HalfLength = Length/2
+  };
+
+  typedef typename Derived::Scalar Scalar;
+
+  inline static Scalar run(const Derived &mat)
+  {
+    return ei_sum_novec_unroller<Derived, Start, HalfLength>::run(mat)
+         + ei_sum_novec_unroller<Derived, Start+HalfLength, Length-HalfLength>::run(mat);
+  }
+};
+
+template<typename Derived, int Start>
+struct ei_sum_novec_unroller<Derived, Start, 1>
+{
+  enum {
+    col = Start / Derived::RowsAtCompileTime,
+    row = Start % Derived::RowsAtCompileTime
+  };
+
+  typedef typename Derived::Scalar Scalar;
+
+  inline static Scalar run(const Derived &mat)
+  {
+    return mat.coeff(row, col);
+  }
+};
+
+/*** vectorization ***/
+  
+template<typename Derived, int Start, int Length>
+struct ei_sum_vec_unroller
+{
+  enum {
+    PacketSize = ei_packet_traits<typename Derived::Scalar>::size,
+    HalfLength = Length/2
+  };
+
+  typedef typename Derived::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  inline static PacketScalar run(const Derived &mat)
+  {
+    return ei_padd(
+            ei_sum_vec_unroller<Derived, Start, HalfLength>::run(mat),
+            ei_sum_vec_unroller<Derived, Start+HalfLength, Length-HalfLength>::run(mat) );
+  }
+};
+
+template<typename Derived, int Start>
+struct ei_sum_vec_unroller<Derived, Start, 1>
+{
+  enum {
+    index = Start * ei_packet_traits<typename Derived::Scalar>::size,
+    row = int(Derived::Flags)&RowMajorBit
+        ? index / int(Derived::ColsAtCompileTime)
+        : index % Derived::RowsAtCompileTime,
+    col = int(Derived::Flags)&RowMajorBit
+        ? index % int(Derived::ColsAtCompileTime)
+        : index / Derived::RowsAtCompileTime,
+    alignment = (Derived::Flags & AlignedBit) ? Aligned : Unaligned
+  };
+
+  typedef typename Derived::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  inline static PacketScalar run(const Derived &mat)
+  {
+    return mat.template packet<alignment>(row, col);
+  }
+};
+
+/***************************************************************************
+* Part 3 : implementation of all cases
+***************************************************************************/
+
+template<typename Derived,
+         int Vectorization = ei_sum_traits<Derived>::Vectorization,
+         int Unrolling = ei_sum_traits<Derived>::Unrolling
+>
+struct ei_sum_impl;
+
+template<typename Derived>
+struct ei_sum_impl<Derived, NoVectorization, NoUnrolling>
+{
+  typedef typename Derived::Scalar Scalar;
+  static Scalar run(const Derived& mat)
+  {
+    ei_assert(mat.rows()>0 && mat.cols()>0 && "you are using a non initialized matrix");
+    Scalar res;
+    res = mat.coeff(0, 0);
+    for(int i = 1; i < mat.rows(); ++i)
+      res += mat.coeff(i, 0);
+    for(int j = 1; j < mat.cols(); ++j)
+      for(int i = 0; i < mat.rows(); ++i)
+        res += mat.coeff(i, j);
+    return res;
+  }
+};
+
+template<typename Derived>
+struct ei_sum_impl<Derived, NoVectorization, CompleteUnrolling>
+  : public ei_sum_novec_unroller<Derived, 0, Derived::SizeAtCompileTime>
+{};
+
+template<typename Derived>
+struct ei_sum_impl<Derived, LinearVectorization, NoUnrolling>
+{
+  typedef typename Derived::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+
+  static Scalar run(const Derived& mat)
+  {
+    const int size = mat.size();
+    const int packetSize = ei_packet_traits<Scalar>::size;
+    const int alignedStart =  (Derived::Flags & AlignedBit)
+                           || !(Derived::Flags & DirectAccessBit)
+                           ? 0
+                           : ei_alignmentOffset(&mat.const_cast_derived().coeffRef(0), size);
+    enum {
+      alignment = (Derived::Flags & DirectAccessBit) || (Derived::Flags & AlignedBit)
+                ? Aligned : Unaligned
+    };
+    const int alignedSize = ((size-alignedStart)/packetSize)*packetSize;
+    const int alignedEnd = alignedStart + alignedSize;
+    Scalar res;
+
+    if(alignedSize)
+    {
+      PacketScalar packet_res = mat.template packet<alignment>(alignedStart);
+      for(int index = alignedStart + packetSize; index < alignedEnd; index += packetSize)
+        packet_res = ei_padd(packet_res, mat.template packet<alignment>(index));
+      res = ei_predux(packet_res);
+    }
+    else // too small to vectorize anything.
+         // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
+    {
+      res = Scalar(0);
+    }
+
+    for(int index = 0; index < alignedStart; ++index)
+      res += mat.coeff(index);
+
+    for(int index = alignedEnd; index < size; ++index)
+      res += mat.coeff(index);
+
+    return res;
+  }
+};
+
+template<typename Derived>
+struct ei_sum_impl<Derived, LinearVectorization, CompleteUnrolling>
+{
+  typedef typename Derived::Scalar Scalar;
+  typedef typename ei_packet_traits<Scalar>::type PacketScalar;
+  enum {
+    PacketSize = ei_packet_traits<Scalar>::size,
+    Size = Derived::SizeAtCompileTime,
+    VectorizationSize = (Size / PacketSize) * PacketSize
+  };
+  static Scalar run(const Derived& mat)
+  {
+    Scalar res = ei_predux(ei_sum_vec_unroller<Derived, 0, Size / PacketSize>::run(mat));
+    if (VectorizationSize != Size)
+      res += ei_sum_novec_unroller<Derived, VectorizationSize, Size-VectorizationSize>::run(mat);
+    return res;
+  }
+};
+
+/***************************************************************************
+* Part 4 : implementation of MatrixBase methods
+***************************************************************************/
+
+/** \returns the sum of all coefficients of *this
+  *
+  * \sa trace()
+  */
+template<typename Derived>
+inline typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::sum() const
+{
+  return ei_sum_impl<Derived>::run(derived());
+}
+
+/** \returns the trace of \c *this, i.e. the sum of the coefficients on the main diagonal.
+  *
+  * \c *this can be any matrix, not necessarily square.
+  *
+  * \sa diagonal(), sum()
+  */
+template<typename Derived>
+inline typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::trace() const
+{
+  return diagonal().sum();
+}
+
+#endif // EIGEN_SUM_H
diff --git a/extern/Eigen2/Eigen/src/Core/Swap.h b/extern/Eigen2/Eigen/src/Core/Swap.h
new file mode 100644
index 00000000000..77d562cd3ac
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Swap.h
@@ -0,0 +1,142 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SWAP_H
+#define EIGEN_SWAP_H
+
+/** \class SwapWrapper
+  *
+  * \internal
+  *
+  * \brief Internal helper class for swapping two expressions
+  */
+template<typename ExpressionType>
+struct ei_traits<SwapWrapper<ExpressionType> >
+{
+  typedef typename ExpressionType::Scalar Scalar;
+  enum {
+    RowsAtCompileTime = ExpressionType::RowsAtCompileTime,
+    ColsAtCompileTime = ExpressionType::ColsAtCompileTime,
+    MaxRowsAtCompileTime = ExpressionType::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = ExpressionType::MaxColsAtCompileTime,
+    Flags = ExpressionType::Flags,
+    CoeffReadCost = ExpressionType::CoeffReadCost
+  };
+};
+
+template<typename ExpressionType> class SwapWrapper
+  : public MatrixBase<SwapWrapper<ExpressionType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(SwapWrapper)
+    typedef typename ei_packet_traits<Scalar>::type Packet;
+
+    inline SwapWrapper(ExpressionType& xpr) : m_expression(xpr) {}
+
+    inline int rows() const { return m_expression.rows(); }
+    inline int cols() const { return m_expression.cols(); }
+    inline int stride() const { return m_expression.stride(); }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_expression.const_cast_derived().coeffRef(row, col);
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_expression.const_cast_derived().coeffRef(index);
+    }
+
+    template<typename OtherDerived>
+    void copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other)
+    {
+      OtherDerived& _other = other.const_cast_derived();
+      ei_internal_assert(row >= 0 && row < rows()
+                         && col >= 0 && col < cols());
+      Scalar tmp = m_expression.coeff(row, col);
+      m_expression.coeffRef(row, col) = _other.coeff(row, col);
+      _other.coeffRef(row, col) = tmp;
+    }
+
+    template<typename OtherDerived>
+    void copyCoeff(int index, const MatrixBase<OtherDerived>& other)
+    {
+      OtherDerived& _other = other.const_cast_derived();
+      ei_internal_assert(index >= 0 && index < m_expression.size());
+      Scalar tmp = m_expression.coeff(index);
+      m_expression.coeffRef(index) = _other.coeff(index);
+      _other.coeffRef(index) = tmp;
+    }
+
+    template<typename OtherDerived, int StoreMode, int LoadMode>
+    void copyPacket(int row, int col, const MatrixBase<OtherDerived>& other)
+    {
+      OtherDerived& _other = other.const_cast_derived();
+      ei_internal_assert(row >= 0 && row < rows()
+                        && col >= 0 && col < cols());
+      Packet tmp = m_expression.template packet<StoreMode>(row, col);
+      m_expression.template writePacket<StoreMode>(row, col,
+        _other.template packet<LoadMode>(row, col)
+      );
+      _other.template writePacket<LoadMode>(row, col, tmp);
+    }
+
+    template<typename OtherDerived, int StoreMode, int LoadMode>
+    void copyPacket(int index, const MatrixBase<OtherDerived>& other)
+    {
+      OtherDerived& _other = other.const_cast_derived();
+      ei_internal_assert(index >= 0 && index < m_expression.size());
+      Packet tmp = m_expression.template packet<StoreMode>(index);
+      m_expression.template writePacket<StoreMode>(index,
+        _other.template packet<LoadMode>(index)
+      );
+      _other.template writePacket<LoadMode>(index, tmp);
+    }
+
+  protected:
+    ExpressionType& m_expression;
+};
+
+/** swaps *this with the expression \a other.
+  *
+  * \note \a other is only marked for internal reasons, but of course
+  * it gets const-casted. One reason is that one will often call swap
+  * on temporary objects (hence non-const references are forbidden).
+  * Another reason is that lazyAssign takes a const argument anyway.
+  */
+template<typename Derived>
+template<typename OtherDerived>
+void MatrixBase<Derived>::swap(const MatrixBase<OtherDerived>& other)
+{
+  (SwapWrapper<Derived>(derived())).lazyAssign(other);
+}
+
+#endif // EIGEN_SWAP_H
+
+
+
+
+
+
diff --git a/extern/Eigen2/Eigen/src/Core/Transpose.h b/extern/Eigen2/Eigen/src/Core/Transpose.h
new file mode 100644
index 00000000000..870edfe320b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Transpose.h
@@ -0,0 +1,228 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_TRANSPOSE_H
+#define EIGEN_TRANSPOSE_H
+
+/** \class Transpose
+  *
+  * \brief Expression of the transpose of a matrix
+  *
+  * \param MatrixType the type of the object of which we are taking the transpose
+  *
+  * This class represents an expression of the transpose of a matrix.
+  * It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
+  * and most of the time this is the only way it is used.
+  *
+  * \sa MatrixBase::transpose(), MatrixBase::adjoint()
+  */
+template<typename MatrixType>
+struct ei_traits<Transpose<MatrixType> >
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    RowsAtCompileTime = MatrixType::ColsAtCompileTime,
+    ColsAtCompileTime = MatrixType::RowsAtCompileTime,
+    MaxRowsAtCompileTime = MatrixType::MaxColsAtCompileTime,
+    MaxColsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+    Flags = ((int(_MatrixTypeNested::Flags) ^ RowMajorBit)
+          & ~(LowerTriangularBit | UpperTriangularBit))
+          | (int(_MatrixTypeNested::Flags)&UpperTriangularBit ? LowerTriangularBit : 0)
+          | (int(_MatrixTypeNested::Flags)&LowerTriangularBit ? UpperTriangularBit : 0),
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost
+  };
+};
+
+template<typename MatrixType> class Transpose
+  : public MatrixBase<Transpose<MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
+
+    inline Transpose(const MatrixType& matrix) : m_matrix(matrix) {}
+
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
+
+    inline int rows() const { return m_matrix.cols(); }
+    inline int cols() const { return m_matrix.rows(); }
+    inline int nonZeros() const { return m_matrix.nonZeros(); }
+    inline int stride(void) const { return m_matrix.stride(); }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived().coeffRef(col, row);
+    }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(col, row);
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(index);
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_matrix.const_cast_derived().coeffRef(index);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int row, int col) const
+    {
+      return m_matrix.template packet<LoadMode>(col, row);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int row, int col, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<LoadMode>(col, row, x);
+    }
+
+    template<int LoadMode>
+    inline const PacketScalar packet(int index) const
+    {
+      return m_matrix.template packet<LoadMode>(index);
+    }
+
+    template<int LoadMode>
+    inline void writePacket(int index, const PacketScalar& x)
+    {
+      m_matrix.const_cast_derived().template writePacket<LoadMode>(index, x);
+    }
+
+  protected:
+    const typename MatrixType::Nested m_matrix;
+};
+
+/** \returns an expression of the transpose of *this.
+  *
+  * Example: \include MatrixBase_transpose.cpp
+  * Output: \verbinclude MatrixBase_transpose.out
+  *
+  * \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
+  * \code
+  * m = m.transpose(); // bug!!! caused by aliasing effect
+  * \endcode
+  * Instead, use the transposeInPlace() method:
+  * \code
+  * m.transposeInPlace();
+  * \endcode
+  * which gives Eigen good opportunities for optimization, or alternatively you can also do:
+  * \code
+  * m = m.transpose().eval();
+  * \endcode
+  *
+  * \sa transposeInPlace(), adjoint() */
+template<typename Derived>
+inline Transpose<Derived>
+MatrixBase<Derived>::transpose()
+{
+  return derived();
+}
+
+/** This is the const version of transpose().
+  *
+  * Make sure you read the warning for transpose() !
+  *
+  * \sa transposeInPlace(), adjoint() */
+template<typename Derived>
+inline const Transpose<Derived>
+MatrixBase<Derived>::transpose() const
+{
+  return derived();
+}
+
+/** \returns an expression of the adjoint (i.e. conjugate transpose) of *this.
+  *
+  * Example: \include MatrixBase_adjoint.cpp
+  * Output: \verbinclude MatrixBase_adjoint.out
+  *
+  * \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
+  * \code
+  * m = m.adjoint(); // bug!!! caused by aliasing effect
+  * \endcode
+  * Instead, do:
+  * \code
+  * m = m.adjoint().eval();
+  * \endcode
+  *
+  * \sa transpose(), conjugate(), class Transpose, class ei_scalar_conjugate_op */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::AdjointReturnType
+MatrixBase<Derived>::adjoint() const
+{
+  return conjugate().nestByValue();
+}
+
+/***************************************************************************
+* "in place" transpose implementation
+***************************************************************************/
+
+template<typename MatrixType,
+  bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) && MatrixType::RowsAtCompileTime!=Dynamic>
+struct ei_inplace_transpose_selector;
+
+template<typename MatrixType>
+struct ei_inplace_transpose_selector<MatrixType,true> { // square matrix
+  static void run(MatrixType& m) {
+    m.template part<StrictlyUpperTriangular>().swap(m.transpose());
+  }
+};
+
+template<typename MatrixType>
+struct ei_inplace_transpose_selector<MatrixType,false> { // non square matrix
+  static void run(MatrixType& m) {
+    if (m.rows()==m.cols())
+      m.template part<StrictlyUpperTriangular>().swap(m.transpose());
+    else
+      m = m.transpose().eval();
+  }
+};
+
+/** This is the "in place" version of transpose: it transposes \c *this.
+  *
+  * In most cases it is probably better to simply use the transposed expression
+  * of a matrix. However, when transposing the matrix data itself is really needed,
+  * then this "in-place" version is probably the right choice because it provides
+  * the following additional features:
+  *  - less error prone: doing the same operation with .transpose() requires special care:
+  *    \code m = m.transpose().eval(); \endcode
+  *  - no temporary object is created (currently only for squared matrices)
+  *  - it allows future optimizations (cache friendliness, etc.)
+  *
+  * \note if the matrix is not square, then \c *this must be a resizable matrix.
+  *
+  * \sa transpose(), adjoint() */
+template<typename Derived>
+inline void MatrixBase<Derived>::transposeInPlace()
+{
+  ei_inplace_transpose_selector<Derived>::run(derived());
+}
+
+#endif // EIGEN_TRANSPOSE_H
diff --git a/extern/Eigen2/Eigen/src/Core/Visitor.h b/extern/Eigen2/Eigen/src/Core/Visitor.h
new file mode 100644
index 00000000000..7569114e90d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/Visitor.h
@@ -0,0 +1,228 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_VISITOR_H
+#define EIGEN_VISITOR_H
+
+template<typename Visitor, typename Derived, int UnrollCount>
+struct ei_visitor_impl
+{
+  enum {
+    col = (UnrollCount-1) / Derived::RowsAtCompileTime,
+    row = (UnrollCount-1) % Derived::RowsAtCompileTime
+  };
+
+  inline static void run(const Derived &mat, Visitor& visitor)
+  {
+    ei_visitor_impl<Visitor, Derived, UnrollCount-1>::run(mat, visitor);
+    visitor(mat.coeff(row, col), row, col);
+  }
+};
+
+template<typename Visitor, typename Derived>
+struct ei_visitor_impl<Visitor, Derived, 1>
+{
+  inline static void run(const Derived &mat, Visitor& visitor)
+  {
+    return visitor.init(mat.coeff(0, 0), 0, 0);
+  }
+};
+
+template<typename Visitor, typename Derived>
+struct ei_visitor_impl<Visitor, Derived, Dynamic>
+{
+  inline static void run(const Derived& mat, Visitor& visitor)
+  {
+    visitor.init(mat.coeff(0,0), 0, 0);
+    for(int i = 1; i < mat.rows(); ++i)
+      visitor(mat.coeff(i, 0), i, 0);
+    for(int j = 1; j < mat.cols(); ++j)
+      for(int i = 0; i < mat.rows(); ++i)
+        visitor(mat.coeff(i, j), i, j);
+  }
+};
+
+
+/** Applies the visitor \a visitor to the whole coefficients of the matrix or vector.
+  *
+  * The template parameter \a Visitor is the type of the visitor and provides the following interface:
+  * \code
+  * struct MyVisitor {
+  *   // called for the first coefficient
+  *   void init(const Scalar& value, int i, int j);
+  *   // called for all other coefficients
+  *   void operator() (const Scalar& value, int i, int j);
+  * };
+  * \endcode
+  *
+  * \note compared to one or two \em for \em loops, visitors offer automatic
+  * unrolling for small fixed size matrix.
+  *
+  * \sa minCoeff(int*,int*), maxCoeff(int*,int*), MatrixBase::redux()
+  */
+template<typename Derived>
+template<typename Visitor>
+void MatrixBase<Derived>::visit(Visitor& visitor) const
+{
+  const bool unroll = SizeAtCompileTime * CoeffReadCost
+                    + (SizeAtCompileTime-1) * ei_functor_traits<Visitor>::Cost
+                    <= EIGEN_UNROLLING_LIMIT;
+  return ei_visitor_impl<Visitor, Derived,
+      unroll ? int(SizeAtCompileTime) : Dynamic
+    >::run(derived(), visitor);
+}
+
+/** \internal
+  * \brief Base class to implement min and max visitors
+  */
+template <typename Scalar>
+struct ei_coeff_visitor
+{
+  int row, col;
+  Scalar res;
+  inline void init(const Scalar& value, int i, int j)
+  {
+    res = value;
+    row = i;
+    col = j;
+  }
+};
+
+/** \internal
+  * \brief Visitor computing the min coefficient with its value and coordinates
+  *
+  * \sa MatrixBase::minCoeff(int*, int*)
+  */
+template <typename Scalar>
+struct ei_min_coeff_visitor : ei_coeff_visitor<Scalar>
+{
+  void operator() (const Scalar& value, int i, int j)
+  {
+    if(value < this->res)
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template<typename Scalar>
+struct ei_functor_traits<ei_min_coeff_visitor<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost
+  };
+};
+
+/** \internal
+  * \brief Visitor computing the max coefficient with its value and coordinates
+  *
+  * \sa MatrixBase::maxCoeff(int*, int*)
+  */
+template <typename Scalar>
+struct ei_max_coeff_visitor : ei_coeff_visitor<Scalar>
+{
+  void operator() (const Scalar& value, int i, int j)
+  {
+    if(value > this->res)
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template<typename Scalar>
+struct ei_functor_traits<ei_max_coeff_visitor<Scalar> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost
+  };
+};
+
+/** \returns the minimum of all coefficients of *this
+  * and puts in *row and *col its location.
+  *
+  * \sa MatrixBase::minCoeff(int*), MatrixBase::maxCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::minCoeff()
+  */
+template<typename Derived>
+typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::minCoeff(int* row, int* col) const
+{
+  ei_min_coeff_visitor<Scalar> minVisitor;
+  this->visit(minVisitor);
+  *row = minVisitor.row;
+  if (col) *col = minVisitor.col;
+  return minVisitor.res;
+}
+
+/** \returns the minimum of all coefficients of *this
+  * and puts in *index its location.
+  *
+  * \sa MatrixBase::minCoeff(int*,int*), MatrixBase::maxCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::minCoeff()
+  */
+template<typename Derived>
+typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::minCoeff(int* index) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  ei_min_coeff_visitor<Scalar> minVisitor;
+  this->visit(minVisitor);
+  *index = (RowsAtCompileTime==1) ? minVisitor.col : minVisitor.row;
+  return minVisitor.res;
+}
+
+/** \returns the maximum of all coefficients of *this
+  * and puts in *row and *col its location.
+  *
+  * \sa MatrixBase::minCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::maxCoeff()
+  */
+template<typename Derived>
+typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::maxCoeff(int* row, int* col) const
+{
+  ei_max_coeff_visitor<Scalar> maxVisitor;
+  this->visit(maxVisitor);
+  *row = maxVisitor.row;
+  if (col) *col = maxVisitor.col;
+  return maxVisitor.res;
+}
+
+/** \returns the maximum of all coefficients of *this
+  * and puts in *index its location.
+  *
+  * \sa MatrixBase::maxCoeff(int*,int*), MatrixBase::minCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::maxCoeff()
+  */
+template<typename Derived>
+typename ei_traits<Derived>::Scalar
+MatrixBase<Derived>::maxCoeff(int* index) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  ei_max_coeff_visitor<Scalar> maxVisitor;
+  this->visit(maxVisitor);
+  *index = (RowsAtCompileTime==1) ? maxVisitor.col : maxVisitor.row;
+  return maxVisitor.res;
+}
+
+#endif // EIGEN_VISITOR_H
diff --git a/extern/Eigen2/Eigen/src/Core/arch/AltiVec/PacketMath.h b/extern/Eigen2/Eigen/src/Core/arch/AltiVec/PacketMath.h
new file mode 100644
index 00000000000..4de3b5e2e0b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/arch/AltiVec/PacketMath.h
@@ -0,0 +1,354 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Konstantinos Margaritis <markos@codex.gr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PACKET_MATH_ALTIVEC_H
+#define EIGEN_PACKET_MATH_ALTIVEC_H
+
+#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 4
+#endif
+
+typedef __vector float          v4f;
+typedef __vector int            v4i;
+typedef __vector unsigned int   v4ui;
+typedef __vector __bool int     v4bi;
+
+// We don't want to write the same code all the time, but we need to reuse the constants
+// and it doesn't really work to declare them global, so we define macros instead
+
+#define USE_CONST_v0i     const v4i   v0i   = vec_splat_s32(0)
+#define USE_CONST_v1i     const v4i   v1i   = vec_splat_s32(1)
+#define USE_CONST_v16i_   const v4i   v16i_ = vec_splat_s32(-16)
+#define USE_CONST_v0f     USE_CONST_v0i; const v4f v0f = (v4f) v0i
+#define USE_CONST_v1f     USE_CONST_v1i; const v4f v1f = vec_ctf(v1i, 0)
+#define USE_CONST_v1i_    const v4ui  v1i_  = vec_splat_u32(-1)
+#define USE_CONST_v0f_    USE_CONST_v1i_; const v4f v0f_ = (v4f) vec_sl(v1i_, v1i_)
+
+template<> struct ei_packet_traits<float>  { typedef v4f type; enum {size=4}; };
+template<> struct ei_packet_traits<int>    { typedef v4i type; enum {size=4}; };
+
+template<> struct ei_unpacket_traits<v4f>  { typedef float  type; enum {size=4}; };
+template<> struct ei_unpacket_traits<v4i>  { typedef int    type; enum {size=4}; };
+
+inline std::ostream & operator <<(std::ostream & s, const v4f & v)
+{
+  union {
+    v4f   v;
+    float n[4];
+  } vt;
+  vt.v = v;
+  s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
+  return s;
+}
+
+inline std::ostream & operator <<(std::ostream & s, const v4i & v)
+{
+  union {
+    v4i   v;
+    int n[4];
+  } vt;
+  vt.v = v;
+  s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
+  return s;
+}
+
+inline std::ostream & operator <<(std::ostream & s, const v4ui & v)
+{
+  union {
+    v4ui   v;
+    unsigned int n[4];
+  } vt;
+  vt.v = v;
+  s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
+  return s;
+}
+
+inline std::ostream & operator <<(std::ostream & s, const v4bi & v)
+{
+  union {
+    __vector __bool int v;
+    unsigned int n[4];
+  } vt;
+  vt.v = v;
+  s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
+  return s;
+}
+
+template<> inline v4f  ei_padd(const v4f&   a, const v4f&   b) { return vec_add(a,b); }
+template<> inline v4i  ei_padd(const v4i&   a, const v4i&   b) { return vec_add(a,b); }
+
+template<> inline v4f  ei_psub(const v4f&   a, const v4f&   b) { return vec_sub(a,b); }
+template<> inline v4i  ei_psub(const v4i&   a, const v4i&   b) { return vec_sub(a,b); }
+
+template<> inline v4f  ei_pmul(const v4f&   a, const v4f&   b) { USE_CONST_v0f; return vec_madd(a,b, v0f); }
+template<> inline v4i  ei_pmul(const v4i&   a, const v4i&   b)
+{
+  // Detailed in: http://freevec.org/content/32bit_signed_integer_multiplication_altivec
+  //Set up constants, variables
+  v4i a1, b1, bswap, low_prod, high_prod, prod, prod_, v1sel;
+  USE_CONST_v0i;
+  USE_CONST_v1i;
+  USE_CONST_v16i_;
+
+  // Get the absolute values 
+  a1  = vec_abs(a);
+  b1  = vec_abs(b);
+
+  // Get the signs using xor
+  v4bi sgn = (v4bi) vec_cmplt(vec_xor(a, b), v0i);
+
+  // Do the multiplication for the asbolute values.
+  bswap = (v4i) vec_rl((v4ui) b1, (v4ui) v16i_ );
+  low_prod = vec_mulo((__vector short)a1, (__vector short)b1);
+  high_prod = vec_msum((__vector short)a1, (__vector short)bswap, v0i);
+  high_prod = (v4i) vec_sl((v4ui) high_prod, (v4ui) v16i_);
+  prod = vec_add( low_prod, high_prod );
+
+  // NOR the product and select only the negative elements according to the sign mask
+  prod_ = vec_nor(prod, prod);
+  prod_ = vec_sel(v0i, prod_, sgn);
+
+  // Add 1 to the result to get the negative numbers
+  v1sel = vec_sel(v0i, v1i, sgn);
+  prod_ = vec_add(prod_, v1sel);
+
+  // Merge the results back to the final vector.
+  prod = vec_sel(prod, prod_, sgn);
+
+  return prod;
+}
+
+template<> inline v4f  ei_pdiv(const v4f&   a, const v4f&   b) {
+  v4f t, y_0, y_1, res;
+  USE_CONST_v0f;
+  USE_CONST_v1f;
+
+  // Altivec does not offer a divide instruction, we have to do a reciprocal approximation
+  y_0 = vec_re(b);
+  
+  // Do one Newton-Raphson iteration to get the needed accuracy
+  t = vec_nmsub(y_0, b, v1f);
+  y_1 = vec_madd(y_0, t, y_0);
+
+  res = vec_madd(a, y_1, v0f);
+  return res;
+}
+
+template<> inline v4f  ei_pmadd(const v4f&  a, const v4f&   b, const v4f&  c) { return vec_madd(a, b, c); }
+
+template<> inline v4f  ei_pmin(const v4f&   a, const v4f&   b) { return vec_min(a,b); }
+template<> inline v4i  ei_pmin(const v4i&   a, const v4i&   b) { return vec_min(a,b); }
+
+template<> inline v4f  ei_pmax(const v4f&   a, const v4f&   b) { return vec_max(a,b); }
+template<> inline v4i  ei_pmax(const v4i&   a, const v4i&   b) { return vec_max(a,b); }
+
+template<> inline v4f  ei_pload(const float* from) { return vec_ld(0, from); }
+template<> inline v4i  ei_pload(const int*   from) { return vec_ld(0, from); }
+
+template<> inline v4f  ei_ploadu(const float*  from)
+{
+  // Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
+  __vector unsigned char MSQ, LSQ;
+  __vector unsigned char mask;
+  MSQ = vec_ld(0, (unsigned char *)from);          // most significant quadword
+  LSQ = vec_ld(15, (unsigned char *)from);         // least significant quadword
+  mask = vec_lvsl(0, from);                        // create the permute mask
+  return (v4f) vec_perm(MSQ, LSQ, mask);           // align the data
+}
+
+template<> inline v4i    ei_ploadu(const int*    from)
+{
+  // Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
+  __vector unsigned char MSQ, LSQ;
+  __vector unsigned char mask;
+  MSQ = vec_ld(0, (unsigned char *)from);          // most significant quadword
+  LSQ = vec_ld(15, (unsigned char *)from);         // least significant quadword
+  mask = vec_lvsl(0, from);                        // create the permute mask
+  return (v4i) vec_perm(MSQ, LSQ, mask);    // align the data
+}
+
+template<> inline v4f  ei_pset1(const float&  from)
+{
+  // Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
+  float __attribute__(aligned(16)) af[4];
+  af[0] = from;
+  v4f vc = vec_ld(0, af);
+  vc = vec_splat(vc, 0);
+  return vc;
+}
+
+template<> inline v4i    ei_pset1(const int&    from)
+{
+  int __attribute__(aligned(16)) ai[4];
+  ai[0] = from;
+  v4i vc = vec_ld(0, ai);
+  vc = vec_splat(vc, 0);
+  return vc;
+}
+
+template<> inline void ei_pstore(float*   to, const v4f&   from) { vec_st(from, 0, to); }
+template<> inline void ei_pstore(int*     to, const v4i&   from) { vec_st(from, 0, to); }
+
+template<> inline void ei_pstoreu(float*  to, const v4f&   from)
+{
+  // Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
+  // Warning: not thread safe!
+  __vector unsigned char MSQ, LSQ, edges;
+  __vector unsigned char edgeAlign, align;
+
+  MSQ = vec_ld(0, (unsigned char *)to);                     // most significant quadword
+  LSQ = vec_ld(15, (unsigned char *)to);                    // least significant quadword
+  edgeAlign = vec_lvsl(0, to);                              // permute map to extract edges
+  edges=vec_perm(LSQ,MSQ,edgeAlign);                        // extract the edges
+  align = vec_lvsr( 0, to );                                // permute map to misalign data
+  MSQ = vec_perm(edges,(__vector unsigned char)from,align);   // misalign the data (MSQ)
+  LSQ = vec_perm((__vector unsigned char)from,edges,align);   // misalign the data (LSQ)
+  vec_st( LSQ, 15, (unsigned char *)to );                   // Store the LSQ part first
+  vec_st( MSQ, 0, (unsigned char *)to );                    // Store the MSQ part
+}
+
+template<> inline void ei_pstoreu(int*    to , const v4i&    from )
+{
+  // Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
+  // Warning: not thread safe!
+  __vector unsigned char MSQ, LSQ, edges;
+  __vector unsigned char edgeAlign, align;
+
+  MSQ = vec_ld(0, (unsigned char *)to);                     // most significant quadword
+  LSQ = vec_ld(15, (unsigned char *)to);                    // least significant quadword
+  edgeAlign = vec_lvsl(0, to);                              // permute map to extract edges
+  edges=vec_perm(LSQ,MSQ,edgeAlign);                        // extract the edges
+  align = vec_lvsr( 0, to );                                // permute map to misalign data
+  MSQ = vec_perm(edges,(__vector unsigned char)from,align);   // misalign the data (MSQ)
+  LSQ = vec_perm((__vector unsigned char)from,edges,align);   // misalign the data (LSQ)
+  vec_st( LSQ, 15, (unsigned char *)to );                   // Store the LSQ part first
+  vec_st( MSQ, 0, (unsigned char *)to );                    // Store the MSQ part
+}
+
+template<> inline float  ei_pfirst(const v4f&  a)
+{
+  float __attribute__(aligned(16)) af[4];
+  vec_st(a, 0, af);
+  return af[0];
+}
+
+template<> inline int    ei_pfirst(const v4i&  a)
+{
+  int __attribute__(aligned(16)) ai[4];
+  vec_st(a, 0, ai);
+  return ai[0];
+}
+
+inline v4f ei_preduxp(const v4f* vecs)
+{
+  v4f v[4], sum[4];
+
+  // It's easier and faster to transpose then add as columns
+  // Check: http://www.freevec.org/function/matrix_4x4_transpose_floats for explanation
+  // Do the transpose, first set of moves
+  v[0] = vec_mergeh(vecs[0], vecs[2]);
+  v[1] = vec_mergel(vecs[0], vecs[2]);
+  v[2] = vec_mergeh(vecs[1], vecs[3]);
+  v[3] = vec_mergel(vecs[1], vecs[3]);
+  // Get the resulting vectors
+  sum[0] = vec_mergeh(v[0], v[2]);
+  sum[1] = vec_mergel(v[0], v[2]);
+  sum[2] = vec_mergeh(v[1], v[3]);
+  sum[3] = vec_mergel(v[1], v[3]);
+
+  // Now do the summation:
+  // Lines 0+1
+  sum[0] = vec_add(sum[0], sum[1]);
+  // Lines 2+3
+  sum[1] = vec_add(sum[2], sum[3]);
+  // Add the results
+  sum[0] = vec_add(sum[0], sum[1]);
+  return sum[0];
+}
+
+inline float ei_predux(const v4f& a)
+{
+  v4f b, sum;
+  b = (v4f)vec_sld(a, a, 8);
+  sum = vec_add(a, b);
+  b = (v4f)vec_sld(sum, sum, 4);
+  sum = vec_add(sum, b);
+  return ei_pfirst(sum);
+}
+
+inline v4i  ei_preduxp(const v4i* vecs)
+{
+  v4i v[4], sum[4];
+
+  // It's easier and faster to transpose then add as columns
+  // Check: http://www.freevec.org/function/matrix_4x4_transpose_floats for explanation
+  // Do the transpose, first set of moves
+  v[0] = vec_mergeh(vecs[0], vecs[2]);
+  v[1] = vec_mergel(vecs[0], vecs[2]);
+  v[2] = vec_mergeh(vecs[1], vecs[3]);
+  v[3] = vec_mergel(vecs[1], vecs[3]);
+  // Get the resulting vectors
+  sum[0] = vec_mergeh(v[0], v[2]);
+  sum[1] = vec_mergel(v[0], v[2]);
+  sum[2] = vec_mergeh(v[1], v[3]);
+  sum[3] = vec_mergel(v[1], v[3]);
+
+  // Now do the summation:
+  // Lines 0+1
+  sum[0] = vec_add(sum[0], sum[1]);
+  // Lines 2+3
+  sum[1] = vec_add(sum[2], sum[3]);
+  // Add the results
+  sum[0] = vec_add(sum[0], sum[1]);
+  return sum[0];
+}
+
+inline int ei_predux(const v4i& a)
+{
+  USE_CONST_v0i;
+  v4i sum;
+  sum = vec_sums(a, v0i);
+  sum = vec_sld(sum, v0i, 12);
+  return ei_pfirst(sum);
+}
+
+template<int Offset>
+struct ei_palign_impl<Offset, v4f>
+{
+  inline static void run(v4f& first, const v4f& second)
+  {
+    first = vec_sld(first, second, Offset*4);
+  }
+};
+
+template<int Offset>
+struct ei_palign_impl<Offset, v4i>
+{
+  inline static void run(v4i& first, const v4i& second)
+  {
+    first = vec_sld(first, second, Offset*4);
+  }
+};
+
+#endif // EIGEN_PACKET_MATH_ALTIVEC_H
diff --git a/extern/Eigen2/Eigen/src/Core/arch/SSE/PacketMath.h b/extern/Eigen2/Eigen/src/Core/arch/SSE/PacketMath.h
new file mode 100644
index 00000000000..9ca65b9be5b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/arch/SSE/PacketMath.h
@@ -0,0 +1,321 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PACKET_MATH_SSE_H
+#define EIGEN_PACKET_MATH_SSE_H
+
+#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
+#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 16
+#endif
+
+template<> struct ei_packet_traits<float>  { typedef __m128  type; enum {size=4}; };
+template<> struct ei_packet_traits<double> { typedef __m128d type; enum {size=2}; };
+template<> struct ei_packet_traits<int>    { typedef __m128i type; enum {size=4}; };
+
+template<> struct ei_unpacket_traits<__m128>  { typedef float  type; enum {size=4}; };
+template<> struct ei_unpacket_traits<__m128d> { typedef double type; enum {size=2}; };
+template<> struct ei_unpacket_traits<__m128i> { typedef int    type; enum {size=4}; };
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pset1<float>(const float&  from) { return _mm_set1_ps(from); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pset1<double>(const double& from) { return _mm_set1_pd(from); }
+template<> EIGEN_STRONG_INLINE __m128i ei_pset1<int>(const int&    from) { return _mm_set1_epi32(from); }
+
+template<> EIGEN_STRONG_INLINE __m128  ei_padd<__m128>(const __m128&  a, const __m128&  b) { return _mm_add_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_padd<__m128d>(const __m128d& a, const __m128d& b) { return _mm_add_pd(a,b); }
+template<> EIGEN_STRONG_INLINE __m128i ei_padd<__m128i>(const __m128i& a, const __m128i& b) { return _mm_add_epi32(a,b); }
+
+template<> EIGEN_STRONG_INLINE __m128  ei_psub<__m128>(const __m128&  a, const __m128&  b) { return _mm_sub_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_psub<__m128d>(const __m128d& a, const __m128d& b) { return _mm_sub_pd(a,b); }
+template<> EIGEN_STRONG_INLINE __m128i ei_psub<__m128i>(const __m128i& a, const __m128i& b) { return _mm_sub_epi32(a,b); }
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pmul<__m128>(const __m128&  a, const __m128&  b) { return _mm_mul_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pmul<__m128d>(const __m128d& a, const __m128d& b) { return _mm_mul_pd(a,b); }
+template<> EIGEN_STRONG_INLINE __m128i ei_pmul<__m128i>(const __m128i& a, const __m128i& b)
+{
+  return _mm_or_si128(
+    _mm_and_si128(
+      _mm_mul_epu32(a,b),
+      _mm_setr_epi32(0xffffffff,0,0xffffffff,0)),
+    _mm_slli_si128(
+      _mm_and_si128(
+        _mm_mul_epu32(_mm_srli_si128(a,4),_mm_srli_si128(b,4)),
+        _mm_setr_epi32(0xffffffff,0,0xffffffff,0)), 4));
+}
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pdiv<__m128>(const __m128&  a, const __m128&  b) { return _mm_div_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pdiv<__m128d>(const __m128d& a, const __m128d& b) { return _mm_div_pd(a,b); }
+template<> EIGEN_STRONG_INLINE __m128i ei_pdiv<__m128i>(const __m128i& /*a*/, const __m128i& /*b*/)
+{ ei_assert(false && "packet integer division are not supported by SSE");
+  __m128i dummy = ei_pset1<int>(0);
+  return dummy;
+}
+
+// for some weird raisons, it has to be overloaded for packet integer
+template<> EIGEN_STRONG_INLINE __m128i ei_pmadd(const __m128i& a, const __m128i& b, const __m128i& c) { return ei_padd(ei_pmul(a,b), c); }
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pmin<__m128>(const __m128&  a, const __m128&  b) { return _mm_min_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pmin<__m128d>(const __m128d& a, const __m128d& b) { return _mm_min_pd(a,b); }
+// FIXME this vectorized min operator is likely to be slower than the standard one
+template<> EIGEN_STRONG_INLINE __m128i ei_pmin<__m128i>(const __m128i& a, const __m128i& b)
+{
+  __m128i mask = _mm_cmplt_epi32(a,b);
+  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+}
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pmax<__m128>(const __m128&  a, const __m128&  b) { return _mm_max_ps(a,b); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pmax<__m128d>(const __m128d& a, const __m128d& b) { return _mm_max_pd(a,b); }
+// FIXME this vectorized max operator is likely to be slower than the standard one
+template<> EIGEN_STRONG_INLINE __m128i ei_pmax<__m128i>(const __m128i& a, const __m128i& b)
+{
+  __m128i mask = _mm_cmpgt_epi32(a,b);
+  return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
+}
+
+template<> EIGEN_STRONG_INLINE __m128  ei_pload<float>(const float*   from) { return _mm_load_ps(from); }
+template<> EIGEN_STRONG_INLINE __m128d ei_pload<double>(const double*  from) { return _mm_load_pd(from); }
+template<> EIGEN_STRONG_INLINE __m128i ei_pload<int>(const int* from) { return _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
+
+template<> EIGEN_STRONG_INLINE __m128  ei_ploadu<float>(const float*   from) { return _mm_loadu_ps(from); }
+// template<> EIGEN_STRONG_INLINE __m128  ei_ploadu(const float*   from) {
+//   if (size_t(from)&0xF)
+//     return _mm_loadu_ps(from);
+//   else 
+//     return _mm_loadu_ps(from);
+// }
+template<> EIGEN_STRONG_INLINE __m128d ei_ploadu<double>(const double*  from) { return _mm_loadu_pd(from); }
+template<> EIGEN_STRONG_INLINE __m128i ei_ploadu<int>(const int* from) { return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from)); }
+
+template<> EIGEN_STRONG_INLINE void ei_pstore<float>(float*  to, const __m128&  from) { _mm_store_ps(to, from); }
+template<> EIGEN_STRONG_INLINE void ei_pstore<double>(double* to, const __m128d& from) { _mm_store_pd(to, from); }
+template<> EIGEN_STRONG_INLINE void ei_pstore<int>(int*    to, const __m128i& from) { _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
+
+template<> EIGEN_STRONG_INLINE void ei_pstoreu<float>(float*  to, const __m128&  from) { _mm_storeu_ps(to, from); }
+template<> EIGEN_STRONG_INLINE void ei_pstoreu<double>(double* to, const __m128d& from) { _mm_storeu_pd(to, from); }
+template<> EIGEN_STRONG_INLINE void ei_pstoreu<int>(int*    to, const __m128i& from) { _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
+
+#ifdef _MSC_VER
+// this fix internal compilation error
+template<> EIGEN_STRONG_INLINE float  ei_pfirst<__m128>(const __m128&  a) { float x = _mm_cvtss_f32(a); return x; }
+template<> EIGEN_STRONG_INLINE double ei_pfirst<__m128d>(const __m128d& a) { double x = _mm_cvtsd_f64(a); return x; }
+template<> EIGEN_STRONG_INLINE int    ei_pfirst<__m128i>(const __m128i& a) { int x = _mm_cvtsi128_si32(a); return x; }
+#else
+template<> EIGEN_STRONG_INLINE float  ei_pfirst<__m128>(const __m128&  a) { return _mm_cvtss_f32(a); }
+template<> EIGEN_STRONG_INLINE double ei_pfirst<__m128d>(const __m128d& a) { return _mm_cvtsd_f64(a); }
+template<> EIGEN_STRONG_INLINE int    ei_pfirst<__m128i>(const __m128i& a) { return _mm_cvtsi128_si32(a); }
+#endif
+
+#ifdef __SSE3__
+// TODO implement SSE2 versions as well as integer versions
+template<> EIGEN_STRONG_INLINE __m128 ei_preduxp<__m128>(const __m128* vecs)
+{
+  return _mm_hadd_ps(_mm_hadd_ps(vecs[0], vecs[1]),_mm_hadd_ps(vecs[2], vecs[3]));
+}
+template<> EIGEN_STRONG_INLINE __m128d ei_preduxp<__m128d>(const __m128d* vecs)
+{
+  return _mm_hadd_pd(vecs[0], vecs[1]);
+}
+// SSSE3 version:
+// EIGEN_STRONG_INLINE __m128i ei_preduxp(const __m128i* vecs)
+// {
+//   return _mm_hadd_epi32(_mm_hadd_epi32(vecs[0], vecs[1]),_mm_hadd_epi32(vecs[2], vecs[3]));
+// }
+
+template<> EIGEN_STRONG_INLINE float ei_predux<__m128>(const __m128& a)
+{
+  __m128 tmp0 = _mm_hadd_ps(a,a);
+  return ei_pfirst(_mm_hadd_ps(tmp0, tmp0));
+}
+
+template<> EIGEN_STRONG_INLINE double ei_predux<__m128d>(const __m128d& a) { return ei_pfirst(_mm_hadd_pd(a, a)); }
+
+// SSSE3 version:
+// EIGEN_STRONG_INLINE float ei_predux(const __m128i& a)
+// {
+//   __m128i tmp0 = _mm_hadd_epi32(a,a);
+//   return ei_pfirst(_mm_hadd_epi32(tmp0, tmp0));
+// }
+#else
+// SSE2 versions
+template<> EIGEN_STRONG_INLINE float ei_predux<__m128>(const __m128& a)
+{
+  __m128 tmp = _mm_add_ps(a, _mm_movehl_ps(a,a));
+  return ei_pfirst(_mm_add_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
+}
+template<> EIGEN_STRONG_INLINE double ei_predux<__m128d>(const __m128d& a)
+{
+  return ei_pfirst(_mm_add_sd(a, _mm_unpackhi_pd(a,a)));
+}
+
+template<> EIGEN_STRONG_INLINE __m128 ei_preduxp<__m128>(const __m128* vecs)
+{
+  __m128 tmp0, tmp1, tmp2;
+  tmp0 = _mm_unpacklo_ps(vecs[0], vecs[1]);
+  tmp1 = _mm_unpackhi_ps(vecs[0], vecs[1]);
+  tmp2 = _mm_unpackhi_ps(vecs[2], vecs[3]);
+  tmp0 = _mm_add_ps(tmp0, tmp1);
+  tmp1 = _mm_unpacklo_ps(vecs[2], vecs[3]);
+  tmp1 = _mm_add_ps(tmp1, tmp2);
+  tmp2 = _mm_movehl_ps(tmp1, tmp0);
+  tmp0 = _mm_movelh_ps(tmp0, tmp1);
+  return _mm_add_ps(tmp0, tmp2);
+}
+
+template<> EIGEN_STRONG_INLINE __m128d ei_preduxp<__m128d>(const __m128d* vecs)
+{
+  return _mm_add_pd(_mm_unpacklo_pd(vecs[0], vecs[1]), _mm_unpackhi_pd(vecs[0], vecs[1]));
+}
+#endif  // SSE3
+
+template<> EIGEN_STRONG_INLINE int ei_predux<__m128i>(const __m128i& a)
+{
+  __m128i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a,a));
+  return ei_pfirst(tmp) + ei_pfirst(_mm_shuffle_epi32(tmp, 1));
+}
+
+template<> EIGEN_STRONG_INLINE __m128i ei_preduxp<__m128i>(const __m128i* vecs)
+{
+  __m128i tmp0, tmp1, tmp2;
+  tmp0 = _mm_unpacklo_epi32(vecs[0], vecs[1]);
+  tmp1 = _mm_unpackhi_epi32(vecs[0], vecs[1]);
+  tmp2 = _mm_unpackhi_epi32(vecs[2], vecs[3]);
+  tmp0 = _mm_add_epi32(tmp0, tmp1);
+  tmp1 = _mm_unpacklo_epi32(vecs[2], vecs[3]);
+  tmp1 = _mm_add_epi32(tmp1, tmp2);
+  tmp2 = _mm_unpacklo_epi64(tmp0, tmp1);
+  tmp0 = _mm_unpackhi_epi64(tmp0, tmp1);
+  return _mm_add_epi32(tmp0, tmp2);
+}
+
+#if (defined __GNUC__)
+// template <> EIGEN_STRONG_INLINE __m128 ei_pmadd(const __m128&  a, const __m128&  b, const __m128&  c)
+// {
+//   __m128 res = b;
+//   asm("mulps %[a], %[b] \n\taddps %[c], %[b]" : [b] "+x" (res) : [a] "x" (a), [c] "x" (c));
+//   return res;
+// }
+// EIGEN_STRONG_INLINE __m128i _mm_alignr_epi8(const __m128i&  a, const __m128i&  b, const int i)
+// {
+//   __m128i res = a;
+//   asm("palignr %[i], %[a], %[b] " : [b] "+x" (res) : [a] "x" (a), [i] "i" (i));
+//   return res;
+// }
+#endif
+
+#ifdef __SSSE3__
+// SSSE3 versions
+template<int Offset>
+struct ei_palign_impl<Offset,__m128>
+{
+  EIGEN_STRONG_INLINE static void run(__m128& first, const __m128& second)
+  {
+    if (Offset!=0)
+      first = _mm_castsi128_ps(_mm_alignr_epi8(_mm_castps_si128(second), _mm_castps_si128(first), Offset*4));
+  }
+};
+
+template<int Offset>
+struct ei_palign_impl<Offset,__m128i>
+{
+  EIGEN_STRONG_INLINE static void run(__m128i& first, const __m128i& second)
+  {
+    if (Offset!=0)
+      first = _mm_alignr_epi8(second,first, Offset*4);
+  }
+};
+
+template<int Offset>
+struct ei_palign_impl<Offset,__m128d>
+{
+  EIGEN_STRONG_INLINE static void run(__m128d& first, const __m128d& second)
+  {
+    if (Offset==1)
+      first = _mm_castsi128_pd(_mm_alignr_epi8(_mm_castpd_si128(second), _mm_castpd_si128(first), 8));
+  }
+};
+#else
+// SSE2 versions
+template<int Offset>
+struct ei_palign_impl<Offset,__m128>
+{
+  EIGEN_STRONG_INLINE static void run(__m128& first, const __m128& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_move_ss(first,second);
+      first = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(first),0x39));
+    }
+    else if (Offset==2)
+    {
+      first = _mm_movehl_ps(first,first);
+      first = _mm_movelh_ps(first,second);
+    }
+    else if (Offset==3)
+    {
+      first = _mm_move_ss(first,second);
+      first = _mm_shuffle_ps(first,second,0x93);
+    }
+  }
+};
+
+template<int Offset>
+struct ei_palign_impl<Offset,__m128i>
+{
+  EIGEN_STRONG_INLINE static void run(__m128i& first, const __m128i& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+      first = _mm_shuffle_epi32(first,0x39);
+    }
+    else if (Offset==2)
+    {
+      first = _mm_castps_si128(_mm_movehl_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(first)));
+      first = _mm_castps_si128(_mm_movelh_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+    }
+    else if (Offset==3)
+    {
+      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
+      first = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second),0x93));
+    }
+  }
+};
+
+template<int Offset>
+struct ei_palign_impl<Offset,__m128d>
+{
+  EIGEN_STRONG_INLINE static void run(__m128d& first, const __m128d& second)
+  {
+    if (Offset==1)
+    {
+      first = _mm_castps_pd(_mm_movehl_ps(_mm_castpd_ps(first),_mm_castpd_ps(first)));
+      first = _mm_castps_pd(_mm_movelh_ps(_mm_castpd_ps(first),_mm_castpd_ps(second)));
+    }
+  }
+};
+#endif
+
+#define ei_vec4f_swizzle1(v,p,q,r,s) \
+  (_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), ((s)<<6|(r)<<4|(q)<<2|(p)))))
+
+#endif // EIGEN_PACKET_MATH_SSE_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/Constants.h b/extern/Eigen2/Eigen/src/Core/util/Constants.h
new file mode 100644
index 00000000000..296c3caa5f6
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/Constants.h
@@ -0,0 +1,254 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CONSTANTS_H
+#define EIGEN_CONSTANTS_H
+
+/** This value means that a quantity is not known at compile-time, and that instead the value is
+  * stored in some runtime variable.
+  *
+  * Explanation for the choice of this value:
+  * - It should be positive and larger than any reasonable compile-time-fixed number of rows or columns.
+  *   This allows to simplify many compile-time conditions throughout Eigen.
+  * - It should be smaller than the sqrt of INT_MAX. Indeed, we often multiply a number of rows with a number
+  *   of columns in order to compute a number of coefficients. Even if we guard that with an "if" checking whether
+  *   the values are Dynamic, we still get a compiler warning "integer overflow". So the only way to get around
+  *   it would be a meta-selector. Doing this everywhere would reduce code readability and lenghten compilation times.
+  *   Also, disabling compiler warnings for integer overflow, sounds like a bad idea.
+  *
+  * If you wish to port Eigen to a platform where sizeof(int)==2, it is perfectly possible to set Dynamic to, say, 100.
+  */
+const int Dynamic = 10000;
+
+/** This value means +Infinity; it is currently used only as the p parameter to MatrixBase::lpNorm<int>().
+  * The value Infinity there means the L-infinity norm.
+  */
+const int Infinity = -1;
+
+/** \defgroup flags flags
+  * \ingroup Core_Module
+  *
+  * These are the possible bits which can be OR'ed to constitute the flags of a matrix or
+  * expression.
+  *
+  * It is important to note that these flags are a purely compile-time notion. They are a compile-time property of
+  * an expression type, implemented as enum's. They are not stored in memory at runtime, and they do not incur any
+  * runtime overhead.
+  *
+  * \sa MatrixBase::Flags
+  */
+
+/** \ingroup flags
+  *
+  * for a matrix, this means that the storage order is row-major.
+  * If this bit is not set, the storage order is column-major.
+  * For an expression, this determines the storage order of
+  * the matrix created by evaluation of that expression. */
+const unsigned int RowMajorBit = 0x1;
+
+/** \ingroup flags
+  *
+  * means the expression should be evaluated by the calling expression */
+const unsigned int EvalBeforeNestingBit = 0x2;
+
+/** \ingroup flags
+  *
+  * means the expression should be evaluated before any assignement */
+const unsigned int EvalBeforeAssigningBit = 0x4;
+
+/** \ingroup flags
+  *
+  * Short version: means the expression might be vectorized
+  *
+  * Long version: means that the coefficients can be handled by packets
+  * and start at a memory location whose alignment meets the requirements
+  * of the present CPU architecture for optimized packet access. In the fixed-size
+  * case, there is the additional condition that the total size of the coefficients
+  * array is a multiple of the packet size, so that it is possible to access all the
+  * coefficients by packets. In the dynamic-size case, there is no such condition
+  * on the total size, so it might not be possible to access the few last coeffs
+  * by packets.
+  *
+  * \note This bit can be set regardless of whether vectorization is actually enabled.
+  *       To check for actual vectorizability, see \a ActualPacketAccessBit.
+  */
+const unsigned int PacketAccessBit = 0x8;
+
+#ifdef EIGEN_VECTORIZE
+/** \ingroup flags
+  *
+  * If vectorization is enabled (EIGEN_VECTORIZE is defined) this constant
+  * is set to the value \a PacketAccessBit.
+  *
+  * If vectorization is not enabled (EIGEN_VECTORIZE is not defined) this constant
+  * is set to the value 0.
+  */
+const unsigned int ActualPacketAccessBit = PacketAccessBit;
+#else
+const unsigned int ActualPacketAccessBit = 0x0;
+#endif
+
+/** \ingroup flags
+  *
+  * Short version: means the expression can be seen as 1D vector.
+  *
+  * Long version: means that one can access the coefficients
+  * of this expression by coeff(int), and coeffRef(int) in the case of a lvalue expression. These
+  * index-based access methods are guaranteed
+  * to not have to do any runtime computation of a (row, col)-pair from the index, so that it
+  * is guaranteed that whenever it is available, index-based access is at least as fast as
+  * (row,col)-based access. Expressions for which that isn't possible don't have the LinearAccessBit.
+  *
+  * If both PacketAccessBit and LinearAccessBit are set, then the
+  * packets of this expression can be accessed by packet(int), and writePacket(int) in the case of a
+  * lvalue expression.
+  *
+  * Typically, all vector expressions have the LinearAccessBit, but there is one exception:
+  * Product expressions don't have it, because it would be troublesome for vectorization, even when the
+  * Product is a vector expression. Thus, vector Product expressions allow index-based coefficient access but
+  * not index-based packet access, so they don't have the LinearAccessBit.
+  */
+const unsigned int LinearAccessBit = 0x10;
+
+/** \ingroup flags
+  *
+  * Means that the underlying array of coefficients can be directly accessed. This means two things.
+  * First, references to the coefficients must be available through coeffRef(int, int). This rules out read-only
+  * expressions whose coefficients are computed on demand by coeff(int, int). Second, the memory layout of the
+  * array of coefficients must be exactly the natural one suggested by rows(), cols(), stride(), and the RowMajorBit.
+  * This rules out expressions such as DiagonalCoeffs, whose coefficients, though referencable, do not have
+  * such a regular memory layout.
+  */
+const unsigned int DirectAccessBit = 0x20;
+
+/** \ingroup flags
+  *
+  * means the first coefficient packet is guaranteed to be aligned */
+const unsigned int AlignedBit = 0x40;
+
+/** \ingroup flags
+  *
+  * means all diagonal coefficients are equal to 0 */
+const unsigned int ZeroDiagBit = 0x80;
+
+/** \ingroup flags
+  *
+  * means all diagonal coefficients are equal to 1 */
+const unsigned int UnitDiagBit = 0x100;
+
+/** \ingroup flags
+  *
+  * means the matrix is selfadjoint (M=M*). */
+const unsigned int SelfAdjointBit = 0x200;
+
+/** \ingroup flags
+  *
+  * means the strictly lower triangular part is 0 */
+const unsigned int UpperTriangularBit = 0x400;
+
+/** \ingroup flags
+  *
+  * means the strictly upper triangular part is 0 */
+const unsigned int LowerTriangularBit = 0x800;
+
+/** \ingroup flags
+  *
+  * means the expression includes sparse matrices and the sparse path has to be taken. */
+const unsigned int SparseBit = 0x1000;
+
+// list of flags that are inherited by default
+const unsigned int HereditaryBits = RowMajorBit
+                                  | EvalBeforeNestingBit
+                                  | EvalBeforeAssigningBit
+                                  | SparseBit;
+
+// Possible values for the Mode parameter of part() and of extract()
+const unsigned int UpperTriangular = UpperTriangularBit;
+const unsigned int StrictlyUpperTriangular = UpperTriangularBit | ZeroDiagBit;
+const unsigned int LowerTriangular = LowerTriangularBit;
+const unsigned int StrictlyLowerTriangular = LowerTriangularBit | ZeroDiagBit;
+const unsigned int SelfAdjoint = SelfAdjointBit;
+
+// additional possible values for the Mode parameter of extract()
+const unsigned int UnitUpperTriangular = UpperTriangularBit | UnitDiagBit;
+const unsigned int UnitLowerTriangular = LowerTriangularBit | UnitDiagBit;
+const unsigned int Diagonal = UpperTriangular | LowerTriangular;
+
+enum { Aligned, Unaligned };
+enum { ForceAligned, AsRequested };
+enum { ConditionalJumpCost = 5 };
+enum CornerType { TopLeft, TopRight, BottomLeft, BottomRight };
+enum DirectionType { Vertical, Horizontal };
+enum ProductEvaluationMode { NormalProduct, CacheFriendlyProduct, DiagonalProduct, SparseTimeSparseProduct, SparseTimeDenseProduct, DenseTimeSparseProduct };
+
+enum {
+  /** \internal Equivalent to a slice vectorization for fixed-size matrices having good alignment
+    * and good size */
+  InnerVectorization,
+  /** \internal Vectorization path using a single loop plus scalar loops for the
+    * unaligned boundaries */
+  LinearVectorization,
+  /** \internal Generic vectorization path using one vectorized loop per row/column with some
+    * scalar loops to handle the unaligned boundaries */
+  SliceVectorization,
+  NoVectorization
+};
+
+enum {
+  NoUnrolling,
+  InnerUnrolling,
+  CompleteUnrolling
+};
+
+enum {
+  ColMajor = 0,
+  RowMajor = 0x1,  // it is only a coincidence that this is equal to RowMajorBit -- don't rely on that
+  /** \internal Don't require alignment for the matrix itself (the array of coefficients, if dynamically allocated, may still be
+                requested to be aligned) */
+  DontAlign = 0,
+  /** \internal Align the matrix itself if it is vectorizable fixed-size */
+  AutoAlign = 0x2
+};
+
+enum {
+  IsDense         = 0,
+  IsSparse        = SparseBit,
+  NoDirectAccess  = 0,
+  HasDirectAccess = DirectAccessBit
+};
+
+const int EiArch_Generic = 0x0;
+const int EiArch_SSE     = 0x1;
+const int EiArch_AltiVec = 0x2;
+
+#if defined EIGEN_VECTORIZE_SSE
+  const int EiArch = EiArch_SSE;
+#elif defined EIGEN_VECTORIZE_ALTIVEC
+  const int EiArch = EiArch_AltiVec;
+#else
+  const int EiArch = EiArch_Generic;
+#endif
+
+#endif // EIGEN_CONSTANTS_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/DisableMSVCWarnings.h b/extern/Eigen2/Eigen/src/Core/util/DisableMSVCWarnings.h
new file mode 100644
index 00000000000..765ddecc53c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/DisableMSVCWarnings.h
@@ -0,0 +1,5 @@
+
+#ifdef _MSC_VER
+  #pragma warning( push )
+  #pragma warning( disable : 4181 4244 4127 4211 4717 )
+#endif
diff --git a/extern/Eigen2/Eigen/src/Core/util/EnableMSVCWarnings.h b/extern/Eigen2/Eigen/src/Core/util/EnableMSVCWarnings.h
new file mode 100644
index 00000000000..8bd61601ebb
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/EnableMSVCWarnings.h
@@ -0,0 +1,4 @@
+
+#ifdef _MSC_VER
+  #pragma warning( pop )
+#endif
diff --git a/extern/Eigen2/Eigen/src/Core/util/ForwardDeclarations.h b/extern/Eigen2/Eigen/src/Core/util/ForwardDeclarations.h
new file mode 100644
index 00000000000..a72a40b1bfc
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/ForwardDeclarations.h
@@ -0,0 +1,125 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_FORWARDDECLARATIONS_H
+#define EIGEN_FORWARDDECLARATIONS_H
+
+template<typename T> struct ei_traits;
+template<typename T> struct NumTraits;
+
+template<typename _Scalar, int _Rows, int _Cols,
+         int _Options = EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION | AutoAlign,
+         int _MaxRows = _Rows, int _MaxCols = _Cols> class Matrix;
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed> class Flagged;
+template<typename ExpressionType> class NestByValue;
+template<typename ExpressionType> class SwapWrapper;
+template<typename MatrixType> class Minor;
+template<typename MatrixType, int BlockRows=Dynamic, int BlockCols=Dynamic, int PacketAccess=AsRequested,
+         int _DirectAccessStatus = ei_traits<MatrixType>::Flags&DirectAccessBit ? DirectAccessBit
+                                 : ei_traits<MatrixType>::Flags&SparseBit> class Block;
+template<typename MatrixType> class Transpose;
+template<typename MatrixType> class Conjugate;
+template<typename NullaryOp, typename MatrixType>         class CwiseNullaryOp;
+template<typename UnaryOp,   typename MatrixType>         class CwiseUnaryOp;
+template<typename BinaryOp,  typename Lhs, typename Rhs>  class CwiseBinaryOp;
+template<typename Lhs, typename Rhs, int ProductMode> class Product;
+template<typename CoeffsVectorType> class DiagonalMatrix;
+template<typename MatrixType> class DiagonalCoeffs;
+template<typename MatrixType, int PacketAccess = AsRequested> class Map;
+template<typename MatrixType, unsigned int Mode> class Part;
+template<typename MatrixType, unsigned int Mode> class Extract;
+template<typename ExpressionType> class Cwise;
+template<typename ExpressionType> class WithFormat;
+template<typename MatrixType> struct CommaInitializer;
+
+
+template<typename Lhs, typename Rhs> struct ei_product_mode;
+template<typename Lhs, typename Rhs, int ProductMode = ei_product_mode<Lhs,Rhs>::value> struct ProductReturnType;
+
+template<typename Scalar> struct ei_scalar_sum_op;
+template<typename Scalar> struct ei_scalar_difference_op;
+template<typename Scalar> struct ei_scalar_product_op;
+template<typename Scalar> struct ei_scalar_quotient_op;
+template<typename Scalar> struct ei_scalar_opposite_op;
+template<typename Scalar> struct ei_scalar_conjugate_op;
+template<typename Scalar> struct ei_scalar_real_op;
+template<typename Scalar> struct ei_scalar_imag_op;
+template<typename Scalar> struct ei_scalar_abs_op;
+template<typename Scalar> struct ei_scalar_abs2_op;
+template<typename Scalar> struct ei_scalar_sqrt_op;
+template<typename Scalar> struct ei_scalar_exp_op;
+template<typename Scalar> struct ei_scalar_log_op;
+template<typename Scalar> struct ei_scalar_cos_op;
+template<typename Scalar> struct ei_scalar_sin_op;
+template<typename Scalar> struct ei_scalar_pow_op;
+template<typename Scalar> struct ei_scalar_inverse_op;
+template<typename Scalar> struct ei_scalar_square_op;
+template<typename Scalar> struct ei_scalar_cube_op;
+template<typename Scalar, typename NewType> struct ei_scalar_cast_op;
+template<typename Scalar> struct ei_scalar_multiple_op;
+template<typename Scalar> struct ei_scalar_quotient1_op;
+template<typename Scalar> struct ei_scalar_min_op;
+template<typename Scalar> struct ei_scalar_max_op;
+template<typename Scalar> struct ei_scalar_random_op;
+template<typename Scalar> struct ei_scalar_add_op;
+template<typename Scalar> struct ei_scalar_constant_op;
+template<typename Scalar> struct ei_scalar_identity_op;
+
+struct IOFormat;
+
+template<typename Scalar>
+void ei_cache_friendly_product(
+  int _rows, int _cols, int depth,
+  bool _lhsRowMajor, const Scalar* _lhs, int _lhsStride,
+  bool _rhsRowMajor, const Scalar* _rhs, int _rhsStride,
+  bool resRowMajor, Scalar* res, int resStride);
+
+// Array module
+template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType> class Select;
+template<typename MatrixType, typename BinaryOp, int Direction> class PartialReduxExpr;
+template<typename ExpressionType, int Direction> class PartialRedux;
+
+template<typename MatrixType> class LU;
+template<typename MatrixType> class QR;
+template<typename MatrixType> class SVD;
+template<typename MatrixType> class LLT;
+template<typename MatrixType> class LDLT;
+
+// Geometry module:
+template<typename Derived, int _Dim> class RotationBase;
+template<typename Lhs, typename Rhs> class Cross;
+template<typename Scalar> class Quaternion;
+template<typename Scalar> class Rotation2D;
+template<typename Scalar> class AngleAxis;
+template<typename Scalar,int Dim> class Transform;
+template <typename _Scalar, int _AmbientDim> class ParametrizedLine;
+template <typename _Scalar, int _AmbientDim> class Hyperplane;
+template<typename Scalar,int Dim> class Translation;
+template<typename Scalar,int Dim> class Scaling;
+
+// Sparse module:
+template<typename Lhs, typename Rhs, int ProductMode> class SparseProduct;
+
+#endif // EIGEN_FORWARDDECLARATIONS_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/Macros.h b/extern/Eigen2/Eigen/src/Core/util/Macros.h
new file mode 100644
index 00000000000..6be6f096055
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/Macros.h
@@ -0,0 +1,273 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MACROS_H
+#define EIGEN_MACROS_H
+
+#undef minor
+
+#define EIGEN_WORLD_VERSION 2
+#define EIGEN_MAJOR_VERSION 0
+#define EIGEN_MINOR_VERSION 6
+
+#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
+                                      (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
+                                                                 EIGEN_MINOR_VERSION>=z))))
+
+// 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable 16 byte alignment on all
+// platforms where vectorization might be enabled. In theory we could always enable alignment, but it can be a cause of problems
+// on some platforms, so we just disable it in certain common platform (compiler+architecture combinations) to avoid these problems.
+#if defined(__GNUC__) && !(defined(__i386__) || defined(__x86_64__) || defined(__powerpc__) || defined(__ia64__))
+#define EIGEN_GCC_AND_ARCH_DOESNT_WANT_ALIGNMENT 1
+#else
+#define EIGEN_GCC_AND_ARCH_DOESNT_WANT_ALIGNMENT 0
+#endif
+
+#if defined(__GNUC__) && (__GNUC__ <= 3)
+#define EIGEN_GCC3_OR_OLDER 1
+#else
+#define EIGEN_GCC3_OR_OLDER 0
+#endif
+
+// FIXME vectorization + alignment is completely disabled with sun studio
+#if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_ALIGNMENT && !EIGEN_GCC3_OR_OLDER && !defined(__SUNPRO_CC)
+  #define EIGEN_ARCH_WANTS_ALIGNMENT 1
+#else
+  #define EIGEN_ARCH_WANTS_ALIGNMENT 0
+#endif
+
+// EIGEN_ALIGN is the true test whether we want to align or not. It takes into account both the user choice to explicitly disable
+// alignment (EIGEN_DONT_ALIGN) and the architecture config (EIGEN_ARCH_WANTS_ALIGNMENT). Henceforth, only EIGEN_ALIGN should be used.
+#if EIGEN_ARCH_WANTS_ALIGNMENT && !defined(EIGEN_DONT_ALIGN)
+  #define EIGEN_ALIGN 1
+#else
+  #define EIGEN_ALIGN 0
+  #ifdef EIGEN_VECTORIZE
+    #error "Vectorization enabled, but our platform checks say that we don't do 16 byte alignment on this platform. If you added vectorization for another architecture, you also need to edit this platform check."
+  #endif
+  #ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+    #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+  #endif
+#endif
+
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION RowMajor
+#else
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION ColMajor
+#endif
+
+/** \internal  Defines the maximal loop size to enable meta unrolling of loops.
+  *            Note that the value here is expressed in Eigen's own notion of "number of FLOPS",
+  *            it does not correspond to the number of iterations or the number of instructions
+  */
+#ifndef EIGEN_UNROLLING_LIMIT
+#define EIGEN_UNROLLING_LIMIT 100
+#endif
+
+/** \internal Define the maximal size in Bytes of blocks fitting in CPU cache.
+  * The current value is set to generate blocks of 256x256 for float
+  *
+  * Typically for a single-threaded application you would set that to 25% of the size of your CPU caches in bytes
+  */
+#ifndef EIGEN_TUNE_FOR_CPU_CACHE_SIZE
+#define EIGEN_TUNE_FOR_CPU_CACHE_SIZE (sizeof(float)*256*256)
+#endif
+
+// FIXME this should go away quickly
+#ifdef EIGEN_TUNE_FOR_L2_CACHE_SIZE
+#error EIGEN_TUNE_FOR_L2_CACHE_SIZE is now called EIGEN_TUNE_FOR_CPU_CACHE_SIZE.
+#endif
+
+#define USING_PART_OF_NAMESPACE_EIGEN \
+EIGEN_USING_MATRIX_TYPEDEFS \
+using Eigen::Matrix; \
+using Eigen::MatrixBase; \
+using Eigen::ei_random; \
+using Eigen::ei_real; \
+using Eigen::ei_imag; \
+using Eigen::ei_conj; \
+using Eigen::ei_abs; \
+using Eigen::ei_abs2; \
+using Eigen::ei_sqrt; \
+using Eigen::ei_exp; \
+using Eigen::ei_log; \
+using Eigen::ei_sin; \
+using Eigen::ei_cos;
+
+#ifdef NDEBUG
+# ifndef EIGEN_NO_DEBUG
+#  define EIGEN_NO_DEBUG
+# endif
+#endif
+
+#ifndef ei_assert
+#ifdef EIGEN_NO_DEBUG
+#define ei_assert(x)
+#else
+#define ei_assert(x) assert(x)
+#endif
+#endif
+
+#ifdef EIGEN_INTERNAL_DEBUGGING
+#define ei_internal_assert(x) ei_assert(x)
+#else
+#define ei_internal_assert(x)
+#endif
+
+#ifdef EIGEN_NO_DEBUG
+#define EIGEN_ONLY_USED_FOR_DEBUG(x) (void)x
+#else
+#define EIGEN_ONLY_USED_FOR_DEBUG(x)
+#endif
+
+// EIGEN_ALWAYS_INLINE_ATTRIB should be use in the declaration of function
+// which should be inlined even in debug mode.
+// FIXME with the always_inline attribute,
+// gcc 3.4.x reports the following compilation error:
+//   Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval<Derived> Eigen::MatrixBase<Scalar, Derived>::eval() const'
+//    : function body not available
+#if EIGEN_GNUC_AT_LEAST(4,0)
+#define EIGEN_ALWAYS_INLINE_ATTRIB __attribute__((always_inline))
+#else
+#define EIGEN_ALWAYS_INLINE_ATTRIB
+#endif
+
+// EIGEN_FORCE_INLINE means "inline as much as possible"
+#if (defined _MSC_VER)
+#define EIGEN_STRONG_INLINE __forceinline
+#else
+#define EIGEN_STRONG_INLINE inline
+#endif
+
+#if (defined __GNUC__)
+#define EIGEN_DONT_INLINE __attribute__((noinline))
+#elif (defined _MSC_VER)
+#define EIGEN_DONT_INLINE __declspec(noinline)
+#else
+#define EIGEN_DONT_INLINE
+#endif
+
+#if (defined __GNUC__)
+#define EIGEN_DEPRECATED __attribute__((deprecated))
+#elif (defined _MSC_VER)
+#define EIGEN_DEPRECATED __declspec(deprecated)
+#else
+#define EIGEN_DEPRECATED
+#endif
+
+/* EIGEN_ALIGN_128 forces data to be 16-byte aligned, EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
+ * so that vectorization doesn't affect binary compatibility.
+ *
+ * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
+ * vectorized and non-vectorized code.
+ */
+#if !EIGEN_ALIGN
+#define EIGEN_ALIGN_128
+#elif (defined __GNUC__)
+#define EIGEN_ALIGN_128 __attribute__((aligned(16)))
+#elif (defined _MSC_VER)
+#define EIGEN_ALIGN_128 __declspec(align(16))
+#else
+#error Please tell me what is the equivalent of __attribute__((aligned(16))) for your compiler
+#endif
+
+#ifdef EIGEN_DONT_USE_RESTRICT_KEYWORD
+  #define EIGEN_RESTRICT
+#endif
+#ifndef EIGEN_RESTRICT
+  #define EIGEN_RESTRICT __restrict
+#endif
+
+#ifndef EIGEN_STACK_ALLOCATION_LIMIT
+#define EIGEN_STACK_ALLOCATION_LIMIT 1000000
+#endif
+
+#ifndef EIGEN_DEFAULT_IO_FORMAT
+#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat()
+#endif
+
+// format used in Eigen's documentation
+// needed to define it here as escaping characters in CMake add_definition's argument seems very problematic.
+#define EIGEN_DOCS_IO_FORMAT IOFormat(3, AlignCols, " ", "\n", "", "")
+
+#define EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename OtherDerived> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::MatrixBase<OtherDerived>& other) \
+{ \
+  return Base::operator Op(other.derived()); \
+} \
+EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \
+{ \
+  return Base::operator Op(other); \
+}
+
+#define EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename Other> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \
+{ \
+  return Base::operator Op(scalar); \
+}
+
+#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) \
+EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, +=) \
+EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Derived, -=) \
+EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, *=) \
+EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, /=)
+
+#define _EIGEN_GENERIC_PUBLIC_INTERFACE(Derived, BaseClass) \
+typedef BaseClass Base; \
+typedef typename Eigen::ei_traits<Derived>::Scalar Scalar; \
+typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; \
+typedef typename Base::PacketScalar PacketScalar; \
+typedef typename Eigen::ei_nested<Derived>::type Nested; \
+enum { RowsAtCompileTime = Eigen::ei_traits<Derived>::RowsAtCompileTime, \
+       ColsAtCompileTime = Eigen::ei_traits<Derived>::ColsAtCompileTime, \
+       MaxRowsAtCompileTime = Eigen::ei_traits<Derived>::MaxRowsAtCompileTime, \
+       MaxColsAtCompileTime = Eigen::ei_traits<Derived>::MaxColsAtCompileTime, \
+       Flags = Eigen::ei_traits<Derived>::Flags, \
+       CoeffReadCost = Eigen::ei_traits<Derived>::CoeffReadCost, \
+       SizeAtCompileTime = Base::SizeAtCompileTime, \
+       MaxSizeAtCompileTime = Base::MaxSizeAtCompileTime, \
+       IsVectorAtCompileTime = Base::IsVectorAtCompileTime };
+
+#define EIGEN_GENERIC_PUBLIC_INTERFACE(Derived) \
+_EIGEN_GENERIC_PUBLIC_INTERFACE(Derived, Eigen::MatrixBase<Derived>)
+
+#define EIGEN_ENUM_MIN(a,b) (((int)a <= (int)b) ? (int)a : (int)b)
+#define EIGEN_ENUM_MAX(a,b) (((int)a >= (int)b) ? (int)a : (int)b)
+
+// just an empty macro !
+#define EIGEN_EMPTY
+
+// concatenate two tokens
+#define EIGEN_CAT2(a,b) a ## b
+#define EIGEN_CAT(a,b) EIGEN_CAT2(a,b)
+
+// convert a token to a string
+#define EIGEN_MAKESTRING2(a) #a
+#define EIGEN_MAKESTRING(a) EIGEN_MAKESTRING2(a)
+
+#endif // EIGEN_MACROS_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/Memory.h b/extern/Eigen2/Eigen/src/Core/util/Memory.h
new file mode 100644
index 00000000000..09ad39d5be9
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/Memory.h
@@ -0,0 +1,368 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2009 Kenneth Riddile <kfriddile@yahoo.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MEMORY_H
+#define EIGEN_MEMORY_H
+
+#if defined(__APPLE__) || defined(_WIN64)
+  #define EIGEN_MALLOC_ALREADY_ALIGNED 1
+#else
+  #define EIGEN_MALLOC_ALREADY_ALIGNED 0
+#endif
+
+#if ((defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0)
+  #define EIGEN_HAS_POSIX_MEMALIGN 1
+#else
+  #define EIGEN_HAS_POSIX_MEMALIGN 0
+#endif
+
+#ifdef EIGEN_VECTORIZE_SSE
+  #define EIGEN_HAS_MM_MALLOC 1
+#else
+  #define EIGEN_HAS_MM_MALLOC 0
+#endif
+
+/** \internal like malloc, but the returned pointer is guaranteed to be 16-byte aligned.
+  * Fast, but wastes 16 additional bytes of memory.
+  * Does not throw any exception.
+  */
+inline void* ei_handmade_aligned_malloc(size_t size)
+{
+  void *original = malloc(size+16);
+  void *aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(original) & ~(size_t(15))) + 16);
+  *(reinterpret_cast<void**>(aligned) - 1) = original;
+  return aligned;
+}
+
+/** \internal frees memory allocated with ei_handmade_aligned_malloc */
+inline void ei_handmade_aligned_free(void *ptr)
+{
+  if(ptr)
+    free(*(reinterpret_cast<void**>(ptr) - 1));
+}
+
+/** \internal allocates \a size bytes. The returned pointer is guaranteed to have 16 bytes alignment.
+  * On allocation error, the returned pointer is undefined, but if exceptions are enabled then a std::bad_alloc is thrown.
+  */
+inline void* ei_aligned_malloc(size_t size)
+{
+  #ifdef EIGEN_NO_MALLOC
+    ei_assert(false && "heap allocation is forbidden (EIGEN_NO_MALLOC is defined)");
+  #endif
+
+  void *result;  
+  #if !EIGEN_ALIGN
+    result = malloc(size);
+  #elif EIGEN_MALLOC_ALREADY_ALIGNED
+    result = malloc(size);
+  #elif EIGEN_HAS_POSIX_MEMALIGN
+    if(posix_memalign(&result, 16, size)) result = 0;
+  #elif EIGEN_HAS_MM_MALLOC
+    result = _mm_malloc(size, 16);
+  #elif (defined _MSC_VER)
+    result = _aligned_malloc(size, 16);
+  #else
+    result = ei_handmade_aligned_malloc(size);
+  #endif
+    
+  #ifdef EIGEN_EXCEPTIONS
+    if(result == 0)
+      throw std::bad_alloc();
+  #endif
+  return result;
+}
+
+/** allocates \a size bytes. If Align is true, then the returned ptr is 16-byte-aligned.
+  * On allocation error, the returned pointer is undefined, but if exceptions are enabled then a std::bad_alloc is thrown.
+  */
+template<bool Align> inline void* ei_conditional_aligned_malloc(size_t size)
+{
+  return ei_aligned_malloc(size);
+}
+
+template<> inline void* ei_conditional_aligned_malloc<false>(size_t size)
+{
+  #ifdef EIGEN_NO_MALLOC
+    ei_assert(false && "heap allocation is forbidden (EIGEN_NO_MALLOC is defined)");
+  #endif
+
+  void *result = malloc(size);
+  #ifdef EIGEN_EXCEPTIONS
+    if(!result) throw std::bad_alloc();
+  #endif
+  return result;
+}
+
+/** allocates \a size objects of type T. The returned pointer is guaranteed to have 16 bytes alignment.
+  * On allocation error, the returned pointer is undefined, but if exceptions are enabled then a std::bad_alloc is thrown.
+  * The default constructor of T is called.
+  */
+template<typename T> inline T* ei_aligned_new(size_t size)
+{
+  void *void_result = ei_aligned_malloc(sizeof(T)*size);
+  return ::new(void_result) T[size];
+}
+
+template<typename T, bool Align> inline T* ei_conditional_aligned_new(size_t size)
+{
+  void *void_result = ei_conditional_aligned_malloc<Align>(sizeof(T)*size);
+  return ::new(void_result) T[size];
+}
+
+/** \internal free memory allocated with ei_aligned_malloc
+  */
+inline void ei_aligned_free(void *ptr)
+{
+  #if !EIGEN_ALIGN
+    free(ptr);
+  #elif EIGEN_MALLOC_ALREADY_ALIGNED
+    free(ptr);
+  #elif EIGEN_HAS_POSIX_MEMALIGN
+    free(ptr);
+  #elif EIGEN_HAS_MM_MALLOC
+    _mm_free(ptr);
+  #elif defined(_MSC_VER)
+    _aligned_free(ptr);
+  #else
+    ei_handmade_aligned_free(ptr);
+  #endif
+}
+
+/** \internal free memory allocated with ei_conditional_aligned_malloc
+  */
+template<bool Align> inline void ei_conditional_aligned_free(void *ptr)
+{
+  ei_aligned_free(ptr);
+}
+
+template<> inline void ei_conditional_aligned_free<false>(void *ptr)
+{
+  free(ptr);
+}
+
+/** \internal delete the elements of an array.
+  * The \a size parameters tells on how many objects to call the destructor of T.
+  */
+template<typename T> inline void ei_delete_elements_of_array(T *ptr, size_t size)
+{
+  // always destruct an array starting from the end.
+  while(size) ptr[--size].~T();
+}
+
+/** \internal delete objects constructed with ei_aligned_new
+  * The \a size parameters tells on how many objects to call the destructor of T.
+  */
+template<typename T> inline void ei_aligned_delete(T *ptr, size_t size)
+{
+  ei_delete_elements_of_array<T>(ptr, size);
+  ei_aligned_free(ptr);
+}
+
+/** \internal delete objects constructed with ei_conditional_aligned_new
+  * The \a size parameters tells on how many objects to call the destructor of T.
+  */
+template<typename T, bool Align> inline void ei_conditional_aligned_delete(T *ptr, size_t size)
+{
+  ei_delete_elements_of_array<T>(ptr, size);
+  ei_conditional_aligned_free<Align>(ptr);
+}
+
+/** \internal \returns the number of elements which have to be skipped such that data are 16 bytes aligned */
+template<typename Scalar>
+inline static int ei_alignmentOffset(const Scalar* ptr, int maxOffset)
+{
+  typedef typename ei_packet_traits<Scalar>::type Packet;
+  const int PacketSize = ei_packet_traits<Scalar>::size;
+  const int PacketAlignedMask = PacketSize-1;
+  const bool Vectorized = PacketSize>1;
+  return Vectorized
+          ? std::min<int>( (PacketSize - (int((size_t(ptr)/sizeof(Scalar))) & PacketAlignedMask))
+                           & PacketAlignedMask, maxOffset)
+          : 0;
+}
+
+/** \internal
+  * ei_aligned_stack_alloc(SIZE) allocates an aligned buffer of SIZE bytes
+  * on the stack if SIZE is smaller than EIGEN_STACK_ALLOCATION_LIMIT.
+  * Otherwise the memory is allocated on the heap.
+  * Data allocated with ei_aligned_stack_alloc \b must be freed by calling ei_aligned_stack_free(PTR,SIZE).
+  * \code
+  * float * data = ei_aligned_stack_alloc(float,array.size());
+  * // ...
+  * ei_aligned_stack_free(data,float,array.size());
+  * \endcode
+  */
+#ifdef __linux__
+  #define ei_aligned_stack_alloc(SIZE) (SIZE<=EIGEN_STACK_ALLOCATION_LIMIT) \
+                                    ? alloca(SIZE) \
+                                    : ei_aligned_malloc(SIZE)
+  #define ei_aligned_stack_free(PTR,SIZE) if(SIZE>EIGEN_STACK_ALLOCATION_LIMIT) ei_aligned_free(PTR)
+#else
+  #define ei_aligned_stack_alloc(SIZE) ei_aligned_malloc(SIZE)
+  #define ei_aligned_stack_free(PTR,SIZE) ei_aligned_free(PTR)
+#endif
+
+#define ei_aligned_stack_new(TYPE,SIZE) ::new(ei_aligned_stack_alloc(sizeof(TYPE)*SIZE)) TYPE[SIZE]
+#define ei_aligned_stack_delete(TYPE,PTR,SIZE) do {ei_delete_elements_of_array<TYPE>(PTR, SIZE); \
+                                                   ei_aligned_stack_free(PTR,sizeof(TYPE)*SIZE);} while(0)
+
+
+#if EIGEN_ALIGN
+  #ifdef EIGEN_EXCEPTIONS
+    #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
+      void* operator new(size_t size, const std::nothrow_t&) throw() { \
+        try { return Eigen::ei_conditional_aligned_malloc<NeedsToAlign>(size); } \
+        catch (...) { return 0; } \
+        return 0; \
+      }
+  #else
+    #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
+      void* operator new(size_t size, const std::nothrow_t&) throw() { \
+        return Eigen::ei_conditional_aligned_malloc<NeedsToAlign>(size); \
+      }
+  #endif
+  
+  #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+      void *operator new(size_t size) { \
+        return Eigen::ei_conditional_aligned_malloc<NeedsToAlign>(size); \
+      } \
+      void *operator new[](size_t size) { \
+        return Eigen::ei_conditional_aligned_malloc<NeedsToAlign>(size); \
+      } \
+      void operator delete(void * ptr) throw() { Eigen::ei_conditional_aligned_free<NeedsToAlign>(ptr); } \
+      void operator delete[](void * ptr) throw() { Eigen::ei_conditional_aligned_free<NeedsToAlign>(ptr); } \
+      /* in-place new and delete. since (at least afaik) there is no actual   */ \
+      /* memory allocated we can safely let the default implementation handle */ \
+      /* this particular case. */ \
+      static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \
+      void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \
+      /* nothrow-new (returns zero instead of std::bad_alloc) */ \
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
+      void operator delete(void *ptr, const std::nothrow_t&) throw() { \
+        Eigen::ei_conditional_aligned_free<NeedsToAlign>(ptr); \
+      } \
+      typedef void ei_operator_new_marker_type;
+#else
+  #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+#endif
+
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size) \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(((Size)!=Eigen::Dynamic) && ((sizeof(Scalar)*(Size))%16==0))
+
+
+/** \class aligned_allocator
+*
+* \brief stl compatible allocator to use with with 16 byte aligned types
+*
+* Example:
+* \code
+* // Matrix4f requires 16 bytes alignment:
+* std::map< int, Matrix4f, std::less<int>, aligned_allocator<Matrix4f> > my_map_mat4;
+* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
+* std::map< int, Vector3f > my_map_vec3;
+* \endcode
+*
+*/
+template<class T>
+class aligned_allocator
+{
+public:
+    typedef size_t    size_type;
+    typedef ptrdiff_t difference_type;
+    typedef T*        pointer;
+    typedef const T*  const_pointer;
+    typedef T&        reference;
+    typedef const T&  const_reference;
+    typedef T         value_type;
+
+    template<class U>
+    struct rebind
+    {
+        typedef aligned_allocator<U> other;
+    };
+
+    pointer address( reference value ) const
+    {
+        return &value;
+    }
+
+    const_pointer address( const_reference value ) const
+    {
+        return &value;
+    }
+
+    aligned_allocator() throw()
+    {
+    }
+
+    aligned_allocator( const aligned_allocator& ) throw()
+    {
+    }
+
+    template<class U>
+    aligned_allocator( const aligned_allocator<U>& ) throw()
+    {
+    }
+
+    ~aligned_allocator() throw()
+    {
+    }
+
+    size_type max_size() const throw()
+    {
+        return std::numeric_limits<size_type>::max();
+    }
+
+    pointer allocate( size_type num, const_pointer* hint = 0 )
+    {
+        static_cast<void>( hint ); // suppress unused variable warning
+        return static_cast<pointer>( ei_aligned_malloc( num * sizeof(T) ) );
+    }
+
+    void construct( pointer p, const T& value )
+    {
+        ::new( p ) T( value );
+    }
+
+    void destroy( pointer p )
+    {
+        p->~T();
+    }
+
+    void deallocate( pointer p, size_type /*num*/ )
+    {
+        ei_aligned_free( p );
+    }
+    
+    bool operator!=(const aligned_allocator<T>& other) const
+    { return false; }
+    
+    bool operator==(const aligned_allocator<T>& other) const
+    { return true; }
+};
+
+#endif // EIGEN_MEMORY_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/Meta.h b/extern/Eigen2/Eigen/src/Core/util/Meta.h
new file mode 100644
index 00000000000..c65c52ef42f
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/Meta.h
@@ -0,0 +1,183 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_META_H
+#define EIGEN_META_H
+
+/** \internal
+  * \file Meta.h
+  * This file contains generic metaprogramming classes which are not specifically related to Eigen.
+  * \note In case you wonder, yes we're aware that Boost already provides all these features,
+  * we however don't want to add a dependency to Boost.
+  */
+
+struct ei_meta_true {  enum { ret = 1 }; };
+struct ei_meta_false { enum { ret = 0 }; };
+
+template<bool Condition, typename Then, typename Else>
+struct ei_meta_if { typedef Then ret; };
+
+template<typename Then, typename Else>
+struct ei_meta_if <false, Then, Else> { typedef Else ret; };
+
+template<typename T, typename U> struct ei_is_same_type { enum { ret = 0 }; };
+template<typename T> struct ei_is_same_type<T,T> { enum { ret = 1 }; };
+
+template<typename T> struct ei_unref { typedef T type; };
+template<typename T> struct ei_unref<T&> { typedef T type; };
+
+template<typename T> struct ei_unpointer { typedef T type; };
+template<typename T> struct ei_unpointer<T*> { typedef T type; };
+template<typename T> struct ei_unpointer<T*const> { typedef T type; };
+
+template<typename T> struct ei_unconst { typedef T type; };
+template<typename T> struct ei_unconst<const T> { typedef T type; };
+template<typename T> struct ei_unconst<T const &> { typedef T & type; };
+template<typename T> struct ei_unconst<T const *> { typedef T * type; };
+
+template<typename T> struct ei_cleantype { typedef T type; };
+template<typename T> struct ei_cleantype<const T>   { typedef typename ei_cleantype<T>::type type; };
+template<typename T> struct ei_cleantype<const T&>  { typedef typename ei_cleantype<T>::type type; };
+template<typename T> struct ei_cleantype<T&>        { typedef typename ei_cleantype<T>::type type; };
+template<typename T> struct ei_cleantype<const T*>  { typedef typename ei_cleantype<T>::type type; };
+template<typename T> struct ei_cleantype<T*>        { typedef typename ei_cleantype<T>::type type; };
+
+/** \internal
+  * Convenient struct to get the result type of a unary or binary functor.
+  *
+  * It supports both the current STL mechanism (using the result_type member) as well as
+  * upcoming next STL generation (using a templated result member).
+  * If none of these members is provided, then the type of the first argument is returned. FIXME, that behavior is a pretty bad hack.
+  */
+template<typename T> struct ei_result_of {};
+
+struct ei_has_none {int a[1];};
+struct ei_has_std_result_type {int a[2];};
+struct ei_has_tr1_result {int a[3];};
+
+template<typename Func, typename ArgType, int SizeOf=sizeof(ei_has_none)>
+struct ei_unary_result_of_select {typedef ArgType type;};
+
+template<typename Func, typename ArgType>
+struct ei_unary_result_of_select<Func, ArgType, sizeof(ei_has_std_result_type)> {typedef typename Func::result_type type;};
+
+template<typename Func, typename ArgType>
+struct ei_unary_result_of_select<Func, ArgType, sizeof(ei_has_tr1_result)> {typedef typename Func::template result<Func(ArgType)>::type type;};
+
+template<typename Func, typename ArgType>
+struct ei_result_of<Func(ArgType)> {
+    template<typename T>
+    static ei_has_std_result_type testFunctor(T const *, typename T::result_type const * = 0);
+    template<typename T>
+    static ei_has_tr1_result      testFunctor(T const *, typename T::template result<T(ArgType)>::type const * = 0);
+    static ei_has_none            testFunctor(...);
+
+    // note that the following indirection is needed for gcc-3.3
+    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
+    typedef typename ei_unary_result_of_select<Func, ArgType, FunctorType>::type type;
+};
+
+template<typename Func, typename ArgType0, typename ArgType1, int SizeOf=sizeof(ei_has_none)>
+struct ei_binary_result_of_select {typedef ArgType0 type;};
+
+template<typename Func, typename ArgType0, typename ArgType1>
+struct ei_binary_result_of_select<Func, ArgType0, ArgType1, sizeof(ei_has_std_result_type)>
+{typedef typename Func::result_type type;};
+
+template<typename Func, typename ArgType0, typename ArgType1>
+struct ei_binary_result_of_select<Func, ArgType0, ArgType1, sizeof(ei_has_tr1_result)>
+{typedef typename Func::template result<Func(ArgType0,ArgType1)>::type type;};
+
+template<typename Func, typename ArgType0, typename ArgType1>
+struct ei_result_of<Func(ArgType0,ArgType1)> {
+    template<typename T>
+    static ei_has_std_result_type testFunctor(T const *, typename T::result_type const * = 0);
+    template<typename T>
+    static ei_has_tr1_result      testFunctor(T const *, typename T::template result<T(ArgType0,ArgType1)>::type const * = 0);
+    static ei_has_none            testFunctor(...);
+
+    // note that the following indirection is needed for gcc-3.3
+    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
+    typedef typename ei_binary_result_of_select<Func, ArgType0, ArgType1, FunctorType>::type type;
+};
+
+/** \internal In short, it computes int(sqrt(\a Y)) with \a Y an integer.
+  * Usage example: \code ei_meta_sqrt<1023>::ret \endcode
+  */
+template<int Y,
+         int InfX = 0,
+         int SupX = ((Y==1) ? 1 : Y/2),
+         bool Done = ((SupX-InfX)<=1 ? true : ((SupX*SupX <= Y) && ((SupX+1)*(SupX+1) > Y))) >
+                                // use ?: instead of || just to shut up a stupid gcc 4.3 warning
+class ei_meta_sqrt
+{
+    enum {
+      MidX = (InfX+SupX)/2,
+      TakeInf = MidX*MidX > Y ? 1 : 0,
+      NewInf = int(TakeInf) ? InfX : int(MidX),
+      NewSup = int(TakeInf) ? int(MidX) : SupX
+    };
+  public:
+    enum { ret = ei_meta_sqrt<Y,NewInf,NewSup>::ret };
+};
+
+template<int Y, int InfX, int SupX>
+class ei_meta_sqrt<Y, InfX, SupX, true> { public:  enum { ret = (SupX*SupX <= Y) ? SupX : InfX }; };
+
+/** \internal determines whether the product of two numeric types is allowed and what the return type is */
+template<typename T, typename U> struct ei_scalar_product_traits
+{
+  // dummy general case where T and U aren't compatible -- not allowed anyway but we catch it elsewhere
+  //enum { Cost = NumTraits<T>::MulCost };
+  typedef T ReturnType;
+};
+
+template<typename T> struct ei_scalar_product_traits<T,T>
+{
+  //enum { Cost = NumTraits<T>::MulCost };
+  typedef T ReturnType;
+};
+
+template<typename T> struct ei_scalar_product_traits<T,std::complex<T> >
+{
+  //enum { Cost = 2*NumTraits<T>::MulCost };
+  typedef std::complex<T> ReturnType;
+};
+
+template<typename T> struct ei_scalar_product_traits<std::complex<T>, T>
+{
+  //enum { Cost = 2*NumTraits<T>::MulCost  };
+  typedef std::complex<T> ReturnType;
+};
+
+// FIXME quick workaround around current limitation of ei_result_of
+template<typename Scalar, typename ArgType0, typename ArgType1>
+struct ei_result_of<ei_scalar_product_op<Scalar>(ArgType0,ArgType1)> {
+typedef typename ei_scalar_product_traits<typename ei_cleantype<ArgType0>::type, typename ei_cleantype<ArgType1>::type>::ReturnType type;
+};
+
+
+
+#endif // EIGEN_META_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/StaticAssert.h b/extern/Eigen2/Eigen/src/Core/util/StaticAssert.h
new file mode 100644
index 00000000000..2c13098a20f
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/StaticAssert.h
@@ -0,0 +1,148 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_STATIC_ASSERT_H
+#define EIGEN_STATIC_ASSERT_H
+
+/* Some notes on Eigen's static assertion mechanism:
+ *
+ *  - in EIGEN_STATIC_ASSERT(CONDITION,MSG) the parameter CONDITION must be a compile time boolean
+ *    expression, and MSG an enum listed in struct ei_static_assert<true>
+ *
+ *  - define EIGEN_NO_STATIC_ASSERT to disable them (and save compilation time)
+ *    in that case, the static assertion is converted to the following runtime assert:
+ *      ei_assert(CONDITION && "MSG")
+ *
+ *  - currently EIGEN_STATIC_ASSERT can only be used in function scope
+ *
+ */
+
+#ifndef EIGEN_NO_STATIC_ASSERT
+
+  #ifdef __GXX_EXPERIMENTAL_CXX0X__
+
+    // if native static_assert is enabled, let's use it
+    #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
+
+  #else // CXX0X
+
+    template<bool condition>
+    struct ei_static_assert {};
+
+    template<>
+    struct ei_static_assert<true>
+    {
+      enum {
+        YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX,
+        YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES,
+        YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES,
+        THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE,
+        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE,
+        YOU_MADE_A_PROGRAMMING_MISTAKE,
+        YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR,
+        UNALIGNED_LOAD_AND_STORE_OPERATIONS_UNIMPLEMENTED_ON_ALTIVEC,
+        NUMERIC_TYPE_MUST_BE_FLOATING_POINT,
+        COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED,
+        WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED,
+        THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE,
+        INVALID_MATRIX_PRODUCT,
+        INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS,
+        INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION,
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY,
+        THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES,
+        THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES,
+        INVALID_MATRIX_TEMPLATE_PARAMETERS,
+        BOTH_MATRICES_MUST_HAVE_THE_SAME_STORAGE_ORDER,
+        THIS_METHOD_IS_ONLY_FOR_DIAGONAL_MATRIX
+      };
+    };
+
+    // Specialized implementation for MSVC to avoid "conditional
+    // expression is constant" warnings.  This implementation doesn't
+    // appear to work under GCC, hence the multiple implementations.
+    #ifdef _MSC_VER
+
+      #define EIGEN_STATIC_ASSERT(CONDITION,MSG) \
+        {Eigen::ei_static_assert<CONDITION ? true : false>::MSG;}
+
+    #else
+
+      #define EIGEN_STATIC_ASSERT(CONDITION,MSG) \
+        if (Eigen::ei_static_assert<CONDITION ? true : false>::MSG) {}
+
+    #endif
+
+  #endif // not CXX0X
+
+#else // EIGEN_NO_STATIC_ASSERT
+
+  #define EIGEN_STATIC_ASSERT(CONDITION,MSG) ei_assert((CONDITION) && #MSG);
+
+#endif // EIGEN_NO_STATIC_ASSERT
+
+
+// static assertion failing if the type \a TYPE is not a vector type
+#define EIGEN_STATIC_ASSERT_VECTOR_ONLY(TYPE) \
+  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime, \
+                      YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX)
+
+// static assertion failing if the type \a TYPE is not fixed-size
+#define EIGEN_STATIC_ASSERT_FIXED_SIZE(TYPE) \
+  EIGEN_STATIC_ASSERT(TYPE::SizeAtCompileTime!=Eigen::Dynamic, \
+                      YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
+
+// static assertion failing if the type \a TYPE is not a vector type of the given size
+#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE) \
+  EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime && TYPE::SizeAtCompileTime==SIZE, \
+                      THIS_METHOD_IS_ONLY_FOR_VECTORS_OF_A_SPECIFIC_SIZE)
+
+// static assertion failing if the type \a TYPE is not a vector type of the given size
+#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS) \
+  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime==ROWS && TYPE::ColsAtCompileTime==COLS, \
+                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
+
+// static assertion failing if the two vector expression types are not compatible (same fixed-size or dynamic size)
+#define EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TYPE0,TYPE1) \
+  EIGEN_STATIC_ASSERT( \
+      (int(TYPE0::SizeAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE1::SizeAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE0::SizeAtCompileTime)==int(TYPE1::SizeAtCompileTime)),\
+    YOU_MIXED_VECTORS_OF_DIFFERENT_SIZES)
+
+#define EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0,TYPE1) \
+      ((int(TYPE0::RowsAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE1::RowsAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE0::RowsAtCompileTime)==int(TYPE1::RowsAtCompileTime)) \
+   && (int(TYPE0::ColsAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE1::ColsAtCompileTime)==Eigen::Dynamic \
+    || int(TYPE0::ColsAtCompileTime)==int(TYPE1::ColsAtCompileTime)))
+
+// static assertion failing if it is guaranteed at compile-time that the two matrix expression types have different sizes
+#define EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(TYPE0,TYPE1) \
+  EIGEN_STATIC_ASSERT( \
+     EIGEN_PREDICATE_SAME_MATRIX_SIZE(TYPE0,TYPE1),\
+    YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES)
+
+#endif // EIGEN_STATIC_ASSERT_H
diff --git a/extern/Eigen2/Eigen/src/Core/util/XprHelper.h b/extern/Eigen2/Eigen/src/Core/util/XprHelper.h
new file mode 100644
index 00000000000..12d6f9a3a3e
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Core/util/XprHelper.h
@@ -0,0 +1,219 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_XPRHELPER_H
+#define EIGEN_XPRHELPER_H
+
+// just a workaround because GCC seems to not really like empty structs
+#ifdef __GNUG__
+  struct ei_empty_struct{char _ei_dummy_;};
+  #define EIGEN_EMPTY_STRUCT : Eigen::ei_empty_struct
+#else
+  #define EIGEN_EMPTY_STRUCT
+#endif
+
+//classes inheriting ei_no_assignment_operator don't generate a default operator=.
+class ei_no_assignment_operator
+{
+  private:
+    ei_no_assignment_operator& operator=(const ei_no_assignment_operator&);
+};
+
+/** \internal If the template parameter Value is Dynamic, this class is just a wrapper around an int variable that
+  * can be accessed using value() and setValue().
+  * Otherwise, this class is an empty structure and value() just returns the template parameter Value.
+  */
+template<int Value> class ei_int_if_dynamic EIGEN_EMPTY_STRUCT
+{
+  public:
+    ei_int_if_dynamic() {}
+    explicit ei_int_if_dynamic(int) {}
+    static int value() { return Value; }
+    void setValue(int) {}
+};
+
+template<> class ei_int_if_dynamic<Dynamic>
+{
+    int m_value;
+    ei_int_if_dynamic() {}
+  public:
+    explicit ei_int_if_dynamic(int value) : m_value(value) {}
+    int value() const { return m_value; }
+    void setValue(int value) { m_value = value; }
+};
+
+template<typename T> struct ei_functor_traits
+{
+  enum
+  {
+    Cost = 10,
+    PacketAccess = false
+  };
+};
+
+template<typename T> struct ei_packet_traits
+{
+  typedef T type;
+  enum {size=1};
+};
+
+template<typename T> struct ei_unpacket_traits
+{
+  typedef T type;
+  enum {size=1};
+};
+
+template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+class ei_compute_matrix_flags
+{
+    enum {
+      row_major_bit = Options&RowMajor ? RowMajorBit : 0,
+      inner_max_size = row_major_bit ? MaxCols : MaxRows,
+      is_big = inner_max_size == Dynamic,
+      is_packet_size_multiple = (Cols*Rows) % ei_packet_traits<Scalar>::size == 0,
+      aligned_bit = ((Options&AutoAlign) && (is_big || is_packet_size_multiple)) ? AlignedBit : 0,
+      packet_access_bit = ei_packet_traits<Scalar>::size > 1 && aligned_bit ? PacketAccessBit : 0
+    };
+
+  public:
+    enum { ret = LinearAccessBit | DirectAccessBit | packet_access_bit | row_major_bit | aligned_bit };
+};
+
+template<int _Rows, int _Cols> struct ei_size_at_compile_time
+{
+  enum { ret = (_Rows==Dynamic || _Cols==Dynamic) ? Dynamic : _Rows * _Cols };
+};
+
+/* ei_eval : the return type of eval(). For matrices, this is just a const reference
+ * in order to avoid a useless copy
+ */
+
+template<typename T, int Sparseness = ei_traits<T>::Flags&SparseBit> class ei_eval;
+
+template<typename T> struct ei_eval<T,IsDense>
+{
+  typedef Matrix<typename ei_traits<T>::Scalar,
+                ei_traits<T>::RowsAtCompileTime,
+                ei_traits<T>::ColsAtCompileTime,
+                AutoAlign | (ei_traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
+                ei_traits<T>::MaxRowsAtCompileTime,
+                ei_traits<T>::MaxColsAtCompileTime
+          > type;
+};
+
+// for matrices, no need to evaluate, just use a const reference to avoid a useless copy
+template<typename _Scalar, int _Rows, int _Cols, int _StorageOrder, int _MaxRows, int _MaxCols>
+struct ei_eval<Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols>, IsDense>
+{
+  typedef const Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols>& type;
+};
+
+/* ei_plain_matrix_type : the difference from ei_eval is that ei_plain_matrix_type is always a plain matrix type,
+ * whereas ei_eval is a const reference in the case of a matrix
+ */
+template<typename T> struct ei_plain_matrix_type
+{
+  typedef Matrix<typename ei_traits<T>::Scalar,
+                ei_traits<T>::RowsAtCompileTime,
+                ei_traits<T>::ColsAtCompileTime,
+                AutoAlign | (ei_traits<T>::Flags&RowMajorBit ? RowMajor : ColMajor),
+                ei_traits<T>::MaxRowsAtCompileTime,
+                ei_traits<T>::MaxColsAtCompileTime
+          > type;
+};
+
+/* ei_plain_matrix_type_column_major : same as ei_plain_matrix_type but guaranteed to be column-major
+ */
+template<typename T> struct ei_plain_matrix_type_column_major
+{
+  typedef Matrix<typename ei_traits<T>::Scalar,
+                ei_traits<T>::RowsAtCompileTime,
+                ei_traits<T>::ColsAtCompileTime,
+                AutoAlign | ColMajor,
+                ei_traits<T>::MaxRowsAtCompileTime,
+                ei_traits<T>::MaxColsAtCompileTime
+          > type;
+};
+
+template<typename T> struct ei_must_nest_by_value { enum { ret = false }; };
+template<typename T> struct ei_must_nest_by_value<NestByValue<T> > { enum { ret = true }; };
+
+/** \internal Determines how a given expression should be nested into another one.
+  * For example, when you do a * (b+c), Eigen will determine how the expression b+c should be
+  * nested into the bigger product expression. The choice is between nesting the expression b+c as-is, or
+  * evaluating that expression b+c into a temporary variable d, and nest d so that the resulting expression is
+  * a*d. Evaluating can be beneficial for example if every coefficient access in the resulting expression causes
+  * many coefficient accesses in the nested expressions -- as is the case with matrix product for example.
+  *
+  * \param T the type of the expression being nested
+  * \param n the number of coefficient accesses in the nested expression for each coefficient access in the bigger expression.
+  *
+  * Example. Suppose that a, b, and c are of type Matrix3d. The user forms the expression a*(b+c).
+  * b+c is an expression "sum of matrices", which we will denote by S. In order to determine how to nest it,
+  * the Product expression uses: ei_nested<S, 3>::ret, which turns out to be Matrix3d because the internal logic of
+  * ei_nested determined that in this case it was better to evaluate the expression b+c into a temporary. On the other hand,
+  * since a is of type Matrix3d, the Product expression nests it as ei_nested<Matrix3d, 3>::ret, which turns out to be
+  * const Matrix3d&, because the internal logic of ei_nested determined that since a was already a matrix, there was no point
+  * in copying it into another matrix.
+  */
+template<typename T, int n=1, typename PlainMatrixType = typename ei_eval<T>::type> struct ei_nested
+{
+  enum {
+    CostEval   = (n+1) * int(NumTraits<typename ei_traits<T>::Scalar>::ReadCost),
+    CostNoEval = (n-1) * int(ei_traits<T>::CoeffReadCost)
+  };
+  typedef typename ei_meta_if<
+    ei_must_nest_by_value<T>::ret,
+    T,
+    typename ei_meta_if<
+      (int(ei_traits<T>::Flags) & EvalBeforeNestingBit)
+      || ( int(CostEval) <= int(CostNoEval) ),
+      PlainMatrixType,
+      const T&
+    >::ret
+  >::ret type;
+};
+
+template<unsigned int Flags> struct ei_are_flags_consistent
+{
+  enum { ret = !( (Flags&UnitDiagBit && Flags&ZeroDiagBit) )
+  };
+};
+
+/** \internal Gives the type of a sub-matrix or sub-vector of a matrix of type \a ExpressionType and size \a Size
+  * TODO: could be a good idea to define a big ReturnType struct ??
+  */
+template<typename ExpressionType, int RowsOrSize=Dynamic, int Cols=Dynamic> struct BlockReturnType {
+  typedef Block<ExpressionType, (ei_traits<ExpressionType>::RowsAtCompileTime == 1 ? 1 : RowsOrSize),
+                                (ei_traits<ExpressionType>::ColsAtCompileTime == 1 ? 1 : RowsOrSize)> SubVectorType;
+  typedef Block<ExpressionType, RowsOrSize, Cols> Type;
+};
+
+template<typename CurrentType, typename NewType> struct ei_cast_return_type
+{
+  typedef typename ei_meta_if<ei_is_same_type<CurrentType,NewType>::ret,const CurrentType&,NewType>::ret type;
+};
+
+#endif // EIGEN_XPRHELPER_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/AlignedBox.h b/extern/Eigen2/Eigen/src/Geometry/AlignedBox.h
new file mode 100644
index 00000000000..14ec9261e3a
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/AlignedBox.h
@@ -0,0 +1,173 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ALIGNEDBOX_H
+#define EIGEN_ALIGNEDBOX_H
+
+/** \geometry_module \ingroup Geometry_Module
+  * \nonstableyet
+  *
+  * \class AlignedBox
+  *
+  * \brief An axis aligned box
+  *
+  * \param _Scalar the type of the scalar coefficients
+  * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
+  *
+  * This class represents an axis aligned box as a pair of the minimal and maximal corners.
+  */
+template <typename _Scalar, int _AmbientDim>
+class AlignedBox
+{
+public:
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
+  enum { AmbientDimAtCompileTime = _AmbientDim };
+  typedef _Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
+
+  /** Default constructor initializing a null box. */
+  inline explicit AlignedBox()
+  { if (AmbientDimAtCompileTime!=Dynamic) setNull(); }
+
+  /** Constructs a null box with \a _dim the dimension of the ambient space. */
+  inline explicit AlignedBox(int _dim) : m_min(_dim), m_max(_dim)
+  { setNull(); }
+
+  /** Constructs a box with extremities \a _min and \a _max. */
+  inline AlignedBox(const VectorType& _min, const VectorType& _max) : m_min(_min), m_max(_max) {}
+
+  /** Constructs a box containing a single point \a p. */
+  inline explicit AlignedBox(const VectorType& p) : m_min(p), m_max(p) {}
+
+  ~AlignedBox() {}
+
+  /** \returns the dimension in which the box holds */
+  inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size()-1 : AmbientDimAtCompileTime; }
+
+  /** \returns true if the box is null, i.e, empty. */
+  inline bool isNull() const { return (m_min.cwise() > m_max).any(); }
+
+  /** Makes \c *this a null/empty box. */
+  inline void setNull()
+  {
+    m_min.setConstant( std::numeric_limits<Scalar>::max());
+    m_max.setConstant(-std::numeric_limits<Scalar>::max());
+  }
+
+  /** \returns the minimal corner */
+  inline const VectorType& min() const { return m_min; }
+  /** \returns a non const reference to the minimal corner */
+  inline VectorType& min() { return m_min; }
+  /** \returns the maximal corner */
+  inline const VectorType& max() const { return m_max; }
+  /** \returns a non const reference to the maximal corner */
+  inline VectorType& max() { return m_max; }
+
+  /** \returns true if the point \a p is inside the box \c *this. */
+  inline bool contains(const VectorType& p) const
+  { return (m_min.cwise()<=p).all() && (p.cwise()<=m_max).all(); }
+
+  /** \returns true if the box \a b is entirely inside the box \c *this. */
+  inline bool contains(const AlignedBox& b) const
+  { return (m_min.cwise()<=b.min()).all() && (b.max().cwise()<=m_max).all(); }
+
+  /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */
+  inline AlignedBox& extend(const VectorType& p)
+  { m_min = m_min.cwise().min(p); m_max = m_max.cwise().max(p); return *this; }
+
+  /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */
+  inline AlignedBox& extend(const AlignedBox& b)
+  { m_min = m_min.cwise().min(b.m_min); m_max = m_max.cwise().max(b.m_max); return *this; }
+
+  /** Clamps \c *this by the box \a b and returns a reference to \c *this. */
+  inline AlignedBox& clamp(const AlignedBox& b)
+  { m_min = m_min.cwise().max(b.m_min); m_max = m_max.cwise().min(b.m_max); return *this; }
+
+  /** Translate \c *this by the vector \a t and returns a reference to \c *this. */
+  inline AlignedBox& translate(const VectorType& t)
+  { m_min += t; m_max += t; return *this; }
+
+  /** \returns the squared distance between the point \a p and the box \c *this,
+    * and zero if \a p is inside the box.
+    * \sa exteriorDistance()
+    */
+  inline Scalar squaredExteriorDistance(const VectorType& p) const;
+
+  /** \returns the distance between the point \a p and the box \c *this,
+    * and zero if \a p is inside the box.
+    * \sa squaredExteriorDistance()
+    */
+  inline Scalar exteriorDistance(const VectorType& p) const
+  { return ei_sqrt(squaredExteriorDistance(p)); }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<AlignedBox,
+           AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
+  {
+    return typename ei_cast_return_type<AlignedBox,
+                    AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
+  }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
+  {
+    m_min = other.min().template cast<Scalar>();
+    m_max = other.max().template cast<Scalar>();
+  }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const AlignedBox& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
+
+protected:
+
+  VectorType m_min, m_max;
+};
+
+template<typename Scalar,int AmbiantDim>
+inline Scalar AlignedBox<Scalar,AmbiantDim>::squaredExteriorDistance(const VectorType& p) const
+{
+  Scalar dist2 = 0.;
+  Scalar aux;
+  for (int k=0; k<dim(); ++k)
+  {
+    if ((aux = (p[k]-m_min[k]))<0.)
+      dist2 += aux*aux;
+    else if ( (aux = (m_max[k]-p[k]))<0. )
+      dist2 += aux*aux;
+  }
+  return dist2;
+}
+
+#endif // EIGEN_ALIGNEDBOX_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/AngleAxis.h b/extern/Eigen2/Eigen/src/Geometry/AngleAxis.h
new file mode 100644
index 00000000000..91e3d0d61ec
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/AngleAxis.h
@@ -0,0 +1,228 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ANGLEAXIS_H
+#define EIGEN_ANGLEAXIS_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class AngleAxis
+  *
+  * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients.
+  *
+  * The following two typedefs are provided for convenience:
+  * \li \c AngleAxisf for \c float
+  * \li \c AngleAxisd for \c double
+  *
+  * \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles
+  *
+  * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
+  * mimic Euler-angles. Here is an example:
+  * \include AngleAxis_mimic_euler.cpp
+  * Output: \verbinclude AngleAxis_mimic_euler.out
+  *
+  * \note This class is not aimed to be used to store a rotation transformation,
+  * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
+  * and transformation objects.
+  *
+  * \sa class Quaternion, class Transform, MatrixBase::UnitX()
+  */
+
+template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
+{
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
+{
+  typedef RotationBase<AngleAxis<_Scalar>,3> Base;
+
+public:
+
+  using Base::operator*;
+
+  enum { Dim = 3 };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,3,3> Matrix3;
+  typedef Matrix<Scalar,3,1> Vector3;
+  typedef Quaternion<Scalar> QuaternionType;
+
+protected:
+
+  Vector3 m_axis;
+  Scalar m_angle;
+
+public:
+
+  /** Default constructor without initialization. */
+  AngleAxis() {}
+  /** Constructs and initialize the angle-axis rotation from an \a angle in radian
+    * and an \a axis which must be normalized. */
+  template<typename Derived>
+  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
+  /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
+  inline AngleAxis(const QuaternionType& q) { *this = q; }
+  /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
+  template<typename Derived>
+  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
+
+  Scalar angle() const { return m_angle; }
+  Scalar& angle() { return m_angle; }
+
+  const Vector3& axis() const { return m_axis; }
+  Vector3& axis() { return m_axis; }
+
+  /** Concatenates two rotations */
+  inline QuaternionType operator* (const AngleAxis& other) const
+  { return QuaternionType(*this) * QuaternionType(other); }
+
+  /** Concatenates two rotations */
+  inline QuaternionType operator* (const QuaternionType& other) const
+  { return QuaternionType(*this) * other; }
+
+  /** Concatenates two rotations */
+  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
+  { return a * QuaternionType(b); }
+
+  /** Concatenates two rotations */
+  inline Matrix3 operator* (const Matrix3& other) const
+  { return toRotationMatrix() * other; }
+
+  /** Concatenates two rotations */
+  inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
+  { return a * b.toRotationMatrix(); }
+
+  /** Applies rotation to vector */
+  inline Vector3 operator* (const Vector3& other) const
+  { return toRotationMatrix() * other; }
+
+  /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
+  AngleAxis inverse() const
+  { return AngleAxis(-m_angle, m_axis); }
+
+  AngleAxis& operator=(const QuaternionType& q);
+  template<typename Derived>
+  AngleAxis& operator=(const MatrixBase<Derived>& m);
+
+  template<typename Derived>
+  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix3 toRotationMatrix(void) const;
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
+  { return typename ei_cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
+  {
+    m_axis = other.axis().template cast<Scalar>();
+    m_angle = Scalar(other.angle());
+  }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
+};
+
+/** \ingroup Geometry_Module
+  * single precision angle-axis type */
+typedef AngleAxis<float> AngleAxisf;
+/** \ingroup Geometry_Module
+  * double precision angle-axis type */
+typedef AngleAxis<double> AngleAxisd;
+
+/** Set \c *this from a quaternion.
+  * The axis is normalized.
+  */
+template<typename Scalar>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
+{
+  Scalar n2 = q.vec().squaredNorm();
+  if (n2 < precision<Scalar>()*precision<Scalar>())
+  {
+    m_angle = 0;
+    m_axis << 1, 0, 0;
+  }
+  else
+  {
+    m_angle = 2*std::acos(q.w());
+    m_axis = q.vec() / ei_sqrt(n2);
+  }
+  return *this;
+}
+
+/** Set \c *this from a 3x3 rotation matrix \a mat.
+  */
+template<typename Scalar>
+template<typename Derived>
+AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
+{
+  // Since a direct conversion would not be really faster,
+  // let's use the robust Quaternion implementation:
+  return *this = QuaternionType(mat);
+}
+
+/** Constructs and \returns an equivalent 3x3 rotation matrix.
+  */
+template<typename Scalar>
+typename AngleAxis<Scalar>::Matrix3
+AngleAxis<Scalar>::toRotationMatrix(void) const
+{
+  Matrix3 res;
+  Vector3 sin_axis  = ei_sin(m_angle) * m_axis;
+  Scalar c = ei_cos(m_angle);
+  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
+
+  Scalar tmp;
+  tmp = cos1_axis.x() * m_axis.y();
+  res.coeffRef(0,1) = tmp - sin_axis.z();
+  res.coeffRef(1,0) = tmp + sin_axis.z();
+
+  tmp = cos1_axis.x() * m_axis.z();
+  res.coeffRef(0,2) = tmp + sin_axis.y();
+  res.coeffRef(2,0) = tmp - sin_axis.y();
+
+  tmp = cos1_axis.y() * m_axis.z();
+  res.coeffRef(1,2) = tmp - sin_axis.x();
+  res.coeffRef(2,1) = tmp + sin_axis.x();
+
+  res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
+
+  return res;
+}
+
+#endif // EIGEN_ANGLEAXIS_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/EulerAngles.h b/extern/Eigen2/Eigen/src/Geometry/EulerAngles.h
new file mode 100644
index 00000000000..204118ac94d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/EulerAngles.h
@@ -0,0 +1,96 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_EULERANGLES_H
+#define EIGEN_EULERANGLES_H
+
+/** \geometry_module \ingroup Geometry_Module
+  * \nonstableyet
+  *
+  * \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
+  *
+  * Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
+  * For instance, in:
+  * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
+  * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
+  * we have the following equality:
+  * \code
+  * mat == AngleAxisf(ea[0], Vector3f::UnitZ())
+  *      * AngleAxisf(ea[1], Vector3f::UnitX())
+  *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
+  * This corresponds to the right-multiply conventions (with right hand side frames).
+  */
+template<typename Derived>
+inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
+MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const
+{
+  /* Implemented from Graphics Gems IV */
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
+
+  Matrix<Scalar,3,1> res;
+  typedef Matrix<typename Derived::Scalar,2,1> Vector2;
+  const Scalar epsilon = precision<Scalar>();
+
+  const int odd = ((a0+1)%3 == a1) ? 0 : 1;
+  const int i = a0;
+  const int j = (a0 + 1 + odd)%3;
+  const int k = (a0 + 2 - odd)%3;
+
+  if (a0==a2)
+  {
+    Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
+    res[1] = ei_atan2(s, coeff(i,i));
+    if (s > epsilon)
+    {
+      res[0] = ei_atan2(coeff(j,i), coeff(k,i));
+      res[2] = ei_atan2(coeff(i,j),-coeff(i,k));
+    }
+    else
+    {
+      res[0] = Scalar(0);
+      res[2] = (coeff(i,i)>0?1:-1)*ei_atan2(-coeff(k,j), coeff(j,j));
+    }
+  }
+  else
+  {
+    Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
+    res[1] = ei_atan2(-coeff(i,k), c);
+    if (c > epsilon)
+    {
+      res[0] = ei_atan2(coeff(j,k), coeff(k,k));
+      res[2] = ei_atan2(coeff(i,j), coeff(i,i));
+    }
+    else
+    {
+      res[0] = Scalar(0);
+      res[2] = (coeff(i,k)>0?1:-1)*ei_atan2(-coeff(k,j), coeff(j,j));
+    }
+  }
+  if (!odd)
+    res = -res;
+  return res;
+}
+
+
+#endif // EIGEN_EULERANGLES_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Hyperplane.h b/extern/Eigen2/Eigen/src/Geometry/Hyperplane.h
new file mode 100644
index 00000000000..22c530d4be0
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Hyperplane.h
@@ -0,0 +1,268 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_HYPERPLANE_H
+#define EIGEN_HYPERPLANE_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Hyperplane
+  *
+  * \brief A hyperplane
+  *
+  * A hyperplane is an affine subspace of dimension n-1 in a space of dimension n.
+  * For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane.
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
+  *             Notice that the dimension of the hyperplane is _AmbientDim-1.
+  *
+  * This class represents an hyperplane as the zero set of the implicit equation
+  * \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part)
+  * and \f$ d \f$ is the distance (offset) to the origin.
+  */
+template <typename _Scalar, int _AmbientDim>
+class Hyperplane
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
+  enum { AmbientDimAtCompileTime = _AmbientDim };
+  typedef _Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
+  typedef Matrix<Scalar,AmbientDimAtCompileTime==Dynamic
+                        ? Dynamic
+                        : AmbientDimAtCompileTime+1,1> Coefficients;
+  typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
+
+  /** Default constructor without initialization */
+  inline explicit Hyperplane() {}
+
+  /** Constructs a dynamic-size hyperplane with \a _dim the dimension
+    * of the ambient space */
+  inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
+
+  /** Construct a plane from its normal \a n and a point \a e onto the plane.
+    * \warning the vector normal is assumed to be normalized.
+    */
+  inline Hyperplane(const VectorType& n, const VectorType& e)
+    : m_coeffs(n.size()+1)
+  {
+    normal() = n;
+    offset() = -e.dot(n);
+  }
+
+  /** Constructs a plane from its normal \a n and distance to the origin \a d
+    * such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$.
+    * \warning the vector normal is assumed to be normalized.
+    */
+  inline Hyperplane(const VectorType& n, Scalar d)
+    : m_coeffs(n.size()+1)
+  {
+    normal() = n;
+    offset() = d;
+  }
+
+  /** Constructs a hyperplane passing through the two points. If the dimension of the ambient space
+    * is greater than 2, then there isn't uniqueness, so an arbitrary choice is made.
+    */
+  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
+  {
+    Hyperplane result(p0.size());
+    result.normal() = (p1 - p0).unitOrthogonal();
+    result.offset() = -result.normal().dot(p0);
+    return result;
+  }
+
+  /** Constructs a hyperplane passing through the three points. The dimension of the ambient space
+    * is required to be exactly 3.
+    */
+  static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
+  {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
+    Hyperplane result(p0.size());
+    result.normal() = (p2 - p0).cross(p1 - p0).normalized();
+    result.offset() = -result.normal().dot(p0);
+    return result;
+  }
+
+  /** Constructs a hyperplane passing through the parametrized line \a parametrized.
+    * If the dimension of the ambient space is greater than 2, then there isn't uniqueness,
+    * so an arbitrary choice is made.
+    */
+  // FIXME to be consitent with the rest this could be implemented as a static Through function ??
+  explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
+  {
+    normal() = parametrized.direction().unitOrthogonal();
+    offset() = -normal().dot(parametrized.origin());
+  }
+
+  ~Hyperplane() {}
+
+  /** \returns the dimension in which the plane holds */
+  inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : AmbientDimAtCompileTime; }
+
+  /** normalizes \c *this */
+  void normalize(void)
+  {
+    m_coeffs /= normal().norm();
+  }
+
+  /** \returns the signed distance between the plane \c *this and a point \a p.
+    * \sa absDistance()
+    */
+  inline Scalar signedDistance(const VectorType& p) const { return p.dot(normal()) + offset(); }
+
+  /** \returns the absolute distance between the plane \c *this and a point \a p.
+    * \sa signedDistance()
+    */
+  inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
+
+  /** \returns the projection of a point \a p onto the plane \c *this.
+    */
+  inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
+
+  /** \returns a constant reference to the unit normal vector of the plane, which corresponds
+    * to the linear part of the implicit equation.
+    */
+  inline const NormalReturnType normal() const { return NormalReturnType(m_coeffs,0,0,dim(),1); }
+
+  /** \returns a non-constant reference to the unit normal vector of the plane, which corresponds
+    * to the linear part of the implicit equation.
+    */
+  inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
+
+  /** \returns the distance to the origin, which is also the "constant term" of the implicit equation
+    * \warning the vector normal is assumed to be normalized.
+    */
+  inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
+
+  /** \returns a non-constant reference to the distance to the origin, which is also the constant part
+    * of the implicit equation */
+  inline Scalar& offset() { return m_coeffs(dim()); }
+
+  /** \returns a constant reference to the coefficients c_i of the plane equation:
+    * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
+    */
+  inline const Coefficients& coeffs() const { return m_coeffs; }
+
+  /** \returns a non-constant reference to the coefficients c_i of the plane equation:
+    * \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
+    */
+  inline Coefficients& coeffs() { return m_coeffs; }
+
+  /** \returns the intersection of *this with \a other.
+    *
+    * \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines.
+    *
+    * \note If \a other is approximately parallel to *this, this method will return any point on *this.
+    */
+  VectorType intersection(const Hyperplane& other)
+  {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
+    Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
+    // since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
+    // whether the two lines are approximately parallel.
+    if(ei_isMuchSmallerThan(det, Scalar(1)))
+    {   // special case where the two lines are approximately parallel. Pick any point on the first line.
+        if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
+            return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
+        else
+            return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
+    }
+    else
+    {   // general case
+        Scalar invdet = Scalar(1) / det;
+        return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
+                          invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
+    }
+  }
+
+  /** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this.
+    *
+    * \param mat the Dim x Dim transformation matrix
+    * \param traits specifies whether the matrix \a mat represents an Isometry
+    *               or a more generic Affine transformation. The default is Affine.
+    */
+  template<typename XprType>
+  inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
+  {
+    if (traits==Affine)
+      normal() = mat.inverse().transpose() * normal();
+    else if (traits==Isometry)
+      normal() = mat * normal();
+    else
+    {
+      ei_assert("invalid traits value in Hyperplane::transform()");
+    }
+    return *this;
+  }
+
+  /** Applies the transformation \a t to \c *this and returns a reference to \c *this.
+    *
+    * \param t the transformation of dimension Dim
+    * \param traits specifies whether the transformation \a t represents an Isometry
+    *               or a more generic Affine transformation. The default is Affine.
+    *               Other kind of transformations are not supported.
+    */
+  inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t,
+                                TransformTraits traits = Affine)
+  {
+    transform(t.linear(), traits);
+    offset() -= t.translation().dot(normal());
+    return *this;
+  }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Hyperplane,
+           Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
+  {
+    return typename ei_cast_return_type<Hyperplane,
+                    Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
+  }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other)
+  { m_coeffs = other.coeffs().template cast<Scalar>(); }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_coeffs.isApprox(other.m_coeffs, prec); }
+
+protected:
+
+  Coefficients m_coeffs;
+};
+
+#endif // EIGEN_HYPERPLANE_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/OrthoMethods.h b/extern/Eigen2/Eigen/src/Geometry/OrthoMethods.h
new file mode 100644
index 00000000000..047152d0b99
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/OrthoMethods.h
@@ -0,0 +1,119 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ORTHOMETHODS_H
+#define EIGEN_ORTHOMETHODS_H
+
+/** \geometry_module
+  *
+  * \returns the cross product of \c *this and \a other
+  *
+  * Here is a very good explanation of cross-product: http://xkcd.com/199/
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline typename MatrixBase<Derived>::PlainMatrixType
+MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
+
+  // Note that there is no need for an expression here since the compiler
+  // optimize such a small temporary very well (even within a complex expression)
+  const typename ei_nested<Derived,2>::type lhs(derived());
+  const typename ei_nested<OtherDerived,2>::type rhs(other.derived());
+  return typename ei_plain_matrix_type<Derived>::type(
+    lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
+    lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
+    lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)
+  );
+}
+
+template<typename Derived, int Size = Derived::SizeAtCompileTime>
+struct ei_unitOrthogonal_selector
+{
+  typedef typename ei_plain_matrix_type<Derived>::type VectorType;
+  typedef typename ei_traits<Derived>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  inline static VectorType run(const Derived& src)
+  {
+    VectorType perp(src.size());
+    /* Let us compute the crossed product of *this with a vector
+     * that is not too close to being colinear to *this.
+     */
+
+    /* unless the x and y coords are both close to zero, we can
+     * simply take ( -y, x, 0 ) and normalize it.
+     */
+    if((!ei_isMuchSmallerThan(src.x(), src.z()))
+    || (!ei_isMuchSmallerThan(src.y(), src.z())))
+    {
+      RealScalar invnm = RealScalar(1)/src.template start<2>().norm();
+      perp.coeffRef(0) = -ei_conj(src.y())*invnm;
+      perp.coeffRef(1) = ei_conj(src.x())*invnm;
+      perp.coeffRef(2) = 0;
+    }
+    /* if both x and y are close to zero, then the vector is close
+     * to the z-axis, so it's far from colinear to the x-axis for instance.
+     * So we take the crossed product with (1,0,0) and normalize it.
+     */
+    else
+    {
+      RealScalar invnm = RealScalar(1)/src.template end<2>().norm();
+      perp.coeffRef(0) = 0;
+      perp.coeffRef(1) = -ei_conj(src.z())*invnm;
+      perp.coeffRef(2) = ei_conj(src.y())*invnm;
+    }
+    if( (Derived::SizeAtCompileTime!=Dynamic && Derived::SizeAtCompileTime>3)
+     || (Derived::SizeAtCompileTime==Dynamic && src.size()>3) )
+      perp.end(src.size()-3).setZero();
+
+    return perp;
+   }
+};
+
+template<typename Derived>
+struct ei_unitOrthogonal_selector<Derived,2>
+{
+  typedef typename ei_plain_matrix_type<Derived>::type VectorType;
+  inline static VectorType run(const Derived& src)
+  { return VectorType(-ei_conj(src.y()), ei_conj(src.x())).normalized(); }
+};
+
+/** \returns a unit vector which is orthogonal to \c *this
+  *
+  * The size of \c *this must be at least 2. If the size is exactly 2,
+  * then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
+  *
+  * \sa cross()
+  */
+template<typename Derived>
+typename MatrixBase<Derived>::PlainMatrixType
+MatrixBase<Derived>::unitOrthogonal() const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return ei_unitOrthogonal_selector<Derived>::run(derived());
+}
+
+#endif // EIGEN_ORTHOMETHODS_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/ParametrizedLine.h b/extern/Eigen2/Eigen/src/Geometry/ParametrizedLine.h
new file mode 100644
index 00000000000..2b990d084f0
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/ParametrizedLine.h
@@ -0,0 +1,155 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_PARAMETRIZEDLINE_H
+#define EIGEN_PARAMETRIZEDLINE_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class ParametrizedLine
+  *
+  * \brief A parametrized line
+  *
+  * A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit
+  * direction vector \f$ \mathbf{d} \f$ such that the line corresponds to
+  * the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ l \in \mathbf{R} \f$.
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
+  */
+template <typename _Scalar, int _AmbientDim>
+class ParametrizedLine
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
+  enum { AmbientDimAtCompileTime = _AmbientDim };
+  typedef _Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
+
+  /** Default constructor without initialization */
+  inline explicit ParametrizedLine() {}
+
+  /** Constructs a dynamic-size line with \a _dim the dimension
+    * of the ambient space */
+  inline explicit ParametrizedLine(int _dim) : m_origin(_dim), m_direction(_dim) {}
+
+  /** Initializes a parametrized line of direction \a direction and origin \a origin.
+    * \warning the vector direction is assumed to be normalized.
+    */
+  ParametrizedLine(const VectorType& origin, const VectorType& direction)
+    : m_origin(origin), m_direction(direction) {}
+
+  explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
+
+  /** Constructs a parametrized line going from \a p0 to \a p1. */
+  static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1)
+  { return ParametrizedLine(p0, (p1-p0).normalized()); }
+
+  ~ParametrizedLine() {}
+
+  /** \returns the dimension in which the line holds */
+  inline int dim() const { return m_direction.size(); }
+
+  const VectorType& origin() const { return m_origin; }
+  VectorType& origin() { return m_origin; }
+
+  const VectorType& direction() const { return m_direction; }
+  VectorType& direction() { return m_direction; }
+
+  /** \returns the squared distance of a point \a p to its projection onto the line \c *this.
+    * \sa distance()
+    */
+  RealScalar squaredDistance(const VectorType& p) const
+  {
+    VectorType diff = p-origin();
+    return (diff - diff.dot(direction())* direction()).squaredNorm();
+  }
+  /** \returns the distance of a point \a p to its projection onto the line \c *this.
+    * \sa squaredDistance()
+    */
+  RealScalar distance(const VectorType& p) const { return ei_sqrt(squaredDistance(p)); }
+
+  /** \returns the projection of a point \a p onto the line \c *this. */
+  VectorType projection(const VectorType& p) const
+  { return origin() + (p-origin()).dot(direction()) * direction(); }
+
+  Scalar intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<ParametrizedLine,
+           ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
+  {
+    return typename ei_cast_return_type<ParametrizedLine,
+                    ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
+  }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime>& other)
+  {
+    m_origin = other.origin().template cast<Scalar>();
+    m_direction = other.direction().template cast<Scalar>();
+  }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
+
+protected:
+
+  VectorType m_origin, m_direction;
+};
+
+/** Constructs a parametrized line from a 2D hyperplane
+  *
+  * \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line
+  */
+template <typename _Scalar, int _AmbientDim>
+inline ParametrizedLine<_Scalar, _AmbientDim>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
+  direction() = hyperplane.normal().unitOrthogonal();
+  origin() = -hyperplane.normal()*hyperplane.offset();
+}
+
+/** \returns the parameter value of the intersection between \c *this and the given hyperplane
+  */
+template <typename _Scalar, int _AmbientDim>
+inline _Scalar ParametrizedLine<_Scalar, _AmbientDim>::intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
+{
+  return -(hyperplane.offset()+origin().dot(hyperplane.normal()))
+          /(direction().dot(hyperplane.normal()));
+}
+
+#endif // EIGEN_PARAMETRIZEDLINE_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Quaternion.h b/extern/Eigen2/Eigen/src/Geometry/Quaternion.h
new file mode 100644
index 00000000000..3fcbff4e71d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Quaternion.h
@@ -0,0 +1,521 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_QUATERNION_H
+#define EIGEN_QUATERNION_H
+
+template<typename Other,
+         int OtherRows=Other::RowsAtCompileTime,
+         int OtherCols=Other::ColsAtCompileTime>
+struct ei_quaternion_assign_impl;
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Quaternion
+  *
+  * \brief The quaternion class used to represent 3D orientations and rotations
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  *
+  * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
+  * orientations and rotations of objects in three dimensions. Compared to other representations
+  * like Euler angles or 3x3 matrices, quatertions offer the following advantages:
+  * \li \b compact storage (4 scalars)
+  * \li \b efficient to compose (28 flops),
+  * \li \b stable spherical interpolation
+  *
+  * The following two typedefs are provided for convenience:
+  * \li \c Quaternionf for \c float
+  * \li \c Quaterniond for \c double
+  *
+  * \sa  class AngleAxis, class Transform
+  */
+
+template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
+{
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
+{
+  typedef RotationBase<Quaternion<_Scalar>,3> Base;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
+
+  using Base::operator*;
+
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+
+  /** the type of the Coefficients 4-vector */
+  typedef Matrix<Scalar, 4, 1> Coefficients;
+  /** the type of a 3D vector */
+  typedef Matrix<Scalar,3,1> Vector3;
+  /** the equivalent rotation matrix type */
+  typedef Matrix<Scalar,3,3> Matrix3;
+  /** the equivalent angle-axis type */
+  typedef AngleAxis<Scalar> AngleAxisType;
+
+  /** \returns the \c x coefficient */
+  inline Scalar x() const { return m_coeffs.coeff(0); }
+  /** \returns the \c y coefficient */
+  inline Scalar y() const { return m_coeffs.coeff(1); }
+  /** \returns the \c z coefficient */
+  inline Scalar z() const { return m_coeffs.coeff(2); }
+  /** \returns the \c w coefficient */
+  inline Scalar w() const { return m_coeffs.coeff(3); }
+
+  /** \returns a reference to the \c x coefficient */
+  inline Scalar& x() { return m_coeffs.coeffRef(0); }
+  /** \returns a reference to the \c y coefficient */
+  inline Scalar& y() { return m_coeffs.coeffRef(1); }
+  /** \returns a reference to the \c z coefficient */
+  inline Scalar& z() { return m_coeffs.coeffRef(2); }
+  /** \returns a reference to the \c w coefficient */
+  inline Scalar& w() { return m_coeffs.coeffRef(3); }
+
+  /** \returns a read-only vector expression of the imaginary part (x,y,z) */
+  inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
+
+  /** \returns a vector expression of the imaginary part (x,y,z) */
+  inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
+
+  /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
+  inline const Coefficients& coeffs() const { return m_coeffs; }
+
+  /** \returns a vector expression of the coefficients (x,y,z,w) */
+  inline Coefficients& coeffs() { return m_coeffs; }
+
+  /** Default constructor leaving the quaternion uninitialized. */
+  inline Quaternion() {}
+
+  /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
+    * its four coefficients \a w, \a x, \a y and \a z.
+    *
+    * \warning Note the order of the arguments: the real \a w coefficient first,
+    * while internally the coefficients are stored in the following order:
+    * [\c x, \c y, \c z, \c w]
+    */
+  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
+  { m_coeffs << x, y, z, w; }
+
+  /** Copy constructor */
+  inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
+
+  /** Constructs and initializes a quaternion from the angle-axis \a aa */
+  explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
+
+  /** Constructs and initializes a quaternion from either:
+    *  - a rotation matrix expression,
+    *  - a 4D vector expression representing quaternion coefficients.
+    * \sa operator=(MatrixBase<Derived>)
+    */
+  template<typename Derived>
+  explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
+
+  Quaternion& operator=(const Quaternion& other);
+  Quaternion& operator=(const AngleAxisType& aa);
+  template<typename Derived>
+  Quaternion& operator=(const MatrixBase<Derived>& m);
+
+  /** \returns a quaternion representing an identity rotation
+    * \sa MatrixBase::Identity()
+    */
+  inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
+
+  /** \sa Quaternion::Identity(), MatrixBase::setIdentity()
+    */
+  inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
+
+  /** \returns the squared norm of the quaternion's coefficients
+    * \sa Quaternion::norm(), MatrixBase::squaredNorm()
+    */
+  inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
+
+  /** \returns the norm of the quaternion's coefficients
+    * \sa Quaternion::squaredNorm(), MatrixBase::norm()
+    */
+  inline Scalar norm() const { return m_coeffs.norm(); }
+
+  /** Normalizes the quaternion \c *this
+    * \sa normalized(), MatrixBase::normalize() */
+  inline void normalize() { m_coeffs.normalize(); }
+  /** \returns a normalized version of \c *this
+    * \sa normalize(), MatrixBase::normalized() */
+  inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
+
+  /** \returns the dot product of \c *this and \a other
+    * Geometrically speaking, the dot product of two unit quaternions
+    * corresponds to the cosine of half the angle between the two rotations.
+    * \sa angularDistance()
+    */
+  inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
+
+  inline Scalar angularDistance(const Quaternion& other) const;
+
+  Matrix3 toRotationMatrix(void) const;
+
+  template<typename Derived1, typename Derived2>
+  Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
+
+  inline Quaternion operator* (const Quaternion& q) const;
+  inline Quaternion& operator*= (const Quaternion& q);
+
+  Quaternion inverse(void) const;
+  Quaternion conjugate(void) const;
+
+  Quaternion slerp(Scalar t, const Quaternion& other) const;
+
+  template<typename Derived>
+  Vector3 operator* (const MatrixBase<Derived>& vec) const;
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
+  { return typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
+  { m_coeffs = other.coeffs().template cast<Scalar>(); }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_coeffs.isApprox(other.m_coeffs, prec); }
+
+protected:
+  Coefficients m_coeffs;
+};
+
+/** \ingroup Geometry_Module
+  * single precision quaternion type */
+typedef Quaternion<float> Quaternionf;
+/** \ingroup Geometry_Module
+  * double precision quaternion type */
+typedef Quaternion<double> Quaterniond;
+
+// Generic Quaternion * Quaternion product
+template<int Arch,typename Scalar> inline Quaternion<Scalar>
+ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
+{
+  return Quaternion<Scalar>
+  (
+    a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
+    a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
+    a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
+    a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
+  );
+}
+
+#ifdef EIGEN_VECTORIZE_SSE
+template<> inline Quaternion<float>
+ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quaternion<float>& _b)
+{
+  const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
+  Quaternion<float> res;
+  __m128 a = _a.coeffs().packet<Aligned>(0);
+  __m128 b = _b.coeffs().packet<Aligned>(0);
+  __m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
+                                       ei_vec4f_swizzle1(b,2,0,1,2)),mask);
+  __m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
+                                       ei_vec4f_swizzle1(b,0,1,2,1)),mask);
+  ei_pstore(&res.x(),
+            _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
+                                  _mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
+                                             ei_vec4f_swizzle1(b,1,2,0,0))),
+                       _mm_add_ps(flip1,flip2)));
+  return res;
+}
+#endif
+
+/** \returns the concatenation of two rotations as a quaternion-quaternion product */
+template <typename Scalar>
+inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
+{
+  return ei_quaternion_product<EiArch>(*this,other);
+}
+
+/** \sa operator*(Quaternion) */
+template <typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
+{
+  return (*this = *this * other);
+}
+
+/** Rotation of a vector by a quaternion.
+  * \remarks If the quaternion is used to rotate several points (>1)
+  * then it is much more efficient to first convert it to a 3x3 Matrix.
+  * Comparison of the operation cost for n transformations:
+  *   - Quaternion:    30n
+  *   - Via a Matrix3: 24 + 15n
+  */
+template <typename Scalar>
+template<typename Derived>
+inline typename Quaternion<Scalar>::Vector3
+Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
+{
+    // Note that this algorithm comes from the optimization by hand
+    // of the conversion to a Matrix followed by a Matrix/Vector product.
+    // It appears to be much faster than the common algorithm found
+    // in the litterature (30 versus 39 flops). It also requires two
+    // Vector3 as temporaries.
+    Vector3 uv;
+    uv = 2 * this->vec().cross(v);
+    return v + this->w() * uv + this->vec().cross(uv);
+}
+
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
+{
+  m_coeffs = other.m_coeffs;
+  return *this;
+}
+
+/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
+  */
+template<typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
+{
+  Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
+  this->w() = ei_cos(ha);
+  this->vec() = ei_sin(ha) * aa.axis();
+  return *this;
+}
+
+/** Set \c *this from the expression \a xpr:
+  *   - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
+  *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
+  *     and \a xpr is converted to a quaternion
+  */
+template<typename Scalar>
+template<typename Derived>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
+{
+  ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
+  return *this;
+}
+
+/** Convert the quaternion to a 3x3 rotation matrix */
+template<typename Scalar>
+inline typename Quaternion<Scalar>::Matrix3
+Quaternion<Scalar>::toRotationMatrix(void) const
+{
+  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
+  // if not inlined then the cost of the return by value is huge ~ +35%,
+  // however, not inlining this function is an order of magnitude slower, so
+  // it has to be inlined, and so the return by value is not an issue
+  Matrix3 res;
+
+  const Scalar tx  = 2*this->x();
+  const Scalar ty  = 2*this->y();
+  const Scalar tz  = 2*this->z();
+  const Scalar twx = tx*this->w();
+  const Scalar twy = ty*this->w();
+  const Scalar twz = tz*this->w();
+  const Scalar txx = tx*this->x();
+  const Scalar txy = ty*this->x();
+  const Scalar txz = tz*this->x();
+  const Scalar tyy = ty*this->y();
+  const Scalar tyz = tz*this->y();
+  const Scalar tzz = tz*this->z();
+
+  res.coeffRef(0,0) = 1-(tyy+tzz);
+  res.coeffRef(0,1) = txy-twz;
+  res.coeffRef(0,2) = txz+twy;
+  res.coeffRef(1,0) = txy+twz;
+  res.coeffRef(1,1) = 1-(txx+tzz);
+  res.coeffRef(1,2) = tyz-twx;
+  res.coeffRef(2,0) = txz-twy;
+  res.coeffRef(2,1) = tyz+twx;
+  res.coeffRef(2,2) = 1-(txx+tyy);
+
+  return res;
+}
+
+/** Sets *this to be a quaternion representing a rotation sending the vector \a a to the vector \a b.
+  *
+  * \returns a reference to *this.
+  *
+  * Note that the two input vectors do \b not have to be normalized.
+  */
+template<typename Scalar>
+template<typename Derived1, typename Derived2>
+inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
+{
+  Vector3 v0 = a.normalized();
+  Vector3 v1 = b.normalized();
+  Scalar c = v0.dot(v1);
+
+  // if dot == 1, vectors are the same
+  if (ei_isApprox(c,Scalar(1)))
+  {
+    // set to identity
+    this->w() = 1; this->vec().setZero();
+    return *this;
+  }
+  // if dot == -1, vectors are opposites
+  if (ei_isApprox(c,Scalar(-1)))
+  {
+    this->vec() = v0.unitOrthogonal();
+    this->w() = 0;
+    return *this;
+  }
+
+  Vector3 axis = v0.cross(v1);
+  Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
+  Scalar invs = Scalar(1)/s;
+  this->vec() = axis * invs;
+  this->w() = s * Scalar(0.5);
+
+  return *this;
+}
+
+/** \returns the multiplicative inverse of \c *this
+  * Note that in most cases, i.e., if you simply want the opposite rotation,
+  * and/or the quaternion is normalized, then it is enough to use the conjugate.
+  *
+  * \sa Quaternion::conjugate()
+  */
+template <typename Scalar>
+inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
+{
+  // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
+  Scalar n2 = this->squaredNorm();
+  if (n2 > 0)
+    return Quaternion(conjugate().coeffs() / n2);
+  else
+  {
+    // return an invalid result to flag the error
+    return Quaternion(Coefficients::Zero());
+  }
+}
+
+/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
+  * if the quaternion is normalized.
+  * The conjugate of a quaternion represents the opposite rotation.
+  *
+  * \sa Quaternion::inverse()
+  */
+template <typename Scalar>
+inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
+{
+  return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
+}
+
+/** \returns the angle (in radian) between two rotations
+  * \sa dot()
+  */
+template <typename Scalar>
+inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
+{
+  double d = ei_abs(this->dot(other));
+  if (d>=1.0)
+    return 0;
+  return Scalar(2) * std::acos(d);
+}
+
+/** \returns the spherical linear interpolation between the two quaternions
+  * \c *this and \a other at the parameter \a t
+  */
+template <typename Scalar>
+Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
+{
+  static const Scalar one = Scalar(1) - precision<Scalar>();
+  Scalar d = this->dot(other);
+  Scalar absD = ei_abs(d);
+  if (absD>=one)
+    return *this;
+
+  // theta is the angle between the 2 quaternions
+  Scalar theta = std::acos(absD);
+  Scalar sinTheta = ei_sin(theta);
+
+  Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
+  Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
+  if (d<0)
+    scale1 = -scale1;
+
+  return Quaternion(scale0 * m_coeffs + scale1 * other.m_coeffs);
+}
+
+// set from a rotation matrix
+template<typename Other>
+struct ei_quaternion_assign_impl<Other,3,3>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(Quaternion<Scalar>& q, const Other& mat)
+  {
+    // This algorithm comes from  "Quaternion Calculus and Fast Animation",
+    // Ken Shoemake, 1987 SIGGRAPH course notes
+    Scalar t = mat.trace();
+    if (t > 0)
+    {
+      t = ei_sqrt(t + Scalar(1.0));
+      q.w() = Scalar(0.5)*t;
+      t = Scalar(0.5)/t;
+      q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
+      q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
+      q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
+    }
+    else
+    {
+      int i = 0;
+      if (mat.coeff(1,1) > mat.coeff(0,0))
+        i = 1;
+      if (mat.coeff(2,2) > mat.coeff(i,i))
+        i = 2;
+      int j = (i+1)%3;
+      int k = (j+1)%3;
+
+      t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
+      q.coeffs().coeffRef(i) = Scalar(0.5) * t;
+      t = Scalar(0.5)/t;
+      q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
+      q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
+      q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
+    }
+  }
+};
+
+// set from a vector of coefficients assumed to be a quaternion
+template<typename Other>
+struct ei_quaternion_assign_impl<Other,4,1>
+{
+  typedef typename Other::Scalar Scalar;
+  inline static void run(Quaternion<Scalar>& q, const Other& vec)
+  {
+    q.coeffs() = vec;
+  }
+};
+
+#endif // EIGEN_QUATERNION_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Rotation2D.h b/extern/Eigen2/Eigen/src/Geometry/Rotation2D.h
new file mode 100644
index 00000000000..dca7f06bf5d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Rotation2D.h
@@ -0,0 +1,159 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ROTATION2D_H
+#define EIGEN_ROTATION2D_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Rotation2D
+  *
+  * \brief Represents a rotation/orientation in a 2 dimensional space.
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  *
+  * This class is equivalent to a single scalar representing a counter clock wise rotation
+  * as a single angle in radian. It provides some additional features such as the automatic
+  * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar
+  * interface to Quaternion in order to facilitate the writing of generic algorithms
+  * dealing with rotations.
+  *
+  * \sa class Quaternion, class Transform
+  */
+template<typename _Scalar> struct ei_traits<Rotation2D<_Scalar> >
+{
+  typedef _Scalar Scalar;
+};
+
+template<typename _Scalar>
+class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2>
+{
+  typedef RotationBase<Rotation2D<_Scalar>,2> Base;
+
+public:
+
+  using Base::operator*;
+
+  enum { Dim = 2 };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  typedef Matrix<Scalar,2,1> Vector2;
+  typedef Matrix<Scalar,2,2> Matrix2;
+
+protected:
+
+  Scalar m_angle;
+
+public:
+
+  /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
+  inline Rotation2D(Scalar a) : m_angle(a) {}
+
+  /** \returns the rotation angle */
+  inline Scalar angle() const { return m_angle; }
+
+  /** \returns a read-write reference to the rotation angle */
+  inline Scalar& angle() { return m_angle; }
+
+  /** \returns the inverse rotation */
+  inline Rotation2D inverse() const { return -m_angle; }
+
+  /** Concatenates two rotations */
+  inline Rotation2D operator*(const Rotation2D& other) const
+  { return m_angle + other.m_angle; }
+
+  /** Concatenates two rotations */
+  inline Rotation2D& operator*=(const Rotation2D& other)
+  { return m_angle += other.m_angle; return *this; }
+
+  /** Applies the rotation to a 2D vector */
+  Vector2 operator* (const Vector2& vec) const
+  { return toRotationMatrix() * vec; }
+
+  template<typename Derived>
+  Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
+  Matrix2 toRotationMatrix(void) const;
+
+  /** \returns the spherical interpolation between \c *this and \a other using
+    * parameter \a t. It is in fact equivalent to a linear interpolation.
+    */
+  inline Rotation2D slerp(Scalar t, const Rotation2D& other) const
+  { return m_angle * (1-t) + other.angle() * t; }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const
+  { return typename ei_cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other)
+  {
+    m_angle = Scalar(other.angle());
+  }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Rotation2D& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return ei_isApprox(m_angle,other.m_angle, prec); }
+};
+
+/** \ingroup Geometry_Module
+  * single precision 2D rotation type */
+typedef Rotation2D<float> Rotation2Df;
+/** \ingroup Geometry_Module
+  * double precision 2D rotation type */
+typedef Rotation2D<double> Rotation2Dd;
+
+/** Set \c *this from a 2x2 rotation matrix \a mat.
+  * In other words, this function extract the rotation angle
+  * from the rotation matrix.
+  */
+template<typename Scalar>
+template<typename Derived>
+Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
+{
+  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
+  m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0));
+  return *this;
+}
+
+/** Constructs and \returns an equivalent 2x2 rotation matrix.
+  */
+template<typename Scalar>
+typename Rotation2D<Scalar>::Matrix2
+Rotation2D<Scalar>::toRotationMatrix(void) const
+{
+  Scalar sinA = ei_sin(m_angle);
+  Scalar cosA = ei_cos(m_angle);
+  return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
+}
+
+#endif // EIGEN_ROTATION2D_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/RotationBase.h b/extern/Eigen2/Eigen/src/Geometry/RotationBase.h
new file mode 100644
index 00000000000..5fec0f18d72
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/RotationBase.h
@@ -0,0 +1,137 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_ROTATIONBASE_H
+#define EIGEN_ROTATIONBASE_H
+
+// this file aims to contains the various representations of rotation/orientation
+// in 2D and 3D space excepted Matrix and Quaternion.
+
+/** \class RotationBase
+  *
+  * \brief Common base class for compact rotation representations
+  *
+  * \param Derived is the derived type, i.e., a rotation type
+  * \param _Dim the dimension of the space
+  */
+template<typename Derived, int _Dim>
+class RotationBase
+{
+  public:
+    enum { Dim = _Dim };
+    /** the scalar type of the coefficients */
+    typedef typename ei_traits<Derived>::Scalar Scalar;
+    
+    /** corresponding linear transformation matrix type */
+    typedef Matrix<Scalar,Dim,Dim> RotationMatrixType;
+
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
+
+    /** \returns an equivalent rotation matrix */
+    inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }
+
+    /** \returns the inverse rotation */
+    inline Derived inverse() const { return derived().inverse(); }
+
+    /** \returns the concatenation of the rotation \c *this with a translation \a t */
+    inline Transform<Scalar,Dim> operator*(const Translation<Scalar,Dim>& t) const
+    { return toRotationMatrix() * t; }
+
+    /** \returns the concatenation of the rotation \c *this with a scaling \a s */
+    inline RotationMatrixType operator*(const Scaling<Scalar,Dim>& s) const
+    { return toRotationMatrix() * s; }
+
+    /** \returns the concatenation of the rotation \c *this with an affine transformation \a t */
+    inline Transform<Scalar,Dim> operator*(const Transform<Scalar,Dim>& t) const
+    { return toRotationMatrix() * t; }
+};
+
+/** \geometry_module
+  *
+  * Constructs a Dim x Dim rotation matrix from the rotation \a r
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
+template<typename OtherDerived>
+Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>
+::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
+{
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
+  *this = r.toRotationMatrix();
+}
+
+/** \geometry_module
+  *
+  * Set a Dim x Dim rotation matrix from the rotation \a r
+  */
+template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
+template<typename OtherDerived>
+Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>&
+Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>
+::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
+{
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
+  return *this = r.toRotationMatrix();
+}
+
+/** \internal
+  *
+  * Helper function to return an arbitrary rotation object to a rotation matrix.
+  *
+  * \param Scalar the numeric type of the matrix coefficients
+  * \param Dim the dimension of the current space
+  *
+  * It returns a Dim x Dim fixed size matrix.
+  *
+  * Default specializations are provided for:
+  *   - any scalar type (2D),
+  *   - any matrix expression,
+  *   - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D)
+  *
+  * Currently ei_toRotationMatrix is only used by Transform.
+  *
+  * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis
+  */
+template<typename Scalar, int Dim>
+inline static Matrix<Scalar,2,2> ei_toRotationMatrix(const Scalar& s)
+{
+  EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
+  return Rotation2D<Scalar>(s).toRotationMatrix();
+}
+
+template<typename Scalar, int Dim, typename OtherDerived>
+inline static Matrix<Scalar,Dim,Dim> ei_toRotationMatrix(const RotationBase<OtherDerived,Dim>& r)
+{
+  return r.toRotationMatrix();
+}
+
+template<typename Scalar, int Dim, typename OtherDerived>
+inline static const MatrixBase<OtherDerived>& ei_toRotationMatrix(const MatrixBase<OtherDerived>& mat)
+{
+  EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
+    YOU_MADE_A_PROGRAMMING_MISTAKE)
+  return mat;
+}
+
+#endif // EIGEN_ROTATIONBASE_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Scaling.h b/extern/Eigen2/Eigen/src/Geometry/Scaling.h
new file mode 100644
index 00000000000..5daf0a49961
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Scaling.h
@@ -0,0 +1,181 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SCALING_H
+#define EIGEN_SCALING_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Scaling
+  *
+  * \brief Represents a possibly non uniform scaling transformation
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients.
+  * \param _Dim the  dimension of the space, can be a compile time value or Dynamic
+  *
+  * \note This class is not aimed to be used to store a scaling transformation,
+  * but rather to make easier the constructions and updates of Transform objects.
+  *
+  * \sa class Translation, class Transform
+  */
+template<typename _Scalar, int _Dim>
+class Scaling
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim)
+  /** dimension of the space */
+  enum { Dim = _Dim };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  /** corresponding vector type */
+  typedef Matrix<Scalar,Dim,1> VectorType;
+  /** corresponding linear transformation matrix type */
+  typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
+  /** corresponding translation type */
+  typedef Translation<Scalar,Dim> TranslationType;
+  /** corresponding affine transformation type */
+  typedef Transform<Scalar,Dim> TransformType;
+
+protected:
+
+  VectorType m_coeffs;
+
+public:
+
+  /** Default constructor without initialization. */
+  Scaling() {}
+  /** Constructs and initialize a uniform scaling transformation */
+  explicit inline Scaling(const Scalar& s) { m_coeffs.setConstant(s); }
+  /** 2D only */
+  inline Scaling(const Scalar& sx, const Scalar& sy)
+  {
+    ei_assert(Dim==2);
+    m_coeffs.x() = sx;
+    m_coeffs.y() = sy;
+  }
+  /** 3D only */
+  inline Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz)
+  {
+    ei_assert(Dim==3);
+    m_coeffs.x() = sx;
+    m_coeffs.y() = sy;
+    m_coeffs.z() = sz;
+  }
+  /** Constructs and initialize the scaling transformation from a vector of scaling coefficients */
+  explicit inline Scaling(const VectorType& coeffs) : m_coeffs(coeffs) {}
+
+  const VectorType& coeffs() const { return m_coeffs; }
+  VectorType& coeffs() { return m_coeffs; }
+
+  /** Concatenates two scaling */
+  inline Scaling operator* (const Scaling& other) const
+  { return Scaling(coeffs().cwise() * other.coeffs()); }
+
+  /** Concatenates a scaling and a translation */
+  inline TransformType operator* (const TranslationType& t) const;
+
+  /** Concatenates a scaling and an affine transformation */
+  inline TransformType operator* (const TransformType& t) const;
+
+  /** Concatenates a scaling and a linear transformation matrix */
+  // TODO returns an expression
+  inline LinearMatrixType operator* (const LinearMatrixType& other) const
+  { return coeffs().asDiagonal() * other; }
+
+  /** Concatenates a linear transformation matrix and a scaling */
+  // TODO returns an expression
+  friend inline LinearMatrixType operator* (const LinearMatrixType& other, const Scaling& s)
+  { return other * s.coeffs().asDiagonal(); }
+
+  template<typename Derived>
+  inline LinearMatrixType operator*(const RotationBase<Derived,Dim>& r) const
+  { return *this * r.toRotationMatrix(); }
+
+  /** Applies scaling to vector */
+  inline VectorType operator* (const VectorType& other) const
+  { return coeffs().asDiagonal() * other; }
+
+  /** \returns the inverse scaling */
+  inline Scaling inverse() const
+  { return Scaling(coeffs().cwise().inverse()); }
+
+  inline Scaling& operator=(const Scaling& other)
+  {
+    m_coeffs = other.m_coeffs;
+    return *this;
+  }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type cast() const
+  { return typename ei_cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Scaling(const Scaling<OtherScalarType,Dim>& other)
+  { m_coeffs = other.coeffs().template cast<Scalar>(); }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Scaling& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_coeffs.isApprox(other.m_coeffs, prec); }
+
+};
+
+/** \addtogroup Geometry_Module */
+//@{
+typedef Scaling<float, 2> Scaling2f;
+typedef Scaling<double,2> Scaling2d;
+typedef Scaling<float, 3> Scaling3f;
+typedef Scaling<double,3> Scaling3d;
+//@}
+
+template<typename Scalar, int Dim>
+inline typename Scaling<Scalar,Dim>::TransformType
+Scaling<Scalar,Dim>::operator* (const TranslationType& t) const
+{
+  TransformType res;
+  res.matrix().setZero();
+  res.linear().diagonal() = coeffs();
+  res.translation() = m_coeffs.cwise() * t.vector();
+  res(Dim,Dim) = Scalar(1);
+  return res;
+}
+
+template<typename Scalar, int Dim>
+inline typename Scaling<Scalar,Dim>::TransformType
+Scaling<Scalar,Dim>::operator* (const TransformType& t) const
+{
+  TransformType res = t;
+  res.prescale(m_coeffs);
+  return res;
+}
+
+#endif // EIGEN_SCALING_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Transform.h b/extern/Eigen2/Eigen/src/Geometry/Transform.h
new file mode 100644
index 00000000000..8425a1cd963
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Transform.h
@@ -0,0 +1,785 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_TRANSFORM_H
+#define EIGEN_TRANSFORM_H
+
+/** Represents some traits of a transformation */
+enum TransformTraits {
+  Isometry,       ///< the transformation is a concatenation of translations and rotations
+  Affine,         ///< the transformation is affine (linear transformation + translation)
+  Projective      ///< the transformation might not be affine
+};
+
+// Note that we have to pass Dim and HDim because it is not allowed to use a template
+// parameter to define a template specialization. To be more precise, in the following
+// specializations, it is not allowed to use Dim+1 instead of HDim.
+template< typename Other,
+          int Dim,
+          int HDim,
+          int OtherRows=Other::RowsAtCompileTime,
+          int OtherCols=Other::ColsAtCompileTime>
+struct ei_transform_product_impl;
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Transform
+  *
+  * \brief Represents an homogeneous transformation in a N dimensional space
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients
+  * \param _Dim the dimension of the space
+  *
+  * The homography is internally represented and stored as a (Dim+1)^2 matrix which
+  * is available through the matrix() method.
+  *
+  * Conversion methods from/to Qt's QMatrix and QTransform are available if the
+  * preprocessor token EIGEN_QT_SUPPORT is defined.
+  *
+  * \sa class Matrix, class Quaternion
+  */
+template<typename _Scalar, int _Dim>
+class Transform
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1))
+  enum {
+    Dim = _Dim,     ///< space dimension in which the transformation holds
+    HDim = _Dim+1   ///< size of a respective homogeneous vector
+  };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  /** type of the matrix used to represent the transformation */
+  typedef Matrix<Scalar,HDim,HDim> MatrixType;
+  /** type of the matrix used to represent the linear part of the transformation */
+  typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
+  /** type of read/write reference to the linear part of the transformation */
+  typedef Block<MatrixType,Dim,Dim> LinearPart;
+  /** type of a vector */
+  typedef Matrix<Scalar,Dim,1> VectorType;
+  /** type of a read/write reference to the translation part of the rotation */
+  typedef Block<MatrixType,Dim,1> TranslationPart;
+  /** corresponding translation type */
+  typedef Translation<Scalar,Dim> TranslationType;
+  /** corresponding scaling transformation type */
+  typedef Scaling<Scalar,Dim> ScalingType;
+
+protected:
+
+  MatrixType m_matrix;
+
+public:
+
+  /** Default constructor without initialization of the coefficients. */
+  inline Transform() { }
+
+  inline Transform(const Transform& other)
+  {
+    m_matrix = other.m_matrix;
+  }
+
+  inline explicit Transform(const TranslationType& t) { *this = t; }
+  inline explicit Transform(const ScalingType& s) { *this = s; }
+  template<typename Derived>
+  inline explicit Transform(const RotationBase<Derived, Dim>& r) { *this = r; }
+
+  inline Transform& operator=(const Transform& other)
+  { m_matrix = other.m_matrix; return *this; }
+
+  template<typename OtherDerived, bool BigMatrix> // MSVC 2005 will commit suicide if BigMatrix has a default value
+  struct construct_from_matrix
+  {
+    static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other)
+    {
+      transform->matrix() = other;
+    }
+  };
+
+  template<typename OtherDerived> struct construct_from_matrix<OtherDerived, true>
+  {
+    static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other)
+    {
+      transform->linear() = other;
+      transform->translation().setZero();
+      transform->matrix()(Dim,Dim) = Scalar(1);
+      transform->matrix().template block<1,Dim>(Dim,0).setZero();
+    }
+  };
+
+  /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */
+  template<typename OtherDerived>
+  inline explicit Transform(const MatrixBase<OtherDerived>& other)
+  {
+    construct_from_matrix<OtherDerived, int(OtherDerived::RowsAtCompileTime) == Dim>::run(this, other);
+  }
+
+  /** Set \c *this from a (Dim+1)^2 matrix. */
+  template<typename OtherDerived>
+  inline Transform& operator=(const MatrixBase<OtherDerived>& other)
+  { m_matrix = other; return *this; }
+
+  #ifdef EIGEN_QT_SUPPORT
+  inline Transform(const QMatrix& other);
+  inline Transform& operator=(const QMatrix& other);
+  inline QMatrix toQMatrix(void) const;
+  inline Transform(const QTransform& other);
+  inline Transform& operator=(const QTransform& other);
+  inline QTransform toQTransform(void) const;
+  #endif
+
+  /** shortcut for m_matrix(row,col);
+    * \sa MatrixBase::operaror(int,int) const */
+  inline Scalar operator() (int row, int col) const { return m_matrix(row,col); }
+  /** shortcut for m_matrix(row,col);
+    * \sa MatrixBase::operaror(int,int) */
+  inline Scalar& operator() (int row, int col) { return m_matrix(row,col); }
+
+  /** \returns a read-only expression of the transformation matrix */
+  inline const MatrixType& matrix() const { return m_matrix; }
+  /** \returns a writable expression of the transformation matrix */
+  inline MatrixType& matrix() { return m_matrix; }
+
+  /** \returns a read-only expression of the linear (linear) part of the transformation */
+  inline const LinearPart linear() const { return m_matrix.template block<Dim,Dim>(0,0); }
+  /** \returns a writable expression of the linear (linear) part of the transformation */
+  inline LinearPart linear() { return m_matrix.template block<Dim,Dim>(0,0); }
+
+  /** \returns a read-only expression of the translation vector of the transformation */
+  inline const TranslationPart translation() const { return m_matrix.template block<Dim,1>(0,Dim); }
+  /** \returns a writable expression of the translation vector of the transformation */
+  inline TranslationPart translation() { return m_matrix.template block<Dim,1>(0,Dim); }
+
+  /** \returns an expression of the product between the transform \c *this and a matrix expression \a other
+  *
+  * The right hand side \a other might be either:
+  * \li a vector of size Dim,
+  * \li an homogeneous vector of size Dim+1,
+  * \li a transformation matrix of size Dim+1 x Dim+1.
+  */
+  // note: this function is defined here because some compilers cannot find the respective declaration
+  template<typename OtherDerived>
+  inline const typename ei_transform_product_impl<OtherDerived,_Dim,_Dim+1>::ResultType
+  operator * (const MatrixBase<OtherDerived> &other) const
+  { return ei_transform_product_impl<OtherDerived,Dim,HDim>::run(*this,other.derived()); }
+
+  /** \returns the product expression of a transformation matrix \a a times a transform \a b
+    * The transformation matrix \a a must have a Dim+1 x Dim+1 sizes. */
+  template<typename OtherDerived>
+  friend inline const typename ProductReturnType<OtherDerived,MatrixType>::Type
+  operator * (const MatrixBase<OtherDerived> &a, const Transform &b)
+  { return a.derived() * b.matrix(); }
+
+  /** Contatenates two transformations */
+  inline const Transform
+  operator * (const Transform& other) const
+  { return Transform(m_matrix * other.matrix()); }
+
+  /** \sa MatrixBase::setIdentity() */
+  void setIdentity() { m_matrix.setIdentity(); }
+  static const typename MatrixType::IdentityReturnType Identity()
+  {
+    return MatrixType::Identity();
+  }
+
+  template<typename OtherDerived>
+  inline Transform& scale(const MatrixBase<OtherDerived> &other);
+
+  template<typename OtherDerived>
+  inline Transform& prescale(const MatrixBase<OtherDerived> &other);
+
+  inline Transform& scale(Scalar s);
+  inline Transform& prescale(Scalar s);
+
+  template<typename OtherDerived>
+  inline Transform& translate(const MatrixBase<OtherDerived> &other);
+
+  template<typename OtherDerived>
+  inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);
+
+  template<typename RotationType>
+  inline Transform& rotate(const RotationType& rotation);
+
+  template<typename RotationType>
+  inline Transform& prerotate(const RotationType& rotation);
+
+  Transform& shear(Scalar sx, Scalar sy);
+  Transform& preshear(Scalar sx, Scalar sy);
+
+  inline Transform& operator=(const TranslationType& t);
+  inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); }
+  inline Transform operator*(const TranslationType& t) const;
+
+  inline Transform& operator=(const ScalingType& t);
+  inline Transform& operator*=(const ScalingType& s) { return scale(s.coeffs()); }
+  inline Transform operator*(const ScalingType& s) const;
+  friend inline Transform operator*(const LinearMatrixType& mat, const Transform& t)
+  {
+    Transform res = t;
+    res.matrix().row(Dim) = t.matrix().row(Dim);
+    res.matrix().template block<Dim,HDim>(0,0) = (mat * t.matrix().template block<Dim,HDim>(0,0)).lazy();
+    return res;
+  }
+
+  template<typename Derived>
+  inline Transform& operator=(const RotationBase<Derived,Dim>& r);
+  template<typename Derived>
+  inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); }
+  template<typename Derived>
+  inline Transform operator*(const RotationBase<Derived,Dim>& r) const;
+
+  LinearMatrixType rotation() const;
+  template<typename RotationMatrixType, typename ScalingMatrixType>
+  void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const;
+  template<typename ScalingMatrixType, typename RotationMatrixType>
+  void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const;
+
+  template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
+  Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
+    const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
+
+  inline const MatrixType inverse(TransformTraits traits = Affine) const;
+
+  /** \returns a const pointer to the column major internal matrix */
+  const Scalar* data() const { return m_matrix.data(); }
+  /** \returns a non-const pointer to the column major internal matrix */
+  Scalar* data() { return m_matrix.data(); }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Transform,Transform<NewScalarType,Dim> >::type cast() const
+  { return typename ei_cast_return_type<Transform,Transform<NewScalarType,Dim> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Transform(const Transform<OtherScalarType,Dim>& other)
+  { m_matrix = other.matrix().template cast<Scalar>(); }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Transform& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_matrix.isApprox(other.m_matrix, prec); }
+
+  #ifdef EIGEN_TRANSFORM_PLUGIN
+  #include EIGEN_TRANSFORM_PLUGIN
+  #endif
+
+protected:
+
+};
+
+/** \ingroup Geometry_Module */
+typedef Transform<float,2> Transform2f;
+/** \ingroup Geometry_Module */
+typedef Transform<float,3> Transform3f;
+/** \ingroup Geometry_Module */
+typedef Transform<double,2> Transform2d;
+/** \ingroup Geometry_Module */
+typedef Transform<double,3> Transform3d;
+
+/**************************
+*** Optional QT support ***
+**************************/
+
+#ifdef EIGEN_QT_SUPPORT
+/** Initialises \c *this from a QMatrix assuming the dimension is 2.
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>::Transform(const QMatrix& other)
+{
+  *this = other;
+}
+
+/** Set \c *this from a QMatrix assuming the dimension is 2.
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QMatrix& other)
+{
+  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  m_matrix << other.m11(), other.m21(), other.dx(),
+              other.m12(), other.m22(), other.dy(),
+              0, 0, 1;
+   return *this;
+}
+
+/** \returns a QMatrix from \c *this assuming the dimension is 2.
+  *
+  * \warning this convertion might loss data if \c *this is not affine
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+QMatrix Transform<Scalar,Dim>::toQMatrix(void) const
+{
+  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
+                 m_matrix.coeff(0,1), m_matrix.coeff(1,1),
+                 m_matrix.coeff(0,2), m_matrix.coeff(1,2));
+}
+
+/** Initialises \c *this from a QTransform assuming the dimension is 2.
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>::Transform(const QTransform& other)
+{
+  *this = other;
+}
+
+/** Set \c *this from a QTransform assuming the dimension is 2.
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QTransform& other)
+{
+  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  m_matrix << other.m11(), other.m21(), other.dx(),
+              other.m12(), other.m22(), other.dy(),
+              other.m13(), other.m23(), other.m33();
+   return *this;
+}
+
+/** \returns a QTransform from \c *this assuming the dimension is 2.
+  *
+  * This function is available only if the token EIGEN_QT_SUPPORT is defined.
+  */
+template<typename Scalar, int Dim>
+QTransform Transform<Scalar,Dim>::toQTransform(void) const
+{
+  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0),
+                    m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1),
+                    m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2));
+}
+#endif
+
+/*********************
+*** Procedural API ***
+*********************/
+
+/** Applies on the right the non uniform scale transformation represented
+  * by the vector \a other to \c *this and returns a reference to \c *this.
+  * \sa prescale()
+  */
+template<typename Scalar, int Dim>
+template<typename OtherDerived>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
+  linear() = (linear() * other.asDiagonal()).lazy();
+  return *this;
+}
+
+/** Applies on the right a uniform scale of a factor \a c to \c *this
+  * and returns a reference to \c *this.
+  * \sa prescale(Scalar)
+  */
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::scale(Scalar s)
+{
+  linear() *= s;
+  return *this;
+}
+
+/** Applies on the left the non uniform scale transformation represented
+  * by the vector \a other to \c *this and returns a reference to \c *this.
+  * \sa scale()
+  */
+template<typename Scalar, int Dim>
+template<typename OtherDerived>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
+  m_matrix.template block<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)).lazy();
+  return *this;
+}
+
+/** Applies on the left a uniform scale of a factor \a c to \c *this
+  * and returns a reference to \c *this.
+  * \sa scale(Scalar)
+  */
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::prescale(Scalar s)
+{
+  m_matrix.template corner<Dim,HDim>(TopLeft) *= s;
+  return *this;
+}
+
+/** Applies on the right the translation matrix represented by the vector \a other
+  * to \c *this and returns a reference to \c *this.
+  * \sa pretranslate()
+  */
+template<typename Scalar, int Dim>
+template<typename OtherDerived>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
+  translation() += linear() * other;
+  return *this;
+}
+
+/** Applies on the left the translation matrix represented by the vector \a other
+  * to \c *this and returns a reference to \c *this.
+  * \sa translate()
+  */
+template<typename Scalar, int Dim>
+template<typename OtherDerived>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
+  translation() += other;
+  return *this;
+}
+
+/** Applies on the right the rotation represented by the rotation \a rotation
+  * to \c *this and returns a reference to \c *this.
+  *
+  * The template parameter \a RotationType is the type of the rotation which
+  * must be known by ei_toRotationMatrix<>.
+  *
+  * Natively supported types includes:
+  *   - any scalar (2D),
+  *   - a Dim x Dim matrix expression,
+  *   - a Quaternion (3D),
+  *   - a AngleAxis (3D)
+  *
+  * This mechanism is easily extendable to support user types such as Euler angles,
+  * or a pair of Quaternion for 4D rotations.
+  *
+  * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType)
+  */
+template<typename Scalar, int Dim>
+template<typename RotationType>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::rotate(const RotationType& rotation)
+{
+  linear() *= ei_toRotationMatrix<Scalar,Dim>(rotation);
+  return *this;
+}
+
+/** Applies on the left the rotation represented by the rotation \a rotation
+  * to \c *this and returns a reference to \c *this.
+  *
+  * See rotate() for further details.
+  *
+  * \sa rotate()
+  */
+template<typename Scalar, int Dim>
+template<typename RotationType>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::prerotate(const RotationType& rotation)
+{
+  m_matrix.template block<Dim,HDim>(0,0) = ei_toRotationMatrix<Scalar,Dim>(rotation)
+                                         * m_matrix.template block<Dim,HDim>(0,0);
+  return *this;
+}
+
+/** Applies on the right the shear transformation represented
+  * by the vector \a other to \c *this and returns a reference to \c *this.
+  * \warning 2D only.
+  * \sa preshear()
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::shear(Scalar sx, Scalar sy)
+{
+  EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  VectorType tmp = linear().col(0)*sy + linear().col(1);
+  linear() << linear().col(0) + linear().col(1)*sx, tmp;
+  return *this;
+}
+
+/** Applies on the left the shear transformation represented
+  * by the vector \a other to \c *this and returns a reference to \c *this.
+  * \warning 2D only.
+  * \sa shear()
+  */
+template<typename Scalar, int Dim>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::preshear(Scalar sx, Scalar sy)
+{
+  EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
+  m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
+  return *this;
+}
+
+/******************************************************
+*** Scaling, Translation and Rotation compatibility ***
+******************************************************/
+
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const TranslationType& t)
+{
+  linear().setIdentity();
+  translation() = t.vector();
+  m_matrix.template block<1,Dim>(Dim,0).setZero();
+  m_matrix(Dim,Dim) = Scalar(1);
+  return *this;
+}
+
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const TranslationType& t) const
+{
+  Transform res = *this;
+  res.translate(t.vector());
+  return res;
+}
+
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const ScalingType& s)
+{
+  m_matrix.setZero();
+  linear().diagonal() = s.coeffs();
+  m_matrix.coeffRef(Dim,Dim) = Scalar(1);
+  return *this;
+}
+
+template<typename Scalar, int Dim>
+inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const ScalingType& s) const
+{
+  Transform res = *this;
+  res.scale(s.coeffs());
+  return res;
+}
+
+template<typename Scalar, int Dim>
+template<typename Derived>
+inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const RotationBase<Derived,Dim>& r)
+{
+  linear() = ei_toRotationMatrix<Scalar,Dim>(r);
+  translation().setZero();
+  m_matrix.template block<1,Dim>(Dim,0).setZero();
+  m_matrix.coeffRef(Dim,Dim) = Scalar(1);
+  return *this;
+}
+
+template<typename Scalar, int Dim>
+template<typename Derived>
+inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const RotationBase<Derived,Dim>& r) const
+{
+  Transform res = *this;
+  res.rotate(r.derived());
+  return res;
+}
+
+/************************
+*** Special functions ***
+************************/
+
+/** \returns the rotation part of the transformation
+  * \nonstableyet
+  *
+  * \svd_module
+  *
+  * \sa computeRotationScaling(), computeScalingRotation(), class SVD
+  */
+template<typename Scalar, int Dim>
+typename Transform<Scalar,Dim>::LinearMatrixType
+Transform<Scalar,Dim>::rotation() const
+{
+  LinearMatrixType result;
+  computeRotationScaling(&result, (LinearMatrixType*)0);
+  return result;
+}
+
+
+/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
+  * not necessarily positive.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * \nonstableyet
+  *
+  * \svd_module
+  *
+  * \sa computeScalingRotation(), rotation(), class SVD
+  */
+template<typename Scalar, int Dim>
+template<typename RotationMatrixType, typename ScalingMatrixType>
+void Transform<Scalar,Dim>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
+{
+  linear().svd().computeRotationScaling(rotation, scaling);
+}
+
+/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
+  * not necessarily positive.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * \nonstableyet
+  *
+  * \svd_module
+  *
+  * \sa computeRotationScaling(), rotation(), class SVD
+  */
+template<typename Scalar, int Dim>
+template<typename ScalingMatrixType, typename RotationMatrixType>
+void Transform<Scalar,Dim>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
+{
+  linear().svd().computeScalingRotation(scaling, rotation);
+}
+
+/** Convenient method to set \c *this from a position, orientation and scale
+  * of a 3D object.
+  */
+template<typename Scalar, int Dim>
+template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
+Transform<Scalar,Dim>&
+Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
+  const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
+{
+  linear() = ei_toRotationMatrix<Scalar,Dim>(orientation);
+  linear() *= scale.asDiagonal();
+  translation() = position;
+  m_matrix.template block<1,Dim>(Dim,0).setZero();
+  m_matrix(Dim,Dim) = Scalar(1);
+  return *this;
+}
+
+/** \nonstableyet
+  *
+  * \returns the inverse transformation matrix according to some given knowledge
+  * on \c *this.
+  *
+  * \param traits allows to optimize the inversion process when the transformion
+  * is known to be not a general transformation. The possible values are:
+  *  - Projective if the transformation is not necessarily affine, i.e., if the
+  *    last row is not guaranteed to be [0 ... 0 1]
+  *  - Affine is the default, the last row is assumed to be [0 ... 0 1]
+  *  - Isometry if the transformation is only a concatenations of translations
+  *    and rotations.
+  *
+  * \warning unless \a traits is always set to NoShear or NoScaling, this function
+  * requires the generic inverse method of MatrixBase defined in the LU module. If
+  * you forget to include this module, then you will get hard to debug linking errors.
+  *
+  * \sa MatrixBase::inverse()
+  */
+template<typename Scalar, int Dim>
+inline const typename Transform<Scalar,Dim>::MatrixType
+Transform<Scalar,Dim>::inverse(TransformTraits traits) const
+{
+  if (traits == Projective)
+  {
+    return m_matrix.inverse();
+  }
+  else
+  {
+    MatrixType res;
+    if (traits == Affine)
+    {
+      res.template corner<Dim,Dim>(TopLeft) = linear().inverse();
+    }
+    else if (traits == Isometry)
+    {
+      res.template corner<Dim,Dim>(TopLeft) = linear().transpose();
+    }
+    else
+    {
+      ei_assert("invalid traits value in Transform::inverse()");
+    }
+    // translation and remaining parts
+    res.template corner<Dim,1>(TopRight) = - res.template corner<Dim,Dim>(TopLeft) * translation();
+    res.template corner<1,Dim>(BottomLeft).setZero();
+    res.coeffRef(Dim,Dim) = Scalar(1);
+    return res;
+  }
+}
+
+/*****************************************************
+*** Specializations of operator* with a MatrixBase ***
+*****************************************************/
+
+template<typename Other, int Dim, int HDim>
+struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim>
+{
+  typedef Transform<typename Other::Scalar,Dim> TransformType;
+  typedef typename TransformType::MatrixType MatrixType;
+  typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
+  static ResultType run(const TransformType& tr, const Other& other)
+  { return tr.matrix() * other; }
+};
+
+template<typename Other, int Dim, int HDim>
+struct ei_transform_product_impl<Other,Dim,HDim, Dim,Dim>
+{
+  typedef Transform<typename Other::Scalar,Dim> TransformType;
+  typedef typename TransformType::MatrixType MatrixType;
+  typedef TransformType ResultType;
+  static ResultType run(const TransformType& tr, const Other& other)
+  {
+    TransformType res;
+    res.translation() = tr.translation();
+    res.matrix().row(Dim) = tr.matrix().row(Dim);
+    res.linear() = (tr.linear() * other).lazy();
+    return res;
+  }
+};
+
+template<typename Other, int Dim, int HDim>
+struct ei_transform_product_impl<Other,Dim,HDim, HDim,1>
+{
+  typedef Transform<typename Other::Scalar,Dim> TransformType;
+  typedef typename TransformType::MatrixType MatrixType;
+  typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
+  static ResultType run(const TransformType& tr, const Other& other)
+  { return tr.matrix() * other; }
+};
+
+template<typename Other, int Dim, int HDim>
+struct ei_transform_product_impl<Other,Dim,HDim, Dim,1>
+{
+  typedef typename Other::Scalar Scalar;
+  typedef Transform<Scalar,Dim> TransformType;
+  typedef typename TransformType::LinearPart MatrixType;
+  typedef const CwiseUnaryOp<
+      ei_scalar_multiple_op<Scalar>,
+      NestByValue<CwiseBinaryOp<
+        ei_scalar_sum_op<Scalar>,
+        NestByValue<typename ProductReturnType<NestByValue<MatrixType>,Other>::Type >,
+        NestByValue<typename TransformType::TranslationPart> > >
+      > ResultType;
+  // FIXME should we offer an optimized version when the last row is known to be 0,0...,0,1 ?
+  static ResultType run(const TransformType& tr, const Other& other)
+  { return ((tr.linear().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue()
+          * (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); }
+};
+
+#endif // EIGEN_TRANSFORM_H
diff --git a/extern/Eigen2/Eigen/src/Geometry/Translation.h b/extern/Eigen2/Eigen/src/Geometry/Translation.h
new file mode 100644
index 00000000000..4b2fc7a56fc
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Geometry/Translation.h
@@ -0,0 +1,198 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_TRANSLATION_H
+#define EIGEN_TRANSLATION_H
+
+/** \geometry_module \ingroup Geometry_Module
+  *
+  * \class Translation
+  *
+  * \brief Represents a translation transformation
+  *
+  * \param _Scalar the scalar type, i.e., the type of the coefficients.
+  * \param _Dim the  dimension of the space, can be a compile time value or Dynamic
+  *
+  * \note This class is not aimed to be used to store a translation transformation,
+  * but rather to make easier the constructions and updates of Transform objects.
+  *
+  * \sa class Scaling, class Transform
+  */
+template<typename _Scalar, int _Dim>
+class Translation
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim)
+  /** dimension of the space */
+  enum { Dim = _Dim };
+  /** the scalar type of the coefficients */
+  typedef _Scalar Scalar;
+  /** corresponding vector type */
+  typedef Matrix<Scalar,Dim,1> VectorType;
+  /** corresponding linear transformation matrix type */
+  typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
+  /** corresponding scaling transformation type */
+  typedef Scaling<Scalar,Dim> ScalingType;
+  /** corresponding affine transformation type */
+  typedef Transform<Scalar,Dim> TransformType;
+
+protected:
+
+  VectorType m_coeffs;
+
+public:
+
+  /** Default constructor without initialization. */
+  Translation() {}
+  /**  */
+  inline Translation(const Scalar& sx, const Scalar& sy)
+  {
+    ei_assert(Dim==2);
+    m_coeffs.x() = sx;
+    m_coeffs.y() = sy;
+  }
+  /**  */
+  inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz)
+  {
+    ei_assert(Dim==3);
+    m_coeffs.x() = sx;
+    m_coeffs.y() = sy;
+    m_coeffs.z() = sz;
+  }
+  /** Constructs and initialize the scaling transformation from a vector of scaling coefficients */
+  explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {}
+
+  const VectorType& vector() const { return m_coeffs; }
+  VectorType& vector() { return m_coeffs; }
+
+  /** Concatenates two translation */
+  inline Translation operator* (const Translation& other) const
+  { return Translation(m_coeffs + other.m_coeffs); }
+
+  /** Concatenates a translation and a scaling */
+  inline TransformType operator* (const ScalingType& other) const;
+
+  /** Concatenates a translation and a linear transformation */
+  inline TransformType operator* (const LinearMatrixType& linear) const;
+
+  template<typename Derived>
+  inline TransformType operator*(const RotationBase<Derived,Dim>& r) const
+  { return *this * r.toRotationMatrix(); }
+
+  /** Concatenates a linear transformation and a translation */
+  // its a nightmare to define a templated friend function outside its declaration
+  friend inline TransformType operator* (const LinearMatrixType& linear, const Translation& t)
+  {
+    TransformType res;
+    res.matrix().setZero();
+    res.linear() = linear;
+    res.translation() = linear * t.m_coeffs;
+    res.matrix().row(Dim).setZero();
+    res(Dim,Dim) = Scalar(1);
+    return res;
+  }
+
+  /** Concatenates a translation and an affine transformation */
+  inline TransformType operator* (const TransformType& t) const;
+
+  /** Applies translation to vector */
+  inline VectorType operator* (const VectorType& other) const
+  { return m_coeffs + other; }
+
+  /** \returns the inverse translation (opposite) */
+  Translation inverse() const { return Translation(-m_coeffs); }
+
+  Translation& operator=(const Translation& other)
+  {
+    m_coeffs = other.m_coeffs;
+    return *this;
+  }
+
+  /** \returns \c *this with scalar type casted to \a NewScalarType
+    *
+    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
+    * then this function smartly returns a const reference to \c *this.
+    */
+  template<typename NewScalarType>
+  inline typename ei_cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const
+  { return typename ei_cast_return_type<Translation,Translation<NewScalarType,Dim> >::type(*this); }
+
+  /** Copy constructor with scalar type conversion */
+  template<typename OtherScalarType>
+  inline explicit Translation(const Translation<OtherScalarType,Dim>& other)
+  { m_coeffs = other.vector().template cast<Scalar>(); }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+    * determined by \a prec.
+    *
+    * \sa MatrixBase::isApprox() */
+  bool isApprox(const Translation& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
+  { return m_coeffs.isApprox(other.m_coeffs, prec); }
+
+};
+
+/** \addtogroup Geometry_Module */
+//@{
+typedef Translation<float, 2> Translation2f;
+typedef Translation<double,2> Translation2d;
+typedef Translation<float, 3> Translation3f;
+typedef Translation<double,3> Translation3d;
+//@}
+
+
+template<typename Scalar, int Dim>
+inline typename Translation<Scalar,Dim>::TransformType
+Translation<Scalar,Dim>::operator* (const ScalingType& other) const
+{
+  TransformType res;
+  res.matrix().setZero();
+  res.linear().diagonal() = other.coeffs();
+  res.translation() = m_coeffs;
+  res(Dim,Dim) = Scalar(1);
+  return res;
+}
+
+template<typename Scalar, int Dim>
+inline typename Translation<Scalar,Dim>::TransformType
+Translation<Scalar,Dim>::operator* (const LinearMatrixType& linear) const
+{
+  TransformType res;
+  res.matrix().setZero();
+  res.linear() = linear;
+  res.translation() = m_coeffs;
+  res.matrix().row(Dim).setZero();
+  res(Dim,Dim) = Scalar(1);
+  return res;
+}
+
+template<typename Scalar, int Dim>
+inline typename Translation<Scalar,Dim>::TransformType
+Translation<Scalar,Dim>::operator* (const TransformType& t) const
+{
+  TransformType res = t;
+  res.pretranslate(m_coeffs);
+  return res;
+}
+
+#endif // EIGEN_TRANSLATION_H
diff --git a/extern/Eigen2/Eigen/src/LU/Determinant.h b/extern/Eigen2/Eigen/src/LU/Determinant.h
new file mode 100644
index 00000000000..4f435054ac6
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/LU/Determinant.h
@@ -0,0 +1,122 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DETERMINANT_H
+#define EIGEN_DETERMINANT_H
+
+template<typename Derived>
+inline const typename Derived::Scalar ei_bruteforce_det3_helper
+(const MatrixBase<Derived>& matrix, int a, int b, int c)
+{
+  return matrix.coeff(0,a)
+         * (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b));
+}
+
+template<typename Derived>
+const typename Derived::Scalar ei_bruteforce_det4_helper
+(const MatrixBase<Derived>& matrix, int j, int k, int m, int n)
+{
+  return (matrix.coeff(j,0) * matrix.coeff(k,1) - matrix.coeff(k,0) * matrix.coeff(j,1))
+       * (matrix.coeff(m,2) * matrix.coeff(n,3) - matrix.coeff(n,2) * matrix.coeff(m,3));
+}
+
+const int TriangularDeterminant = 0;
+
+template<typename Derived,
+         int DeterminantType =
+           (Derived::Flags & (UpperTriangularBit | LowerTriangularBit))
+           ? TriangularDeterminant : Derived::RowsAtCompileTime
+> struct ei_determinant_impl
+{
+  static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    return m.lu().determinant();
+  }
+};
+
+template<typename Derived> struct ei_determinant_impl<Derived, TriangularDeterminant>
+{
+  static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    if (Derived::Flags & UnitDiagBit)
+      return 1;
+    else if (Derived::Flags & ZeroDiagBit)
+      return 0;
+    else
+      return m.diagonal().redux(ei_scalar_product_op<typename ei_traits<Derived>::Scalar>());
+  }
+};
+
+template<typename Derived> struct ei_determinant_impl<Derived, 1>
+{
+  static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    return m.coeff(0,0);
+  }
+};
+
+template<typename Derived> struct ei_determinant_impl<Derived, 2>
+{
+  static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1);
+  }
+};
+
+template<typename Derived> struct ei_determinant_impl<Derived, 3>
+{
+  static typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    return ei_bruteforce_det3_helper(m,0,1,2)
+          - ei_bruteforce_det3_helper(m,1,0,2)
+          + ei_bruteforce_det3_helper(m,2,0,1);
+  }
+};
+
+template<typename Derived> struct ei_determinant_impl<Derived, 4>
+{
+  static typename ei_traits<Derived>::Scalar run(const Derived& m)
+  {
+    // trick by Martin Costabel to compute 4x4 det with only 30 muls
+    return ei_bruteforce_det4_helper(m,0,1,2,3)
+          - ei_bruteforce_det4_helper(m,0,2,1,3)
+          + ei_bruteforce_det4_helper(m,0,3,1,2)
+          + ei_bruteforce_det4_helper(m,1,2,0,3)
+          - ei_bruteforce_det4_helper(m,1,3,0,2)
+          + ei_bruteforce_det4_helper(m,2,3,0,1);
+  }
+};
+
+/** \lu_module
+  *
+  * \returns the determinant of this matrix
+  */
+template<typename Derived>
+inline typename ei_traits<Derived>::Scalar MatrixBase<Derived>::determinant() const
+{
+  assert(rows() == cols());
+  return ei_determinant_impl<Derived>::run(derived());
+}
+
+#endif // EIGEN_DETERMINANT_H
diff --git a/extern/Eigen2/Eigen/src/LU/Inverse.h b/extern/Eigen2/Eigen/src/LU/Inverse.h
new file mode 100644
index 00000000000..3d4d6348949
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/LU/Inverse.h
@@ -0,0 +1,258 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_INVERSE_H
+#define EIGEN_INVERSE_H
+
+/********************************************************************
+*** Part 1 : optimized implementations for fixed-size 2,3,4 cases ***
+********************************************************************/
+
+template<typename MatrixType>
+void ei_compute_inverse_in_size2_case(const MatrixType& matrix, MatrixType* result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  const Scalar invdet = Scalar(1) / matrix.determinant();
+  result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
+  result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
+  result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
+  result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
+}
+
+template<typename XprType, typename MatrixType>
+bool ei_compute_inverse_in_size2_case_with_check(const XprType& matrix, MatrixType* result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  const Scalar det = matrix.determinant();
+  if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
+  const Scalar invdet = Scalar(1) / det;
+  result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
+  result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
+  result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
+  result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
+  return true;
+}
+
+template<typename MatrixType>
+void ei_compute_inverse_in_size3_case(const MatrixType& matrix, MatrixType* result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  const Scalar det_minor00 = matrix.minor(0,0).determinant();
+  const Scalar det_minor10 = matrix.minor(1,0).determinant();
+  const Scalar det_minor20 = matrix.minor(2,0).determinant();
+  const Scalar invdet = Scalar(1) / ( det_minor00 * matrix.coeff(0,0)
+                                    - det_minor10 * matrix.coeff(1,0)
+                                    + det_minor20 * matrix.coeff(2,0) );
+  result->coeffRef(0, 0) = det_minor00 * invdet;
+  result->coeffRef(0, 1) = -det_minor10 * invdet;
+  result->coeffRef(0, 2) = det_minor20 * invdet;
+  result->coeffRef(1, 0) = -matrix.minor(0,1).determinant() * invdet;
+  result->coeffRef(1, 1) = matrix.minor(1,1).determinant() * invdet;
+  result->coeffRef(1, 2) = -matrix.minor(2,1).determinant() * invdet;
+  result->coeffRef(2, 0) = matrix.minor(0,2).determinant() * invdet;
+  result->coeffRef(2, 1) = -matrix.minor(1,2).determinant() * invdet;
+  result->coeffRef(2, 2) = matrix.minor(2,2).determinant() * invdet;
+}
+
+template<typename MatrixType>
+bool ei_compute_inverse_in_size4_case_helper(const MatrixType& matrix, MatrixType* result)
+{
+  /* Let's split M into four 2x2 blocks:
+    * (P Q)
+    * (R S)
+    * If P is invertible, with inverse denoted by P_inverse, and if
+    * (S - R*P_inverse*Q) is also invertible, then the inverse of M is
+    * (P' Q')
+    * (R' S')
+    * where
+    * S' = (S - R*P_inverse*Q)^(-1)
+    * P' = P1 + (P1*Q) * S' *(R*P_inverse)
+    * Q' = -(P_inverse*Q) * S'
+    * R' = -S' * (R*P_inverse)
+    */
+  typedef Block<MatrixType,2,2> XprBlock22;
+  typedef typename MatrixBase<XprBlock22>::PlainMatrixType Block22;
+  Block22 P_inverse;
+  if(ei_compute_inverse_in_size2_case_with_check(matrix.template block<2,2>(0,0), &P_inverse))
+  {
+    const Block22 Q = matrix.template block<2,2>(0,2);
+    const Block22 P_inverse_times_Q = P_inverse * Q;
+    const XprBlock22 R = matrix.template block<2,2>(2,0);
+    const Block22 R_times_P_inverse = R * P_inverse;
+    const Block22 R_times_P_inverse_times_Q = R_times_P_inverse * Q;
+    const XprBlock22 S = matrix.template block<2,2>(2,2);
+    const Block22 X = S - R_times_P_inverse_times_Q;
+    Block22 Y;
+    ei_compute_inverse_in_size2_case(X, &Y);
+    result->template block<2,2>(2,2) = Y;
+    result->template block<2,2>(2,0) = - Y * R_times_P_inverse;
+    const Block22 Z = P_inverse_times_Q * Y;
+    result->template block<2,2>(0,2) = - Z;
+    result->template block<2,2>(0,0) = P_inverse + Z * R_times_P_inverse;
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+template<typename MatrixType>
+void ei_compute_inverse_in_size4_case(const MatrixType& matrix, MatrixType* result)
+{
+  if(ei_compute_inverse_in_size4_case_helper(matrix, result))
+  {
+    // good ! The topleft 2x2 block was invertible, so the 2x2 blocks approach is successful.
+    return;
+  }
+  else
+  {
+    // rare case: the topleft 2x2 block is not invertible (but the matrix itself is assumed to be).
+    // since this is a rare case, we don't need to optimize it. We just want to handle it with little
+    // additional code.
+    MatrixType m(matrix);
+    m.row(0).swap(m.row(2));
+    m.row(1).swap(m.row(3));
+    if(ei_compute_inverse_in_size4_case_helper(m, result))
+    {
+      // good, the topleft 2x2 block of m is invertible. Since m is different from matrix in that some
+      // rows were permuted, the actual inverse of matrix is derived from the inverse of m by permuting
+      // the corresponding columns.
+      result->col(0).swap(result->col(2));
+      result->col(1).swap(result->col(3));
+    }
+    else
+    {
+      // last possible case. Since matrix is assumed to be invertible, this last case has to work.
+      // first, undo the swaps previously made
+      m.row(0).swap(m.row(2));
+      m.row(1).swap(m.row(3));
+      // swap row 0 with the the row among 0 and 1 that has the biggest 2 first coeffs
+      int swap0with = ei_abs(m.coeff(0,0))+ei_abs(m.coeff(0,1))>ei_abs(m.coeff(1,0))+ei_abs(m.coeff(1,1)) ? 0 : 1;
+      m.row(0).swap(m.row(swap0with));
+      // swap row 1 with the the row among 2 and 3 that has the biggest 2 first coeffs
+      int swap1with = ei_abs(m.coeff(2,0))+ei_abs(m.coeff(2,1))>ei_abs(m.coeff(3,0))+ei_abs(m.coeff(3,1)) ? 2 : 3;
+      m.row(1).swap(m.row(swap1with));
+      ei_compute_inverse_in_size4_case_helper(m, result);
+      result->col(1).swap(result->col(swap1with));
+      result->col(0).swap(result->col(swap0with));
+    }
+  }
+}
+
+/***********************************************
+*** Part 2 : selector and MatrixBase methods ***
+***********************************************/
+
+template<typename MatrixType, int Size = MatrixType::RowsAtCompileTime>
+struct ei_compute_inverse
+{
+  static inline void run(const MatrixType& matrix, MatrixType* result)
+  {
+    LU<MatrixType> lu(matrix);
+    lu.computeInverse(result);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse<MatrixType, 1>
+{
+  static inline void run(const MatrixType& matrix, MatrixType* result)
+  {
+    typedef typename MatrixType::Scalar Scalar;
+    result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse<MatrixType, 2>
+{
+  static inline void run(const MatrixType& matrix, MatrixType* result)
+  {
+    ei_compute_inverse_in_size2_case(matrix, result);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse<MatrixType, 3>
+{
+  static inline void run(const MatrixType& matrix, MatrixType* result)
+  {
+    ei_compute_inverse_in_size3_case(matrix, result);
+  }
+};
+
+template<typename MatrixType>
+struct ei_compute_inverse<MatrixType, 4>
+{
+  static inline void run(const MatrixType& matrix, MatrixType* result)
+  {
+    ei_compute_inverse_in_size4_case(matrix, result);
+  }
+};
+
+/** \lu_module
+  *
+  * Computes the matrix inverse of this matrix.
+  *
+  * \note This matrix must be invertible, otherwise the result is undefined.
+  *
+  * \param result Pointer to the matrix in which to store the result.
+  *
+  * Example: \include MatrixBase_computeInverse.cpp
+  * Output: \verbinclude MatrixBase_computeInverse.out
+  *
+  * \sa inverse()
+  */
+template<typename Derived>
+inline void MatrixBase<Derived>::computeInverse(PlainMatrixType *result) const
+{
+  ei_assert(rows() == cols());
+  EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
+  ei_compute_inverse<PlainMatrixType>::run(eval(), result);
+}
+
+/** \lu_module
+  *
+  * \returns the matrix inverse of this matrix.
+  *
+  * \note This matrix must be invertible, otherwise the result is undefined.
+  *
+  * \note This method returns a matrix by value, which can be inefficient. To avoid that overhead,
+  * use computeInverse() instead.
+  *
+  * Example: \include MatrixBase_inverse.cpp
+  * Output: \verbinclude MatrixBase_inverse.out
+  *
+  * \sa computeInverse()
+  */
+template<typename Derived>
+inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::inverse() const
+{
+  PlainMatrixType result(rows(), cols());
+  computeInverse(&result);
+  return result;
+}
+
+#endif // EIGEN_INVERSE_H
diff --git a/extern/Eigen2/Eigen/src/LU/LU.h b/extern/Eigen2/Eigen/src/LU/LU.h
new file mode 100644
index 00000000000..176e76a91a3
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/LU/LU.h
@@ -0,0 +1,541 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_LU_H
+#define EIGEN_LU_H
+
+/** \ingroup LU_Module
+  *
+  * \class LU
+  *
+  * \brief LU decomposition of a matrix with complete pivoting, and related features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LU decomposition
+  *
+  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A
+  * is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q
+  * are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal
+  * coefficients) of U are sorted in such a way that any zeros are at the end, so that the rank
+  * of A is the index of the first zero on the diagonal of U (with indices starting at 0) if any.
+  *
+  * This decomposition provides the generic approach to solving systems of linear equations, computing
+  * the rank, invertibility, inverse, kernel, and determinant.
+  *
+  * This LU decomposition is very stable and well tested with large matrices. Even exact rank computation
+  * works at sizes larger than 1000x1000. However there are use cases where the SVD decomposition is inherently
+  * more stable when dealing with numerically damaged input. For example, computing the kernel is more stable with
+  * SVD because the SVD can determine which singular values are negligible while LU has to work at the level of matrix
+  * coefficients that are less meaningful in this respect.
+  *
+  * The data of the LU decomposition can be directly accessed through the methods matrixLU(),
+  * permutationP(), permutationQ().
+  *
+  * As an exemple, here is how the original matrix can be retrieved:
+  * \include class_LU.cpp
+  * Output: \verbinclude class_LU.out
+  *
+  * \sa MatrixBase::lu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse()
+  */
+template<typename MatrixType> class LU
+{
+  public:
+
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef Matrix<int, 1, MatrixType::ColsAtCompileTime> IntRowVectorType;
+    typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> IntColVectorType;
+    typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime> RowVectorType;
+    typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVectorType;
+
+    enum { MaxSmallDimAtCompileTime = EIGEN_ENUM_MIN(
+             MatrixType::MaxColsAtCompileTime,
+             MatrixType::MaxRowsAtCompileTime)
+    };
+
+    typedef Matrix<typename MatrixType::Scalar,
+                  MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" is the number of cols of the original matrix
+                                                 // so that the product "matrix * kernel = zero" makes sense
+                  Dynamic,                       // we don't know at compile-time the dimension of the kernel
+                  MatrixType::Options,
+                  MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter
+                  MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, whose dimension is the number
+                                                   // of columns of the original matrix
+    > KernelResultType;
+
+    typedef Matrix<typename MatrixType::Scalar,
+                   MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose dimension is the number
+                                                  // of rows of the original matrix
+                   Dynamic,                       // we don't know at compile time the dimension of the image (the rank)
+                   MatrixType::Options,
+                   MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix,
+                   MatrixType::MaxColsAtCompileTime  // so it has the same number of rows and at most as many columns.
+    > ImageResultType;
+
+    /** Constructor.
+      *
+      * \param matrix the matrix of which to compute the LU decomposition.
+      */
+    LU(const MatrixType& matrix);
+
+    /** \returns the LU decomposition matrix: the upper-triangular part is U, the
+      * unit-lower-triangular part is L (at least for square matrices; in the non-square
+      * case, special care is needed, see the documentation of class LU).
+      *
+      * \sa matrixL(), matrixU()
+      */
+    inline const MatrixType& matrixLU() const
+    {
+      return m_lu;
+    }
+
+    /** \returns a vector of integers, whose size is the number of rows of the matrix being decomposed,
+      * representing the P permutation i.e. the permutation of the rows. For its precise meaning,
+      * see the examples given in the documentation of class LU.
+      *
+      * \sa permutationQ()
+      */
+    inline const IntColVectorType& permutationP() const
+    {
+      return m_p;
+    }
+
+    /** \returns a vector of integers, whose size is the number of columns of the matrix being
+      * decomposed, representing the Q permutation i.e. the permutation of the columns.
+      * For its precise meaning, see the examples given in the documentation of class LU.
+      *
+      * \sa permutationP()
+      */
+    inline const IntRowVectorType& permutationQ() const
+    {
+      return m_q;
+    }
+
+    /** Computes a basis of the kernel of the matrix, also called the null-space of the matrix.
+      *
+      * \note This method is only allowed on non-invertible matrices, as determined by
+      * isInvertible(). Calling it on an invertible matrix will make an assertion fail.
+      *
+      * \param result a pointer to the matrix in which to store the kernel. The columns of this
+      * matrix will be set to form a basis of the kernel (it will be resized
+      * if necessary).
+      *
+      * Example: \include LU_computeKernel.cpp
+      * Output: \verbinclude LU_computeKernel.out
+      *
+      * \sa kernel(), computeImage(), image()
+      */
+    template<typename KernelMatrixType>
+    void computeKernel(KernelMatrixType *result) const;
+
+    /** Computes a basis of the image of the matrix, also called the column-space or range of he matrix.
+      *
+      * \note Calling this method on the zero matrix will make an assertion fail.
+      *
+      * \param result a pointer to the matrix in which to store the image. The columns of this
+      * matrix will be set to form a basis of the image (it will be resized
+      * if necessary).
+      *
+      * Example: \include LU_computeImage.cpp
+      * Output: \verbinclude LU_computeImage.out
+      *
+      * \sa image(), computeKernel(), kernel()
+      */
+    template<typename ImageMatrixType>
+    void computeImage(ImageMatrixType *result) const;
+
+    /** \returns the kernel of the matrix, also called its null-space. The columns of the returned matrix
+      * will form a basis of the kernel.
+      *
+      * \note: this method is only allowed on non-invertible matrices, as determined by
+      * isInvertible(). Calling it on an invertible matrix will make an assertion fail.
+      *
+      * \note: this method returns a matrix by value, which induces some inefficiency.
+      *        If you prefer to avoid this overhead, use computeKernel() instead.
+      *
+      * Example: \include LU_kernel.cpp
+      * Output: \verbinclude LU_kernel.out
+      *
+      * \sa computeKernel(), image()
+      */
+    const KernelResultType kernel() const;
+
+    /** \returns the image of the matrix, also called its column-space. The columns of the returned matrix
+      * will form a basis of the kernel.
+      *
+      * \note: Calling this method on the zero matrix will make an assertion fail.
+      *
+      * \note: this method returns a matrix by value, which induces some inefficiency.
+      *        If you prefer to avoid this overhead, use computeImage() instead.
+      *
+      * Example: \include LU_image.cpp
+      * Output: \verbinclude LU_image.out
+      *
+      * \sa computeImage(), kernel()
+      */
+    const ImageResultType image() const;
+
+    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
+      * *this is the LU decomposition, if any exists.
+      *
+      * \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
+      *          the only requirement in order for the equation to make sense is that
+      *          b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
+      * \param result a pointer to the vector or matrix in which to store the solution, if any exists.
+      *          Resized if necessary, so that result->rows()==A.cols() and result->cols()==b.cols().
+      *          If no solution exists, *result is left with undefined coefficients.
+      *
+      * \returns true if any solution exists, false if no solution exists.
+      *
+      * \note If there exist more than one solution, this method will arbitrarily choose one.
+      *       If you need a complete analysis of the space of solutions, take the one solution obtained
+      *       by this method and add to it elements of the kernel, as determined by kernel().
+      *
+      * Example: \include LU_solve.cpp
+      * Output: \verbinclude LU_solve.out
+      *
+      * \sa MatrixBase::solveTriangular(), kernel(), computeKernel(), inverse(), computeInverse()
+      */
+    template<typename OtherDerived, typename ResultType>
+    bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const;
+
+    /** \returns the determinant of the matrix of which
+      * *this is the LU decomposition. It has only linear complexity
+      * (that is, O(n) where n is the dimension of the square matrix)
+      * as the LU decomposition has already been computed.
+      *
+      * \note This is only for square matrices.
+      *
+      * \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
+      *       optimized paths.
+      *
+      * \warning a determinant can be very big or small, so for matrices
+      * of large enough dimension, there is a risk of overflow/underflow.
+      *
+      * \sa MatrixBase::determinant()
+      */
+    typename ei_traits<MatrixType>::Scalar determinant() const;
+
+    /** \returns the rank of the matrix of which *this is the LU decomposition.
+      *
+      * \note This is computed at the time of the construction of the LU decomposition. This
+      *       method does not perform any further computation.
+      */
+    inline int rank() const
+    {
+      return m_rank;
+    }
+
+    /** \returns the dimension of the kernel of the matrix of which *this is the LU decomposition.
+      *
+      * \note Since the rank is computed at the time of the construction of the LU decomposition, this
+      *       method almost does not perform any further computation.
+      */
+    inline int dimensionOfKernel() const
+    {
+      return m_lu.cols() - m_rank;
+    }
+
+    /** \returns true if the matrix of which *this is the LU decomposition represents an injective
+      *          linear map, i.e. has trivial kernel; false otherwise.
+      *
+      * \note Since the rank is computed at the time of the construction of the LU decomposition, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isInjective() const
+    {
+      return m_rank == m_lu.cols();
+    }
+
+    /** \returns true if the matrix of which *this is the LU decomposition represents a surjective
+      *          linear map; false otherwise.
+      *
+      * \note Since the rank is computed at the time of the construction of the LU decomposition, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isSurjective() const
+    {
+      return m_rank == m_lu.rows();
+    }
+
+    /** \returns true if the matrix of which *this is the LU decomposition is invertible.
+      *
+      * \note Since the rank is computed at the time of the construction of the LU decomposition, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isInvertible() const
+    {
+      return isInjective() && isSurjective();
+    }
+
+    /** Computes the inverse of the matrix of which *this is the LU decomposition.
+      *
+      * \param result a pointer to the matrix into which to store the inverse. Resized if needed.
+      *
+      * \note If this matrix is not invertible, *result is left with undefined coefficients.
+      *       Use isInvertible() to first determine whether this matrix is invertible.
+      *
+      * \sa MatrixBase::computeInverse(), inverse()
+      */
+    inline void computeInverse(MatrixType *result) const
+    {
+      solve(MatrixType::Identity(m_lu.rows(), m_lu.cols()), result);
+    }
+
+    /** \returns the inverse of the matrix of which *this is the LU decomposition.
+      *
+      * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
+      *       Use isInvertible() to first determine whether this matrix is invertible.
+      *
+      * \sa computeInverse(), MatrixBase::inverse()
+      */
+    inline MatrixType inverse() const
+    {
+      MatrixType result;
+      computeInverse(&result);
+      return result;
+    }
+
+  protected:
+    const MatrixType& m_originalMatrix;
+    MatrixType m_lu;
+    IntColVectorType m_p;
+    IntRowVectorType m_q;
+    int m_det_pq;
+    int m_rank;
+    RealScalar m_precision;
+};
+
+template<typename MatrixType>
+LU<MatrixType>::LU(const MatrixType& matrix)
+  : m_originalMatrix(matrix),
+    m_lu(matrix),
+    m_p(matrix.rows()),
+    m_q(matrix.cols())
+{
+  const int size = matrix.diagonal().size();
+  const int rows = matrix.rows();
+  const int cols = matrix.cols();
+  
+  // this formula comes from experimenting (see "LU precision tuning" thread on the list)
+  // and turns out to be identical to Higham's formula used already in LDLt.
+  m_precision = machine_epsilon<Scalar>() * size;
+
+  IntColVectorType rows_transpositions(matrix.rows());
+  IntRowVectorType cols_transpositions(matrix.cols());
+  int number_of_transpositions = 0;
+
+  RealScalar biggest = RealScalar(0);
+  m_rank = size;
+  for(int k = 0; k < size; ++k)
+  {
+    int row_of_biggest_in_corner, col_of_biggest_in_corner;
+    RealScalar biggest_in_corner;
+
+    biggest_in_corner = m_lu.corner(Eigen::BottomRight, rows-k, cols-k)
+                        .cwise().abs()
+                        .maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
+    row_of_biggest_in_corner += k;
+    col_of_biggest_in_corner += k;
+    if(k==0) biggest = biggest_in_corner;
+
+    // if the corner is negligible, then we have less than full rank, and we can finish early
+    if(ei_isMuchSmallerThan(biggest_in_corner, biggest, m_precision))
+    {
+      m_rank = k;
+      for(int i = k; i < size; i++)
+      {
+        rows_transpositions.coeffRef(i) = i;
+        cols_transpositions.coeffRef(i) = i;
+      }
+      break;
+    }
+
+    rows_transpositions.coeffRef(k) = row_of_biggest_in_corner;
+    cols_transpositions.coeffRef(k) = col_of_biggest_in_corner;
+    if(k != row_of_biggest_in_corner) {
+      m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
+      ++number_of_transpositions;
+    }
+    if(k != col_of_biggest_in_corner) {
+      m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner));
+      ++number_of_transpositions;
+    }
+    if(k<rows-1)
+      m_lu.col(k).end(rows-k-1) /= m_lu.coeff(k,k);
+    if(k<size-1)
+      for(int col = k + 1; col < cols; ++col)
+        m_lu.col(col).end(rows-k-1) -= m_lu.col(k).end(rows-k-1) * m_lu.coeff(k,col);
+  }
+
+  for(int k = 0; k < matrix.rows(); ++k) m_p.coeffRef(k) = k;
+  for(int k = size-1; k >= 0; --k)
+    std::swap(m_p.coeffRef(k), m_p.coeffRef(rows_transpositions.coeff(k)));
+
+  for(int k = 0; k < matrix.cols(); ++k) m_q.coeffRef(k) = k;
+  for(int k = 0; k < size; ++k)
+    std::swap(m_q.coeffRef(k), m_q.coeffRef(cols_transpositions.coeff(k)));
+
+  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
+}
+
+template<typename MatrixType>
+typename ei_traits<MatrixType>::Scalar LU<MatrixType>::determinant() const
+{
+  return Scalar(m_det_pq) * m_lu.diagonal().redux(ei_scalar_product_op<Scalar>());
+}
+
+template<typename MatrixType>
+template<typename KernelMatrixType>
+void LU<MatrixType>::computeKernel(KernelMatrixType *result) const
+{
+  ei_assert(!isInvertible());
+  const int dimker = dimensionOfKernel(), cols = m_lu.cols();
+  result->resize(cols, dimker);
+
+  /* Let us use the following lemma:
+    *
+    * Lemma: If the matrix A has the LU decomposition PAQ = LU,
+    * then Ker A = Q(Ker U).
+    *
+    * Proof: trivial: just keep in mind that P, Q, L are invertible.
+    */
+
+  /* Thus, all we need to do is to compute Ker U, and then apply Q.
+    *
+    * U is upper triangular, with eigenvalues sorted so that any zeros appear at the end.
+    * Thus, the diagonal of U ends with exactly
+    * m_dimKer zero's. Let us use that to construct m_dimKer linearly
+    * independent vectors in Ker U.
+    */
+
+  Matrix<Scalar, Dynamic, Dynamic, MatrixType::Options,
+         MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime>
+    y(-m_lu.corner(TopRight, m_rank, dimker));
+
+  m_lu.corner(TopLeft, m_rank, m_rank)
+      .template marked<UpperTriangular>()
+      .solveTriangularInPlace(y);
+
+  for(int i = 0; i < m_rank; ++i) result->row(m_q.coeff(i)) = y.row(i);
+  for(int i = m_rank; i < cols; ++i) result->row(m_q.coeff(i)).setZero();
+  for(int k = 0; k < dimker; ++k) result->coeffRef(m_q.coeff(m_rank+k), k) = Scalar(1);
+}
+
+template<typename MatrixType>
+const typename LU<MatrixType>::KernelResultType
+LU<MatrixType>::kernel() const
+{
+  KernelResultType result(m_lu.cols(), dimensionOfKernel());
+  computeKernel(&result);
+  return result;
+}
+
+template<typename MatrixType>
+template<typename ImageMatrixType>
+void LU<MatrixType>::computeImage(ImageMatrixType *result) const
+{
+  ei_assert(m_rank > 0);
+  result->resize(m_originalMatrix.rows(), m_rank);
+  for(int i = 0; i < m_rank; ++i)
+    result->col(i) = m_originalMatrix.col(m_q.coeff(i));
+}
+
+template<typename MatrixType>
+const typename LU<MatrixType>::ImageResultType
+LU<MatrixType>::image() const
+{
+  ImageResultType result(m_originalMatrix.rows(), m_rank);
+  computeImage(&result);
+  return result;
+}
+
+template<typename MatrixType>
+template<typename OtherDerived, typename ResultType>
+bool LU<MatrixType>::solve(
+  const MatrixBase<OtherDerived>& b,
+  ResultType *result
+) const
+{
+  /* The decomposition PAQ = LU can be rewritten as A = P^{-1} L U Q^{-1}.
+   * So we proceed as follows:
+   * Step 1: compute c = Pb.
+   * Step 2: replace c by the solution x to Lx = c. Exists because L is invertible.
+   * Step 3: replace c by the solution x to Ux = c. Check if a solution really exists.
+   * Step 4: result = Qc;
+   */
+
+  const int rows = m_lu.rows(), cols = m_lu.cols();
+  ei_assert(b.rows() == rows);
+  const int smalldim = std::min(rows, cols);
+
+  typename OtherDerived::PlainMatrixType c(b.rows(), b.cols());
+
+  // Step 1
+  for(int i = 0; i < rows; ++i) c.row(m_p.coeff(i)) = b.row(i);
+
+  // Step 2
+  m_lu.corner(Eigen::TopLeft,smalldim,smalldim).template marked<UnitLowerTriangular>()
+    .solveTriangularInPlace(
+      c.corner(Eigen::TopLeft, smalldim, c.cols()));
+  if(rows>cols)
+  {
+    c.corner(Eigen::BottomLeft, rows-cols, c.cols())
+      -= m_lu.corner(Eigen::BottomLeft, rows-cols, cols) * c.corner(Eigen::TopLeft, cols, c.cols());
+  }
+
+  // Step 3
+  if(!isSurjective())
+  {
+    // is c is in the image of U ?
+    RealScalar biggest_in_c = m_rank>0 ? c.corner(TopLeft, m_rank, c.cols()).cwise().abs().maxCoeff() : 0;
+    for(int col = 0; col < c.cols(); ++col)
+      for(int row = m_rank; row < c.rows(); ++row)
+        if(!ei_isMuchSmallerThan(c.coeff(row,col), biggest_in_c, m_precision))
+          return false;
+  }
+  m_lu.corner(TopLeft, m_rank, m_rank)
+      .template marked<UpperTriangular>()
+      .solveTriangularInPlace(c.corner(TopLeft, m_rank, c.cols()));
+
+  // Step 4
+  result->resize(m_lu.cols(), b.cols());
+  for(int i = 0; i < m_rank; ++i) result->row(m_q.coeff(i)) = c.row(i);
+  for(int i = m_rank; i < m_lu.cols(); ++i) result->row(m_q.coeff(i)).setZero();
+  return true;
+}
+
+/** \lu_module
+  *
+  * \return the LU decomposition of \c *this.
+  *
+  * \sa class LU
+  */
+template<typename Derived>
+inline const LU<typename MatrixBase<Derived>::PlainMatrixType>
+MatrixBase<Derived>::lu() const
+{
+  return LU<PlainMatrixType>(eval());
+}
+
+#endif // EIGEN_LU_H
diff --git a/extern/Eigen2/Eigen/src/LeastSquares/LeastSquares.h b/extern/Eigen2/Eigen/src/LeastSquares/LeastSquares.h
new file mode 100644
index 00000000000..b2595ede1fe
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/LeastSquares/LeastSquares.h
@@ -0,0 +1,182 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_LEASTSQUARES_H
+#define EIGEN_LEASTSQUARES_H
+
+/** \ingroup LeastSquares_Module
+  *
+  * \leastsquares_module
+  *
+  * For a set of points, this function tries to express
+  * one of the coords as a linear (affine) function of the other coords.
+  *
+  * This is best explained by an example. This function works in full
+  * generality, for points in a space of arbitrary dimension, and also over
+  * the complex numbers, but for this example we will work in dimension 3
+  * over the real numbers (doubles).
+  *
+  * So let us work with the following set of 5 points given by their
+  * \f$(x,y,z)\f$ coordinates:
+  * @code
+    Vector3d points[5];
+    points[0] = Vector3d( 3.02, 6.89, -4.32 );
+    points[1] = Vector3d( 2.01, 5.39, -3.79 );
+    points[2] = Vector3d( 2.41, 6.01, -4.01 );
+    points[3] = Vector3d( 2.09, 5.55, -3.86 );
+    points[4] = Vector3d( 2.58, 6.32, -4.10 );
+  * @endcode
+  * Suppose that we want to express the second coordinate (\f$y\f$) as a linear
+  * expression in \f$x\f$ and \f$z\f$, that is,
+  * \f[ y=ax+bz+c \f]
+  * for some constants \f$a,b,c\f$. Thus, we want to find the best possible
+  * constants \f$a,b,c\f$ so that the plane of equation \f$y=ax+bz+c\f$ fits
+  * best the five above points. To do that, call this function as follows:
+  * @code
+    Vector3d coeffs; // will store the coefficients a, b, c
+    linearRegression(
+      5,
+      &points,
+      &coeffs,
+      1 // the coord to express as a function of
+        // the other ones. 0 means x, 1 means y, 2 means z.
+    );
+  * @endcode
+  * Now the vector \a coeffs is approximately
+  * \f$( 0.495 ,  -1.927 ,  -2.906 )\f$.
+  * Thus, we get \f$a=0.495, b = -1.927, c = -2.906\f$. Let us check for
+  * instance how near points[0] is from the plane of equation \f$y=ax+bz+c\f$.
+  * Looking at the coords of points[0], we see that:
+  * \f[ax+bz+c = 0.495 * 3.02 + (-1.927) * (-4.32) + (-2.906) = 6.91.\f]
+  * On the other hand, we have \f$y=6.89\f$. We see that the values
+  * \f$6.91\f$ and \f$6.89\f$
+  * are near, so points[0] is very near the plane of equation \f$y=ax+bz+c\f$.
+  *
+  * Let's now describe precisely the parameters:
+  * @param numPoints the number of points
+  * @param points the array of pointers to the points on which to perform the linear regression
+  * @param result pointer to the vector in which to store the result.
+                  This vector must be of the same type and size as the
+                  data points. The meaning of its coords is as follows.
+                  For brevity, let \f$n=Size\f$,
+                  \f$r_i=result[i]\f$,
+                  and \f$f=funcOfOthers\f$. Denote by
+                  \f$x_0,\ldots,x_{n-1}\f$
+                  the n coordinates in the n-dimensional space.
+                  Then the resulting equation is:
+                  \f[ x_f = r_0 x_0 + \cdots + r_{f-1}x_{f-1}
+                   + r_{f+1}x_{f+1} + \cdots + r_{n-1}x_{n-1} + r_n. \f]
+  * @param funcOfOthers Determines which coord to express as a function of the
+                        others. Coords are numbered starting from 0, so that a
+                        value of 0 means \f$x\f$, 1 means \f$y\f$,
+                        2 means \f$z\f$, ...
+  *
+  * \sa fitHyperplane()
+  */
+template<typename VectorType>
+void linearRegression(int numPoints,
+                      VectorType **points,
+                      VectorType *result,
+                      int funcOfOthers )
+{
+  typedef typename VectorType::Scalar Scalar;
+  typedef Hyperplane<Scalar, VectorType::SizeAtCompileTime> HyperplaneType;
+  const int size = points[0]->size();
+  result->resize(size);
+  HyperplaneType h(size);
+  fitHyperplane(numPoints, points, &h);
+  for(int i = 0; i < funcOfOthers; i++)
+    result->coeffRef(i) = - h.coeffs()[i] / h.coeffs()[funcOfOthers];
+  for(int i = funcOfOthers; i < size; i++)
+    result->coeffRef(i) = - h.coeffs()[i+1] / h.coeffs()[funcOfOthers];
+}
+
+/** \ingroup LeastSquares_Module
+  *
+  * \leastsquares_module
+  *
+  * This function is quite similar to linearRegression(), so we refer to the
+  * documentation of this function and only list here the differences.
+  *
+  * The main difference from linearRegression() is that this function doesn't
+  * take a \a funcOfOthers argument. Instead, it finds a general equation
+  * of the form
+  * \f[ r_0 x_0 + \cdots + r_{n-1}x_{n-1} + r_n = 0, \f]
+  * where \f$n=Size\f$, \f$r_i=retCoefficients[i]\f$, and we denote by
+  * \f$x_0,\ldots,x_{n-1}\f$ the n coordinates in the n-dimensional space.
+  *
+  * Thus, the vector \a retCoefficients has size \f$n+1\f$, which is another
+  * difference from linearRegression().
+  *
+  * In practice, this function performs an hyper-plane fit in a total least square sense
+  * via the following steps:
+  *  1 - center the data to the mean
+  *  2 - compute the covariance matrix
+  *  3 - pick the eigenvector corresponding to the smallest eigenvalue of the covariance matrix
+  * The ratio of the smallest eigenvalue and the second one gives us a hint about the relevance
+  * of the solution. This value is optionally returned in \a soundness.
+  *
+  * \sa linearRegression()
+  */
+template<typename VectorType, typename HyperplaneType>
+void fitHyperplane(int numPoints,
+                   VectorType **points,
+                   HyperplaneType *result,
+                   typename NumTraits<typename VectorType::Scalar>::Real* soundness = 0)
+{
+  typedef typename VectorType::Scalar Scalar;
+  typedef Matrix<Scalar,VectorType::SizeAtCompileTime,VectorType::SizeAtCompileTime> CovMatrixType;
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType)
+  ei_assert(numPoints >= 1);
+  int size = points[0]->size();
+  ei_assert(size+1 == result->coeffs().size());
+
+  // compute the mean of the data
+  VectorType mean = VectorType::Zero(size);
+  for(int i = 0; i < numPoints; ++i)
+    mean += *(points[i]);
+  mean /= numPoints;
+
+  // compute the covariance matrix
+  CovMatrixType covMat = CovMatrixType::Zero(size, size);
+  VectorType remean = VectorType::Zero(size);
+  for(int i = 0; i < numPoints; ++i)
+  {
+    VectorType diff = (*(points[i]) - mean).conjugate();
+    covMat += diff * diff.adjoint();
+  }
+
+  // now we just have to pick the eigen vector with smallest eigen value
+  SelfAdjointEigenSolver<CovMatrixType> eig(covMat);
+  result->normal() = eig.eigenvectors().col(0);
+  if (soundness)
+    *soundness = eig.eigenvalues().coeff(0)/eig.eigenvalues().coeff(1);
+
+  // let's compute the constant coefficient such that the
+  // plane pass trough the mean point:
+  result->offset() = - (result->normal().cwise()* mean).sum();
+}
+
+
+#endif // EIGEN_LEASTSQUARES_H
diff --git a/extern/Eigen2/Eigen/src/QR/EigenSolver.h b/extern/Eigen2/Eigen/src/QR/EigenSolver.h
new file mode 100644
index 00000000000..70f21cebcdb
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/EigenSolver.h
@@ -0,0 +1,722 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_EIGENSOLVER_H
+#define EIGEN_EIGENSOLVER_H
+
+/** \ingroup QR_Module
+  * \nonstableyet
+  *
+  * \class EigenSolver
+  *
+  * \brief Eigen values/vectors solver for non selfadjoint matrices
+  *
+  * \param MatrixType the type of the matrix of which we are computing the eigen decomposition
+  *
+  * Currently it only support real matrices.
+  *
+  * \note this code was adapted from JAMA (public domain)
+  *
+  * \sa MatrixBase::eigenvalues(), SelfAdjointEigenSolver
+  */
+template<typename _MatrixType> class EigenSolver
+{
+  public:
+
+    typedef _MatrixType MatrixType;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef std::complex<RealScalar> Complex;
+    typedef Matrix<Complex, MatrixType::ColsAtCompileTime, 1> EigenvalueType;
+    typedef Matrix<Complex, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> EigenvectorType;
+    typedef Matrix<RealScalar, MatrixType::ColsAtCompileTime, 1> RealVectorType;
+    typedef Matrix<RealScalar, Dynamic, 1> RealVectorTypeX;
+
+    /** 
+    * \brief Default Constructor.
+    *
+    * The default constructor is useful in cases in which the user intends to
+    * perform decompositions via EigenSolver::compute(const MatrixType&).
+    */
+    EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false) {}
+
+    EigenSolver(const MatrixType& matrix)
+      : m_eivec(matrix.rows(), matrix.cols()),
+        m_eivalues(matrix.cols()),
+        m_isInitialized(false)
+    {
+      compute(matrix);
+    }
+
+
+    EigenvectorType eigenvectors(void) const;
+
+    /** \returns a real matrix V of pseudo eigenvectors.
+      *
+      * Let D be the block diagonal matrix with the real eigenvalues in 1x1 blocks,
+      * and any complex values u+iv in 2x2 blocks [u v ; -v u]. Then, the matrices D
+      * and V satisfy A*V = V*D.
+      *
+      * More precisely, if the diagonal matrix of the eigen values is:\n
+      * \f$
+      * \left[ \begin{array}{cccccc}
+      * u+iv &      &      &      &   &   \\
+      *      & u-iv &      &      &   &   \\
+      *      &      & a+ib &      &   &   \\
+      *      &      &      & a-ib &   &   \\
+      *      &      &      &      & x &   \\
+      *      &      &      &      &   & y \\
+      * \end{array} \right]
+      * \f$ \n
+      * then, we have:\n
+      * \f$
+      * D =\left[ \begin{array}{cccccc}
+      *  u & v &    &   &   &   \\
+      * -v & u &    &   &   &   \\
+      *    &   &  a & b &   &   \\
+      *    &   & -b & a &   &   \\
+      *    &   &    &   & x &   \\
+      *    &   &    &   &   & y \\
+      * \end{array} \right]
+      * \f$
+      *
+      * \sa pseudoEigenvalueMatrix()
+      */
+    const MatrixType& pseudoEigenvectors() const 
+    { 
+      ei_assert(m_isInitialized && "EigenSolver is not initialized.");
+      return m_eivec; 
+    }
+
+    MatrixType pseudoEigenvalueMatrix() const;
+
+    /** \returns the eigenvalues as a column vector */
+    EigenvalueType eigenvalues() const 
+    { 
+      ei_assert(m_isInitialized && "EigenSolver is not initialized.");
+      return m_eivalues; 
+    }
+
+    void compute(const MatrixType& matrix);
+
+  private:
+
+    void orthes(MatrixType& matH, RealVectorType& ort);
+    void hqr2(MatrixType& matH);
+
+  protected:
+    MatrixType m_eivec;
+    EigenvalueType m_eivalues;
+    bool m_isInitialized;
+};
+
+/** \returns the real block diagonal matrix D of the eigenvalues.
+  *
+  * See pseudoEigenvectors() for the details.
+  */
+template<typename MatrixType>
+MatrixType EigenSolver<MatrixType>::pseudoEigenvalueMatrix() const
+{
+  ei_assert(m_isInitialized && "EigenSolver is not initialized.");
+  int n = m_eivec.cols();
+  MatrixType matD = MatrixType::Zero(n,n);
+  for (int i=0; i<n; ++i)
+  {
+    if (ei_isMuchSmallerThan(ei_imag(m_eivalues.coeff(i)), ei_real(m_eivalues.coeff(i))))
+      matD.coeffRef(i,i) = ei_real(m_eivalues.coeff(i));
+    else
+    {
+      matD.template block<2,2>(i,i) <<  ei_real(m_eivalues.coeff(i)), ei_imag(m_eivalues.coeff(i)),
+                                       -ei_imag(m_eivalues.coeff(i)), ei_real(m_eivalues.coeff(i));
+      ++i;
+    }
+  }
+  return matD;
+}
+
+/** \returns the normalized complex eigenvectors as a matrix of column vectors.
+  *
+  * \sa eigenvalues(), pseudoEigenvectors()
+  */
+template<typename MatrixType>
+typename EigenSolver<MatrixType>::EigenvectorType EigenSolver<MatrixType>::eigenvectors(void) const
+{
+  ei_assert(m_isInitialized && "EigenSolver is not initialized.");
+  int n = m_eivec.cols();
+  EigenvectorType matV(n,n);
+  for (int j=0; j<n; ++j)
+  {
+    if (ei_isMuchSmallerThan(ei_abs(ei_imag(m_eivalues.coeff(j))), ei_abs(ei_real(m_eivalues.coeff(j)))))
+    {
+      // we have a real eigen value
+      matV.col(j) = m_eivec.col(j).template cast<Complex>();
+    }
+    else
+    {
+      // we have a pair of complex eigen values
+      for (int i=0; i<n; ++i)
+      {
+        matV.coeffRef(i,j)   = Complex(m_eivec.coeff(i,j),  m_eivec.coeff(i,j+1));
+        matV.coeffRef(i,j+1) = Complex(m_eivec.coeff(i,j), -m_eivec.coeff(i,j+1));
+      }
+      matV.col(j).normalize();
+      matV.col(j+1).normalize();
+      ++j;
+    }
+  }
+  return matV;
+}
+
+template<typename MatrixType>
+void EigenSolver<MatrixType>::compute(const MatrixType& matrix)
+{
+  assert(matrix.cols() == matrix.rows());
+  int n = matrix.cols();
+  m_eivalues.resize(n,1);
+
+  MatrixType matH = matrix;
+  RealVectorType ort(n);
+
+  // Reduce to Hessenberg form.
+  orthes(matH, ort);
+
+  // Reduce Hessenberg to real Schur form.
+  hqr2(matH);
+
+  m_isInitialized = true;
+}
+
+// Nonsymmetric reduction to Hessenberg form.
+template<typename MatrixType>
+void EigenSolver<MatrixType>::orthes(MatrixType& matH, RealVectorType& ort)
+{
+  //  This is derived from the Algol procedures orthes and ortran,
+  //  by Martin and Wilkinson, Handbook for Auto. Comp.,
+  //  Vol.ii-Linear Algebra, and the corresponding
+  //  Fortran subroutines in EISPACK.
+
+  int n = m_eivec.cols();
+  int low = 0;
+  int high = n-1;
+
+  for (int m = low+1; m <= high-1; ++m)
+  {
+    // Scale column.
+    RealScalar scale = matH.block(m, m-1, high-m+1, 1).cwise().abs().sum();
+    if (scale != 0.0)
+    {
+      // Compute Householder transformation.
+      RealScalar h = 0.0;
+      // FIXME could be rewritten, but this one looks better wrt cache
+      for (int i = high; i >= m; i--)
+      {
+        ort.coeffRef(i) = matH.coeff(i,m-1)/scale;
+        h += ort.coeff(i) * ort.coeff(i);
+      }
+      RealScalar g = ei_sqrt(h);
+      if (ort.coeff(m) > 0)
+        g = -g;
+      h = h - ort.coeff(m) * g;
+      ort.coeffRef(m) = ort.coeff(m) - g;
+
+      // Apply Householder similarity transformation
+      // H = (I-u*u'/h)*H*(I-u*u')/h)
+      int bSize = high-m+1;
+      matH.block(m, m, bSize, n-m) -= ((ort.segment(m, bSize)/h)
+        * (ort.segment(m, bSize).transpose() *  matH.block(m, m, bSize, n-m)).lazy()).lazy();
+
+      matH.block(0, m, high+1, bSize) -= ((matH.block(0, m, high+1, bSize) * ort.segment(m, bSize)).lazy()
+        * (ort.segment(m, bSize)/h).transpose()).lazy();
+
+      ort.coeffRef(m) = scale*ort.coeff(m);
+      matH.coeffRef(m,m-1) = scale*g;
+    }
+  }
+
+  // Accumulate transformations (Algol's ortran).
+  m_eivec.setIdentity();
+
+  for (int m = high-1; m >= low+1; m--)
+  {
+    if (matH.coeff(m,m-1) != 0.0)
+    {
+      ort.segment(m+1, high-m) = matH.col(m-1).segment(m+1, high-m);
+
+      int bSize = high-m+1;
+      m_eivec.block(m, m, bSize, bSize) += ( (ort.segment(m, bSize) /  (matH.coeff(m,m-1) * ort.coeff(m) ) )
+        * (ort.segment(m, bSize).transpose() * m_eivec.block(m, m, bSize, bSize)).lazy());
+    }
+  }
+}
+
+// Complex scalar division.
+template<typename Scalar>
+std::complex<Scalar> cdiv(Scalar xr, Scalar xi, Scalar yr, Scalar yi)
+{
+  Scalar r,d;
+  if (ei_abs(yr) > ei_abs(yi))
+  {
+      r = yi/yr;
+      d = yr + r*yi;
+      return std::complex<Scalar>((xr + r*xi)/d, (xi - r*xr)/d);
+  }
+  else
+  {
+      r = yr/yi;
+      d = yi + r*yr;
+      return std::complex<Scalar>((r*xr + xi)/d, (r*xi - xr)/d);
+  }
+}
+
+
+// Nonsymmetric reduction from Hessenberg to real Schur form.
+template<typename MatrixType>
+void EigenSolver<MatrixType>::hqr2(MatrixType& matH)
+{
+  //  This is derived from the Algol procedure hqr2,
+  //  by Martin and Wilkinson, Handbook for Auto. Comp.,
+  //  Vol.ii-Linear Algebra, and the corresponding
+  //  Fortran subroutine in EISPACK.
+
+  // Initialize
+  int nn = m_eivec.cols();
+  int n = nn-1;
+  int low = 0;
+  int high = nn-1;
+  Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
+  Scalar exshift = 0.0;
+  Scalar p=0,q=0,r=0,s=0,z=0,t,w,x,y;
+
+  // Store roots isolated by balanc and compute matrix norm
+  // FIXME to be efficient the following would requires a triangular reduxion code
+  // Scalar norm = matH.upper().cwise().abs().sum() + matH.corner(BottomLeft,n,n).diagonal().cwise().abs().sum();
+  Scalar norm = 0.0;
+  for (int j = 0; j < nn; ++j)
+  {
+    // FIXME what's the purpose of the following since the condition is always false
+    if ((j < low) || (j > high))
+    {
+      m_eivalues.coeffRef(j) = Complex(matH.coeff(j,j), 0.0);
+    }
+    norm += matH.row(j).segment(std::max(j-1,0), nn-std::max(j-1,0)).cwise().abs().sum();
+  }
+
+  // Outer loop over eigenvalue index
+  int iter = 0;
+  while (n >= low)
+  {
+    // Look for single small sub-diagonal element
+    int l = n;
+    while (l > low)
+    {
+      s = ei_abs(matH.coeff(l-1,l-1)) + ei_abs(matH.coeff(l,l));
+      if (s == 0.0)
+          s = norm;
+      if (ei_abs(matH.coeff(l,l-1)) < eps * s)
+        break;
+      l--;
+    }
+
+    // Check for convergence
+    // One root found
+    if (l == n)
+    {
+      matH.coeffRef(n,n) = matH.coeff(n,n) + exshift;
+      m_eivalues.coeffRef(n) = Complex(matH.coeff(n,n), 0.0);
+      n--;
+      iter = 0;
+    }
+    else if (l == n-1) // Two roots found
+    {
+      w = matH.coeff(n,n-1) * matH.coeff(n-1,n);
+      p = (matH.coeff(n-1,n-1) - matH.coeff(n,n)) * Scalar(0.5);
+      q = p * p + w;
+      z = ei_sqrt(ei_abs(q));
+      matH.coeffRef(n,n) = matH.coeff(n,n) + exshift;
+      matH.coeffRef(n-1,n-1) = matH.coeff(n-1,n-1) + exshift;
+      x = matH.coeff(n,n);
+
+      // Scalar pair
+      if (q >= 0)
+      {
+        if (p >= 0)
+          z = p + z;
+        else
+          z = p - z;
+
+        m_eivalues.coeffRef(n-1) = Complex(x + z, 0.0);
+        m_eivalues.coeffRef(n) = Complex(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0);
+
+        x = matH.coeff(n,n-1);
+        s = ei_abs(x) + ei_abs(z);
+        p = x / s;
+        q = z / s;
+        r = ei_sqrt(p * p+q * q);
+        p = p / r;
+        q = q / r;
+
+        // Row modification
+        for (int j = n-1; j < nn; ++j)
+        {
+          z = matH.coeff(n-1,j);
+          matH.coeffRef(n-1,j) = q * z + p * matH.coeff(n,j);
+          matH.coeffRef(n,j) = q * matH.coeff(n,j) - p * z;
+        }
+
+        // Column modification
+        for (int i = 0; i <= n; ++i)
+        {
+          z = matH.coeff(i,n-1);
+          matH.coeffRef(i,n-1) = q * z + p * matH.coeff(i,n);
+          matH.coeffRef(i,n) = q * matH.coeff(i,n) - p * z;
+        }
+
+        // Accumulate transformations
+        for (int i = low; i <= high; ++i)
+        {
+          z = m_eivec.coeff(i,n-1);
+          m_eivec.coeffRef(i,n-1) = q * z + p * m_eivec.coeff(i,n);
+          m_eivec.coeffRef(i,n) = q * m_eivec.coeff(i,n) - p * z;
+        }
+      }
+      else // Complex pair
+      {
+        m_eivalues.coeffRef(n-1) = Complex(x + p, z);
+        m_eivalues.coeffRef(n)   = Complex(x + p, -z);
+      }
+      n = n - 2;
+      iter = 0;
+    }
+    else // No convergence yet
+    {
+      // Form shift
+      x = matH.coeff(n,n);
+      y = 0.0;
+      w = 0.0;
+      if (l < n)
+      {
+          y = matH.coeff(n-1,n-1);
+          w = matH.coeff(n,n-1) * matH.coeff(n-1,n);
+      }
+
+      // Wilkinson's original ad hoc shift
+      if (iter == 10)
+      {
+        exshift += x;
+        for (int i = low; i <= n; ++i)
+          matH.coeffRef(i,i) -= x;
+        s = ei_abs(matH.coeff(n,n-1)) + ei_abs(matH.coeff(n-1,n-2));
+        x = y = Scalar(0.75) * s;
+        w = Scalar(-0.4375) * s * s;
+      }
+
+      // MATLAB's new ad hoc shift
+      if (iter == 30)
+      {
+        s = Scalar((y - x) / 2.0);
+        s = s * s + w;
+        if (s > 0)
+        {
+          s = ei_sqrt(s);
+          if (y < x)
+            s = -s;
+          s = Scalar(x - w / ((y - x) / 2.0 + s));
+          for (int i = low; i <= n; ++i)
+            matH.coeffRef(i,i) -= s;
+          exshift += s;
+          x = y = w = Scalar(0.964);
+        }
+      }
+
+      iter = iter + 1;   // (Could check iteration count here.)
+
+      // Look for two consecutive small sub-diagonal elements
+      int m = n-2;
+      while (m >= l)
+      {
+        z = matH.coeff(m,m);
+        r = x - z;
+        s = y - z;
+        p = (r * s - w) / matH.coeff(m+1,m) + matH.coeff(m,m+1);
+        q = matH.coeff(m+1,m+1) - z - r - s;
+        r = matH.coeff(m+2,m+1);
+        s = ei_abs(p) + ei_abs(q) + ei_abs(r);
+        p = p / s;
+        q = q / s;
+        r = r / s;
+        if (m == l) {
+          break;
+        }
+        if (ei_abs(matH.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) <
+          eps * (ei_abs(p) * (ei_abs(matH.coeff(m-1,m-1)) + ei_abs(z) +
+          ei_abs(matH.coeff(m+1,m+1)))))
+        {
+          break;
+        }
+        m--;
+      }
+
+      for (int i = m+2; i <= n; ++i)
+      {
+        matH.coeffRef(i,i-2) = 0.0;
+        if (i > m+2)
+          matH.coeffRef(i,i-3) = 0.0;
+      }
+
+      // Double QR step involving rows l:n and columns m:n
+      for (int k = m; k <= n-1; ++k)
+      {
+        int notlast = (k != n-1);
+        if (k != m) {
+          p = matH.coeff(k,k-1);
+          q = matH.coeff(k+1,k-1);
+          r = notlast ? matH.coeff(k+2,k-1) : Scalar(0);
+          x = ei_abs(p) + ei_abs(q) + ei_abs(r);
+          if (x != 0.0)
+          {
+            p = p / x;
+            q = q / x;
+            r = r / x;
+          }
+        }
+
+        if (x == 0.0)
+          break;
+
+        s = ei_sqrt(p * p + q * q + r * r);
+
+        if (p < 0)
+          s = -s;
+
+        if (s != 0)
+        {
+          if (k != m)
+            matH.coeffRef(k,k-1) = -s * x;
+          else if (l != m)
+            matH.coeffRef(k,k-1) = -matH.coeff(k,k-1);
+
+          p = p + s;
+          x = p / s;
+          y = q / s;
+          z = r / s;
+          q = q / p;
+          r = r / p;
+
+          // Row modification
+          for (int j = k; j < nn; ++j)
+          {
+            p = matH.coeff(k,j) + q * matH.coeff(k+1,j);
+            if (notlast)
+            {
+              p = p + r * matH.coeff(k+2,j);
+              matH.coeffRef(k+2,j) = matH.coeff(k+2,j) - p * z;
+            }
+            matH.coeffRef(k,j) = matH.coeff(k,j) - p * x;
+            matH.coeffRef(k+1,j) = matH.coeff(k+1,j) - p * y;
+          }
+
+          // Column modification
+          for (int i = 0; i <= std::min(n,k+3); ++i)
+          {
+            p = x * matH.coeff(i,k) + y * matH.coeff(i,k+1);
+            if (notlast)
+            {
+              p = p + z * matH.coeff(i,k+2);
+              matH.coeffRef(i,k+2) = matH.coeff(i,k+2) - p * r;
+            }
+            matH.coeffRef(i,k) = matH.coeff(i,k) - p;
+            matH.coeffRef(i,k+1) = matH.coeff(i,k+1) - p * q;
+          }
+
+          // Accumulate transformations
+          for (int i = low; i <= high; ++i)
+          {
+            p = x * m_eivec.coeff(i,k) + y * m_eivec.coeff(i,k+1);
+            if (notlast)
+            {
+              p = p + z * m_eivec.coeff(i,k+2);
+              m_eivec.coeffRef(i,k+2) = m_eivec.coeff(i,k+2) - p * r;
+            }
+            m_eivec.coeffRef(i,k) = m_eivec.coeff(i,k) - p;
+            m_eivec.coeffRef(i,k+1) = m_eivec.coeff(i,k+1) - p * q;
+          }
+        }  // (s != 0)
+      }  // k loop
+    }  // check convergence
+  }  // while (n >= low)
+
+  // Backsubstitute to find vectors of upper triangular form
+  if (norm == 0.0)
+  {
+      return;
+  }
+
+  for (n = nn-1; n >= 0; n--)
+  {
+    p = m_eivalues.coeff(n).real();
+    q = m_eivalues.coeff(n).imag();
+
+    // Scalar vector
+    if (q == 0)
+    {
+      int l = n;
+      matH.coeffRef(n,n) = 1.0;
+      for (int i = n-1; i >= 0; i--)
+      {
+        w = matH.coeff(i,i) - p;
+        r = (matH.row(i).segment(l,n-l+1) * matH.col(n).segment(l, n-l+1))(0,0);
+
+        if (m_eivalues.coeff(i).imag() < 0.0)
+        {
+          z = w;
+          s = r;
+        }
+        else
+        {
+          l = i;
+          if (m_eivalues.coeff(i).imag() == 0.0)
+          {
+            if (w != 0.0)
+              matH.coeffRef(i,n) = -r / w;
+            else
+              matH.coeffRef(i,n) = -r / (eps * norm);
+          }
+          else // Solve real equations
+          {
+            x = matH.coeff(i,i+1);
+            y = matH.coeff(i+1,i);
+            q = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
+            t = (x * s - z * r) / q;
+            matH.coeffRef(i,n) = t;
+            if (ei_abs(x) > ei_abs(z))
+              matH.coeffRef(i+1,n) = (-r - w * t) / x;
+            else
+              matH.coeffRef(i+1,n) = (-s - y * t) / z;
+          }
+
+          // Overflow control
+          t = ei_abs(matH.coeff(i,n));
+          if ((eps * t) * t > 1)
+            matH.col(n).end(nn-i) /= t;
+        }
+      }
+    }
+    else if (q < 0) // Complex vector
+    {
+      std::complex<Scalar> cc;
+      int l = n-1;
+
+      // Last vector component imaginary so matrix is triangular
+      if (ei_abs(matH.coeff(n,n-1)) > ei_abs(matH.coeff(n-1,n)))
+      {
+        matH.coeffRef(n-1,n-1) = q / matH.coeff(n,n-1);
+        matH.coeffRef(n-1,n) = -(matH.coeff(n,n) - p) / matH.coeff(n,n-1);
+      }
+      else
+      {
+        cc = cdiv<Scalar>(0.0,-matH.coeff(n-1,n),matH.coeff(n-1,n-1)-p,q);
+        matH.coeffRef(n-1,n-1) = ei_real(cc);
+        matH.coeffRef(n-1,n) = ei_imag(cc);
+      }
+      matH.coeffRef(n,n-1) = 0.0;
+      matH.coeffRef(n,n) = 1.0;
+      for (int i = n-2; i >= 0; i--)
+      {
+        Scalar ra,sa,vr,vi;
+        ra = (matH.block(i,l, 1, n-l+1) * matH.block(l,n-1, n-l+1, 1)).lazy()(0,0);
+        sa = (matH.block(i,l, 1, n-l+1) * matH.block(l,n, n-l+1, 1)).lazy()(0,0);
+        w = matH.coeff(i,i) - p;
+
+        if (m_eivalues.coeff(i).imag() < 0.0)
+        {
+          z = w;
+          r = ra;
+          s = sa;
+        }
+        else
+        {
+          l = i;
+          if (m_eivalues.coeff(i).imag() == 0)
+          {
+            cc = cdiv(-ra,-sa,w,q);
+            matH.coeffRef(i,n-1) = ei_real(cc);
+            matH.coeffRef(i,n) = ei_imag(cc);
+          }
+          else
+          {
+            // Solve complex equations
+            x = matH.coeff(i,i+1);
+            y = matH.coeff(i+1,i);
+            vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
+            vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q;
+            if ((vr == 0.0) && (vi == 0.0))
+              vr = eps * norm * (ei_abs(w) + ei_abs(q) + ei_abs(x) + ei_abs(y) + ei_abs(z));
+
+            cc= cdiv(x*r-z*ra+q*sa,x*s-z*sa-q*ra,vr,vi);
+            matH.coeffRef(i,n-1) = ei_real(cc);
+            matH.coeffRef(i,n) = ei_imag(cc);
+            if (ei_abs(x) > (ei_abs(z) + ei_abs(q)))
+            {
+              matH.coeffRef(i+1,n-1) = (-ra - w * matH.coeff(i,n-1) + q * matH.coeff(i,n)) / x;
+              matH.coeffRef(i+1,n) = (-sa - w * matH.coeff(i,n) - q * matH.coeff(i,n-1)) / x;
+            }
+            else
+            {
+              cc = cdiv(-r-y*matH.coeff(i,n-1),-s-y*matH.coeff(i,n),z,q);
+              matH.coeffRef(i+1,n-1) = ei_real(cc);
+              matH.coeffRef(i+1,n) = ei_imag(cc);
+            }
+          }
+
+          // Overflow control
+          t = std::max(ei_abs(matH.coeff(i,n-1)),ei_abs(matH.coeff(i,n)));
+          if ((eps * t) * t > 1)
+            matH.block(i, n-1, nn-i, 2) /= t;
+
+        }
+      }
+    }
+  }
+
+  // Vectors of isolated roots
+  for (int i = 0; i < nn; ++i)
+  {
+    // FIXME again what's the purpose of this test ?
+    // in this algo low==0 and high==nn-1 !!
+    if (i < low || i > high)
+    {
+      m_eivec.row(i).end(nn-i) = matH.row(i).end(nn-i);
+    }
+  }
+
+  // Back transformation to get eigenvectors of original matrix
+  int bRows = high-low+1;
+  for (int j = nn-1; j >= low; j--)
+  {
+    int bSize = std::min(j,high)-low+1;
+    m_eivec.col(j).segment(low, bRows) = (m_eivec.block(low, low, bRows, bSize) * matH.col(j).segment(low, bSize));
+  }
+}
+
+#endif // EIGEN_EIGENSOLVER_H
diff --git a/extern/Eigen2/Eigen/src/QR/HessenbergDecomposition.h b/extern/Eigen2/Eigen/src/QR/HessenbergDecomposition.h
new file mode 100644
index 00000000000..6d0ff794ec2
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/HessenbergDecomposition.h
@@ -0,0 +1,250 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_HESSENBERGDECOMPOSITION_H
+#define EIGEN_HESSENBERGDECOMPOSITION_H
+
+/** \ingroup QR_Module
+  * \nonstableyet
+  *
+  * \class HessenbergDecomposition
+  *
+  * \brief Reduces a squared matrix to an Hessemberg form
+  *
+  * \param MatrixType the type of the matrix of which we are computing the Hessenberg decomposition
+  *
+  * This class performs an Hessenberg decomposition of a matrix \f$ A \f$ such that:
+  * \f$ A = Q H Q^* \f$ where \f$ Q \f$ is unitary and \f$ H \f$ a Hessenberg matrix.
+  *
+  * \sa class Tridiagonalization, class Qr
+  */
+template<typename _MatrixType> class HessenbergDecomposition
+{
+  public:
+
+    typedef _MatrixType MatrixType;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    enum {
+      Size = MatrixType::RowsAtCompileTime,
+      SizeMinusOne = MatrixType::RowsAtCompileTime==Dynamic
+                   ? Dynamic
+                   : MatrixType::RowsAtCompileTime-1
+    };
+
+    typedef Matrix<Scalar, SizeMinusOne, 1> CoeffVectorType;
+    typedef Matrix<RealScalar, Size, 1> DiagonalType;
+    typedef Matrix<RealScalar, SizeMinusOne, 1> SubDiagonalType;
+
+    typedef typename NestByValue<DiagonalCoeffs<MatrixType> >::RealReturnType DiagonalReturnType;
+
+    typedef typename NestByValue<DiagonalCoeffs<
+        NestByValue<Block<MatrixType,SizeMinusOne,SizeMinusOne> > > >::RealReturnType SubDiagonalReturnType;
+
+    /** This constructor initializes a HessenbergDecomposition object for
+      * further use with HessenbergDecomposition::compute()
+      */
+    HessenbergDecomposition(int size = Size==Dynamic ? 2 : Size)
+      : m_matrix(size,size), m_hCoeffs(size-1)
+    {}
+
+    HessenbergDecomposition(const MatrixType& matrix)
+      : m_matrix(matrix),
+        m_hCoeffs(matrix.cols()-1)
+    {
+      _compute(m_matrix, m_hCoeffs);
+    }
+
+    /** Computes or re-compute the Hessenberg decomposition for the matrix \a matrix.
+      *
+      * This method allows to re-use the allocated data.
+      */
+    void compute(const MatrixType& matrix)
+    {
+      m_matrix = matrix;
+      m_hCoeffs.resize(matrix.rows()-1,1);
+      _compute(m_matrix, m_hCoeffs);
+    }
+
+    /** \returns the householder coefficients allowing to
+      * reconstruct the matrix Q from the packed data.
+      *
+      * \sa packedMatrix()
+      */
+    CoeffVectorType householderCoefficients(void) const { return m_hCoeffs; }
+
+    /** \returns the internal result of the decomposition.
+      *
+      * The returned matrix contains the following information:
+      *  - the upper part and lower sub-diagonal represent the Hessenberg matrix H
+      *  - the rest of the lower part contains the Householder vectors that, combined with
+      *    Householder coefficients returned by householderCoefficients(),
+      *    allows to reconstruct the matrix Q as follow:
+      *       Q = H_{N-1} ... H_1 H_0
+      *    where the matrices H are the Householder transformation:
+      *       H_i = (I - h_i * v_i * v_i')
+      *    where h_i == householderCoefficients()[i] and v_i is a Householder vector:
+      *       v_i = [ 0, ..., 0, 1, M(i+2,i), ..., M(N-1,i) ]
+      *
+      * See LAPACK for further details on this packed storage.
+      */
+    const MatrixType& packedMatrix(void) const { return m_matrix; }
+
+    MatrixType matrixQ(void) const;
+    MatrixType matrixH(void) const;
+
+  private:
+
+    static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs);
+
+  protected:
+    MatrixType m_matrix;
+    CoeffVectorType m_hCoeffs;
+};
+
+#ifndef EIGEN_HIDE_HEAVY_CODE
+
+/** \internal
+  * Performs a tridiagonal decomposition of \a matA in place.
+  *
+  * \param matA the input selfadjoint matrix
+  * \param hCoeffs returned Householder coefficients
+  *
+  * The result is written in the lower triangular part of \a matA.
+  *
+  * Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
+  *
+  * \sa packedMatrix()
+  */
+template<typename MatrixType>
+void HessenbergDecomposition<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs)
+{
+  assert(matA.rows()==matA.cols());
+  int n = matA.rows();
+  for (int i = 0; i<n-2; ++i)
+  {
+    // let's consider the vector v = i-th column starting at position i+1
+
+    // start of the householder transformation
+    // squared norm of the vector v skipping the first element
+    RealScalar v1norm2 = matA.col(i).end(n-(i+2)).squaredNorm();
+
+    if (ei_isMuchSmallerThan(v1norm2,static_cast<Scalar>(1)))
+    {
+      hCoeffs.coeffRef(i) = 0.;
+    }
+    else
+    {
+      Scalar v0 = matA.col(i).coeff(i+1);
+      RealScalar beta = ei_sqrt(ei_abs2(v0)+v1norm2);
+      if (ei_real(v0)>=0.)
+        beta = -beta;
+      matA.col(i).end(n-(i+2)) *= (Scalar(1)/(v0-beta));
+      matA.col(i).coeffRef(i+1) = beta;
+      Scalar h = (beta - v0) / beta;
+      // end of the householder transformation
+
+      // Apply similarity transformation to remaining columns,
+      // i.e., A = H' A H where H = I - h v v' and v = matA.col(i).end(n-i-1)
+      matA.col(i).coeffRef(i+1) = 1;
+
+      // first let's do A = H A
+      matA.corner(BottomRight,n-i-1,n-i-1) -= ((ei_conj(h) * matA.col(i).end(n-i-1)) *
+        (matA.col(i).end(n-i-1).adjoint() * matA.corner(BottomRight,n-i-1,n-i-1))).lazy();
+
+      // now let's do A = A H
+      matA.corner(BottomRight,n,n-i-1) -= ((matA.corner(BottomRight,n,n-i-1) * matA.col(i).end(n-i-1))
+                                        * (h * matA.col(i).end(n-i-1).adjoint())).lazy();
+
+      matA.col(i).coeffRef(i+1) = beta;
+      hCoeffs.coeffRef(i) = h;
+    }
+  }
+  if (NumTraits<Scalar>::IsComplex)
+  {
+    // Householder transformation on the remaining single scalar
+    int i = n-2;
+    Scalar v0 = matA.coeff(i+1,i);
+
+    RealScalar beta = ei_sqrt(ei_abs2(v0));
+    if (ei_real(v0)>=0.)
+      beta = -beta;
+    Scalar h = (beta - v0) / beta;
+    hCoeffs.coeffRef(i) = h;
+
+    // A = H* A
+    matA.corner(BottomRight,n-i-1,n-i) -= ei_conj(h) * matA.corner(BottomRight,n-i-1,n-i);
+
+    // A = A H
+    matA.col(n-1) -= h * matA.col(n-1);
+  }
+  else
+  {
+    hCoeffs.coeffRef(n-2) = 0;
+  }
+}
+
+/** reconstructs and returns the matrix Q */
+template<typename MatrixType>
+typename HessenbergDecomposition<MatrixType>::MatrixType
+HessenbergDecomposition<MatrixType>::matrixQ(void) const
+{
+  int n = m_matrix.rows();
+  MatrixType matQ = MatrixType::Identity(n,n);
+  for (int i = n-2; i>=0; i--)
+  {
+    Scalar tmp = m_matrix.coeff(i+1,i);
+    m_matrix.const_cast_derived().coeffRef(i+1,i) = 1;
+
+    matQ.corner(BottomRight,n-i-1,n-i-1) -=
+      ((m_hCoeffs.coeff(i) * m_matrix.col(i).end(n-i-1)) *
+      (m_matrix.col(i).end(n-i-1).adjoint() * matQ.corner(BottomRight,n-i-1,n-i-1)).lazy()).lazy();
+
+    m_matrix.const_cast_derived().coeffRef(i+1,i) = tmp;
+  }
+  return matQ;
+}
+
+#endif // EIGEN_HIDE_HEAVY_CODE
+
+/** constructs and returns the matrix H.
+  * Note that the matrix H is equivalent to the upper part of the packed matrix
+  * (including the lower sub-diagonal). Therefore, it might be often sufficient
+  * to directly use the packed matrix instead of creating a new one.
+  */
+template<typename MatrixType>
+typename HessenbergDecomposition<MatrixType>::MatrixType
+HessenbergDecomposition<MatrixType>::matrixH(void) const
+{
+  // FIXME should this function (and other similar) rather take a matrix as argument
+  // and fill it (to avoid temporaries)
+  int n = m_matrix.rows();
+  MatrixType matH = m_matrix;
+  if (n>2)
+    matH.corner(BottomLeft,n-2, n-2).template part<LowerTriangular>().setZero();
+  return matH;
+}
+
+#endif // EIGEN_HESSENBERGDECOMPOSITION_H
diff --git a/extern/Eigen2/Eigen/src/QR/QR.h b/extern/Eigen2/Eigen/src/QR/QR.h
new file mode 100644
index 00000000000..90751dd428d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/QR.h
@@ -0,0 +1,334 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_QR_H
+#define EIGEN_QR_H
+
+/** \ingroup QR_Module
+  * \nonstableyet
+  *
+  * \class QR
+  *
+  * \brief QR decomposition of a matrix
+  *
+  * \param MatrixType the type of the matrix of which we are computing the QR decomposition
+  *
+  * This class performs a QR decomposition using Householder transformations. The result is
+  * stored in a compact way compatible with LAPACK.
+  *
+  * \sa MatrixBase::qr()
+  */
+template<typename MatrixType> class QR
+{
+  public:
+
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef Block<MatrixType, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixRBlockType;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixTypeR;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
+
+    /** 
+    * \brief Default Constructor.
+    *
+    * The default constructor is useful in cases in which the user intends to
+    * perform decompositions via QR::compute(const MatrixType&).
+    */
+    QR() : m_qr(), m_hCoeffs(), m_isInitialized(false) {}
+
+    QR(const MatrixType& matrix)
+      : m_qr(matrix.rows(), matrix.cols()),
+        m_hCoeffs(matrix.cols()),
+        m_isInitialized(false)
+    {
+      compute(matrix);
+    }
+    
+    /** \deprecated use isInjective()
+      * \returns whether or not the matrix is of full rank
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    EIGEN_DEPRECATED bool isFullRank() const 
+    { 
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      return rank() == m_qr.cols(); 
+    }
+    
+    /** \returns the rank of the matrix of which *this is the QR decomposition.
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    int rank() const;
+    
+    /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    inline int dimensionOfKernel() const
+    {
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      return m_qr.cols() - rank();
+    }
+    
+    /** \returns true if the matrix of which *this is the QR decomposition represents an injective
+      *          linear map, i.e. has trivial kernel; false otherwise.
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isInjective() const
+    {
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      return rank() == m_qr.cols();
+    }
+    
+    /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
+      *          linear map; false otherwise.
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isSurjective() const
+    {
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      return rank() == m_qr.rows();
+    }
+
+    /** \returns true if the matrix of which *this is the QR decomposition is invertible.
+      *
+      * \note Since the rank is computed only once, i.e. the first time it is needed, this
+      *       method almost does not perform any further computation.
+      */
+    inline bool isInvertible() const
+    {
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      return isInjective() && isSurjective();
+    }
+    
+    /** \returns a read-only expression of the matrix R of the actual the QR decomposition */
+    const Part<NestByValue<MatrixRBlockType>, UpperTriangular>
+    matrixR(void) const
+    {
+      ei_assert(m_isInitialized && "QR is not initialized.");
+      int cols = m_qr.cols();
+      return MatrixRBlockType(m_qr, 0, 0, cols, cols).nestByValue().template part<UpperTriangular>();
+    }
+
+    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
+      * *this is the QR decomposition, if any exists.
+      *
+      * \param b the right-hand-side of the equation to solve.
+      *
+      * \param result a pointer to the vector/matrix in which to store the solution, if any exists.
+      *          Resized if necessary, so that result->rows()==A.cols() and result->cols()==b.cols().
+      *          If no solution exists, *result is left with undefined coefficients.
+      *
+      * \returns true if any solution exists, false if no solution exists.
+      *
+      * \note If there exist more than one solution, this method will arbitrarily choose one.
+      *       If you need a complete analysis of the space of solutions, take the one solution obtained
+      *       by this method and add to it elements of the kernel, as determined by kernel().
+      *
+      * \note The case where b is a matrix is not yet implemented. Also, this
+      *       code is space inefficient.
+      *
+      * Example: \include QR_solve.cpp
+      * Output: \verbinclude QR_solve.out
+      *
+      * \sa MatrixBase::solveTriangular(), kernel(), computeKernel(), inverse(), computeInverse()
+      */
+    template<typename OtherDerived, typename ResultType>
+    bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const;
+
+    MatrixType matrixQ(void) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+    MatrixType m_qr;
+    VectorType m_hCoeffs;
+    mutable int m_rank;
+    mutable bool m_rankIsUptodate;
+    bool m_isInitialized;
+};
+
+/** \returns the rank of the matrix of which *this is the QR decomposition. */
+template<typename MatrixType>
+int QR<MatrixType>::rank() const
+{
+  ei_assert(m_isInitialized && "QR is not initialized.");
+  if (!m_rankIsUptodate)
+  {
+    RealScalar maxCoeff = m_qr.diagonal().cwise().abs().maxCoeff();
+    int n = m_qr.cols();
+    m_rank = 0;
+    while(m_rank<n && !ei_isMuchSmallerThan(m_qr.diagonal().coeff(m_rank), maxCoeff))
+      ++m_rank;
+    m_rankIsUptodate = true;
+  }
+  return m_rank;
+}
+
+#ifndef EIGEN_HIDE_HEAVY_CODE
+
+template<typename MatrixType>
+void QR<MatrixType>::compute(const MatrixType& matrix)
+{ 
+  m_rankIsUptodate = false;
+  m_qr = matrix;
+  m_hCoeffs.resize(matrix.cols());
+
+  int rows = matrix.rows();
+  int cols = matrix.cols();
+  RealScalar eps2 = precision<RealScalar>()*precision<RealScalar>();
+
+  for (int k = 0; k < cols; ++k)
+  {
+    int remainingSize = rows-k;
+
+    RealScalar beta;
+    Scalar v0 = m_qr.col(k).coeff(k);
+
+    if (remainingSize==1)
+    {
+      if (NumTraits<Scalar>::IsComplex)
+      {
+        // Householder transformation on the remaining single scalar
+        beta = ei_abs(v0);
+        if (ei_real(v0)>0)
+          beta = -beta;
+        m_qr.coeffRef(k,k) = beta;
+        m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
+      }
+      else
+      {
+        m_hCoeffs.coeffRef(k) = 0;
+      }
+    }
+    else if ((beta=m_qr.col(k).end(remainingSize-1).squaredNorm())>eps2)
+    // FIXME what about ei_imag(v0) ??
+    {
+      // form k-th Householder vector
+      beta = ei_sqrt(ei_abs2(v0)+beta);
+      if (ei_real(v0)>=0.)
+        beta = -beta;
+      m_qr.col(k).end(remainingSize-1) /= v0-beta;
+      m_qr.coeffRef(k,k) = beta;
+      Scalar h = m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
+
+      // apply the Householder transformation (I - h v v') to remaining columns, i.e.,
+      // R <- (I - h v v') * R   where v = [1,m_qr(k+1,k), m_qr(k+2,k), ...]
+      int remainingCols = cols - k -1;
+      if (remainingCols>0)
+      {
+        m_qr.coeffRef(k,k) = Scalar(1);
+        m_qr.corner(BottomRight, remainingSize, remainingCols) -= ei_conj(h) * m_qr.col(k).end(remainingSize)
+            * (m_qr.col(k).end(remainingSize).adjoint() * m_qr.corner(BottomRight, remainingSize, remainingCols));
+        m_qr.coeffRef(k,k) = beta;
+      }
+    }
+    else
+    {
+      m_hCoeffs.coeffRef(k) = 0;
+    }
+  }
+  m_isInitialized = true;
+}
+
+template<typename MatrixType>
+template<typename OtherDerived, typename ResultType>
+bool QR<MatrixType>::solve(
+  const MatrixBase<OtherDerived>& b,
+  ResultType *result
+) const
+{
+  ei_assert(m_isInitialized && "QR is not initialized.");
+  const int rows = m_qr.rows();
+  ei_assert(b.rows() == rows);
+  result->resize(rows, b.cols());
+  
+  // TODO(keir): There is almost certainly a faster way to multiply by
+  // Q^T without explicitly forming matrixQ(). Investigate.
+  *result = matrixQ().transpose()*b;
+  
+  if(!isSurjective())
+  {
+    // is result is in the image of R ?
+    RealScalar biggest_in_res = result->corner(TopLeft, m_rank, result->cols()).cwise().abs().maxCoeff();
+    for(int col = 0; col < result->cols(); ++col)
+      for(int row = m_rank; row < result->rows(); ++row)
+        if(!ei_isMuchSmallerThan(result->coeff(row,col), biggest_in_res))
+          return false;
+  }
+  m_qr.corner(TopLeft, m_rank, m_rank)
+      .template marked<UpperTriangular>()
+      .solveTriangularInPlace(result->corner(TopLeft, m_rank, result->cols()));
+
+  return true;
+}
+
+/** \returns the matrix Q */
+template<typename MatrixType>
+MatrixType QR<MatrixType>::matrixQ() const
+{
+  ei_assert(m_isInitialized && "QR is not initialized.");
+  // compute the product Q_0 Q_1 ... Q_n-1,
+  // where Q_k is the k-th Householder transformation I - h_k v_k v_k'
+  // and v_k is the k-th Householder vector [1,m_qr(k+1,k), m_qr(k+2,k), ...]
+  int rows = m_qr.rows();
+  int cols = m_qr.cols();
+  MatrixType res = MatrixType::Identity(rows, cols);
+  for (int k = cols-1; k >= 0; k--)
+  {
+    // to make easier the computation of the transformation, let's temporarily
+    // overwrite m_qr(k,k) such that the end of m_qr.col(k) is exactly our Householder vector.
+    Scalar beta = m_qr.coeff(k,k);
+    m_qr.const_cast_derived().coeffRef(k,k) = 1;
+    int endLength = rows-k;
+    res.corner(BottomRight,endLength, cols-k) -= ((m_hCoeffs.coeff(k) * m_qr.col(k).end(endLength))
+      * (m_qr.col(k).end(endLength).adjoint() * res.corner(BottomRight,endLength, cols-k)).lazy()).lazy();
+    m_qr.const_cast_derived().coeffRef(k,k) = beta;
+  }
+  return res;
+}
+
+#endif // EIGEN_HIDE_HEAVY_CODE
+
+/** \return the QR decomposition of \c *this.
+  *
+  * \sa class QR
+  */
+template<typename Derived>
+const QR<typename MatrixBase<Derived>::PlainMatrixType>
+MatrixBase<Derived>::qr() const
+{
+  return QR<PlainMatrixType>(eval());
+}
+
+
+#endif // EIGEN_QR_H
diff --git a/extern/Eigen2/Eigen/src/QR/QrInstantiations.cpp b/extern/Eigen2/Eigen/src/QR/QrInstantiations.cpp
new file mode 100644
index 00000000000..dacb05d3d1f
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/QrInstantiations.cpp
@@ -0,0 +1,43 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_EXTERN_INSTANTIATIONS
+#define EIGEN_EXTERN_INSTANTIATIONS
+#endif
+#include "../../Core"
+#undef EIGEN_EXTERN_INSTANTIATIONS
+
+#include "../../QR"
+
+namespace Eigen
+{
+
+template static void ei_tridiagonal_qr_step(float* , float* , int, int, float* , int);
+template static void ei_tridiagonal_qr_step(double* , double* , int, int, double* , int);
+template static void ei_tridiagonal_qr_step(float* , float* , int, int, std::complex<float>* , int);
+template static void ei_tridiagonal_qr_step(double* , double* , int, int, std::complex<double>* , int);
+
+EIGEN_QR_MODULE_INSTANTIATE();
+
+}
diff --git a/extern/Eigen2/Eigen/src/QR/SelfAdjointEigenSolver.h b/extern/Eigen2/Eigen/src/QR/SelfAdjointEigenSolver.h
new file mode 100644
index 00000000000..70984efab9d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/SelfAdjointEigenSolver.h
@@ -0,0 +1,402 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SELFADJOINTEIGENSOLVER_H
+#define EIGEN_SELFADJOINTEIGENSOLVER_H
+
+/** \qr_module \ingroup QR_Module
+  * \nonstableyet
+  *
+  * \class SelfAdjointEigenSolver
+  *
+  * \brief Eigen values/vectors solver for selfadjoint matrix
+  *
+  * \param MatrixType the type of the matrix of which we are computing the eigen decomposition
+  *
+  * \note MatrixType must be an actual Matrix type, it can't be an expression type.
+  *
+  * \sa MatrixBase::eigenvalues(), class EigenSolver
+  */
+template<typename _MatrixType> class SelfAdjointEigenSolver
+{
+  public:
+
+    enum {Size = _MatrixType::RowsAtCompileTime };
+    typedef _MatrixType MatrixType;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef std::complex<RealScalar> Complex;
+    typedef Matrix<RealScalar, MatrixType::ColsAtCompileTime, 1> RealVectorType;
+    typedef Matrix<RealScalar, Dynamic, 1> RealVectorTypeX;
+    typedef Tridiagonalization<MatrixType> TridiagonalizationType;
+
+    SelfAdjointEigenSolver()
+        : m_eivec(int(Size), int(Size)),
+          m_eivalues(int(Size))
+    {
+      ei_assert(Size!=Dynamic);
+    }
+
+    SelfAdjointEigenSolver(int size)
+        : m_eivec(size, size),
+          m_eivalues(size)
+    {}
+
+    /** Constructors computing the eigenvalues of the selfadjoint matrix \a matrix,
+      * as well as the eigenvectors if \a computeEigenvectors is true.
+      *
+      * \sa compute(MatrixType,bool), SelfAdjointEigenSolver(MatrixType,MatrixType,bool)
+      */
+    SelfAdjointEigenSolver(const MatrixType& matrix, bool computeEigenvectors = true)
+      : m_eivec(matrix.rows(), matrix.cols()),
+        m_eivalues(matrix.cols())
+    {
+      compute(matrix, computeEigenvectors);
+    }
+
+    /** Constructors computing the eigenvalues of the generalized eigen problem
+      * \f$ Ax = lambda B x \f$ with \a matA the selfadjoint matrix \f$ A \f$
+      * and \a matB the positive definite matrix \f$ B \f$ . The eigenvectors
+      * are computed if \a computeEigenvectors is true.
+      *
+      * \sa compute(MatrixType,MatrixType,bool), SelfAdjointEigenSolver(MatrixType,bool)
+      */
+    SelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
+      : m_eivec(matA.rows(), matA.cols()),
+        m_eivalues(matA.cols())
+    {
+      compute(matA, matB, computeEigenvectors);
+    }
+
+    void compute(const MatrixType& matrix, bool computeEigenvectors = true);
+
+    void compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true);
+
+    /** \returns the computed eigen vectors as a matrix of column vectors */
+    MatrixType eigenvectors(void) const
+    {
+      #ifndef NDEBUG
+      ei_assert(m_eigenvectorsOk);
+      #endif
+      return m_eivec;
+    }
+
+    /** \returns the computed eigen values */
+    RealVectorType eigenvalues(void) const { return m_eivalues; }
+
+    /** \returns the positive square root of the matrix
+      *
+      * \note the matrix itself must be positive in order for this to make sense.
+      */
+    MatrixType operatorSqrt() const
+    {
+      return m_eivec * m_eivalues.cwise().sqrt().asDiagonal() * m_eivec.adjoint();
+    }
+
+    /** \returns the positive inverse square root of the matrix
+      *
+      * \note the matrix itself must be positive definite in order for this to make sense.
+      */
+    MatrixType operatorInverseSqrt() const
+    {
+      return m_eivec * m_eivalues.cwise().inverse().cwise().sqrt().asDiagonal() * m_eivec.adjoint();
+    }
+
+
+  protected:
+    MatrixType m_eivec;
+    RealVectorType m_eivalues;
+    #ifndef NDEBUG
+    bool m_eigenvectorsOk;
+    #endif
+};
+
+#ifndef EIGEN_HIDE_HEAVY_CODE
+
+// from Golub's "Matrix Computations", algorithm 5.1.3
+template<typename Scalar>
+static void ei_givens_rotation(Scalar a, Scalar b, Scalar& c, Scalar& s)
+{
+  if (b==0)
+  {
+    c = 1; s = 0;
+  }
+  else if (ei_abs(b)>ei_abs(a))
+  {
+    Scalar t = -a/b;
+    s = Scalar(1)/ei_sqrt(1+t*t);
+    c = s * t;
+  }
+  else
+  {
+    Scalar t = -b/a;
+    c = Scalar(1)/ei_sqrt(1+t*t);
+    s = c * t;
+  }
+}
+
+/** \internal
+  *
+  * \qr_module
+  *
+  * Performs a QR step on a tridiagonal symmetric matrix represented as a
+  * pair of two vectors \a diag and \a subdiag.
+  *
+  * \param matA the input selfadjoint matrix
+  * \param hCoeffs returned Householder coefficients
+  *
+  * For compilation efficiency reasons, this procedure does not use eigen expression
+  * for its arguments.
+  *
+  * Implemented from Golub's "Matrix Computations", algorithm 8.3.2:
+  * "implicit symmetric QR step with Wilkinson shift"
+  */
+template<typename RealScalar, typename Scalar>
+static void ei_tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, int start, int end, Scalar* matrixQ, int n);
+
+/** Computes the eigenvalues of the selfadjoint matrix \a matrix,
+  * as well as the eigenvectors if \a computeEigenvectors is true.
+  *
+  * \sa SelfAdjointEigenSolver(MatrixType,bool), compute(MatrixType,MatrixType,bool)
+  */
+template<typename MatrixType>
+void SelfAdjointEigenSolver<MatrixType>::compute(const MatrixType& matrix, bool computeEigenvectors)
+{
+  #ifndef NDEBUG
+  m_eigenvectorsOk = computeEigenvectors;
+  #endif
+  assert(matrix.cols() == matrix.rows());
+  int n = matrix.cols();
+  m_eivalues.resize(n,1);
+
+  if(n==1)
+  {
+    m_eivalues.coeffRef(0,0) = ei_real(matrix.coeff(0,0));
+    m_eivec.setOnes();
+    return;
+  }
+
+  m_eivec = matrix;
+
+  // FIXME, should tridiag be a local variable of this function or an attribute of SelfAdjointEigenSolver ?
+  // the latter avoids multiple memory allocation when the same SelfAdjointEigenSolver is used multiple times...
+  // (same for diag and subdiag)
+  RealVectorType& diag = m_eivalues;
+  typename TridiagonalizationType::SubDiagonalType subdiag(n-1);
+  TridiagonalizationType::decomposeInPlace(m_eivec, diag, subdiag, computeEigenvectors);
+
+  int end = n-1;
+  int start = 0;
+  while (end>0)
+  {
+    for (int i = start; i<end; ++i)
+      if (ei_isMuchSmallerThan(ei_abs(subdiag[i]),(ei_abs(diag[i])+ei_abs(diag[i+1]))))
+        subdiag[i] = 0;
+
+    // find the largest unreduced block
+    while (end>0 && subdiag[end-1]==0)
+      end--;
+    if (end<=0)
+      break;
+    start = end - 1;
+    while (start>0 && subdiag[start-1]!=0)
+      start--;
+
+    ei_tridiagonal_qr_step(diag.data(), subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
+  }
+
+  // Sort eigenvalues and corresponding vectors.
+  // TODO make the sort optional ?
+  // TODO use a better sort algorithm !!
+  for (int i = 0; i < n-1; ++i)
+  {
+    int k;
+    m_eivalues.segment(i,n-i).minCoeff(&k);
+    if (k > 0)
+    {
+      std::swap(m_eivalues[i], m_eivalues[k+i]);
+      m_eivec.col(i).swap(m_eivec.col(k+i));
+    }
+  }
+}
+
+/** Computes the eigenvalues of the generalized eigen problem
+  * \f$ Ax = lambda B x \f$ with \a matA the selfadjoint matrix \f$ A \f$
+  * and \a matB the positive definite matrix \f$ B \f$ . The eigenvectors
+  * are computed if \a computeEigenvectors is true.
+  *
+  * \sa SelfAdjointEigenSolver(MatrixType,MatrixType,bool), compute(MatrixType,bool)
+  */
+template<typename MatrixType>
+void SelfAdjointEigenSolver<MatrixType>::
+compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors)
+{
+  ei_assert(matA.cols()==matA.rows() && matB.rows()==matA.rows() && matB.cols()==matB.rows());
+
+  // Compute the cholesky decomposition of matB = L L'
+  LLT<MatrixType> cholB(matB);
+
+  // compute C = inv(L) A inv(L')
+  MatrixType matC = matA;
+  cholB.matrixL().solveTriangularInPlace(matC);
+  // FIXME since we currently do not support A * inv(L'), let's do (inv(L) A')' :
+  matC = matC.adjoint().eval();
+  cholB.matrixL().template marked<LowerTriangular>().solveTriangularInPlace(matC);
+  matC = matC.adjoint().eval();
+  // this version works too:
+//   matC = matC.transpose();
+//   cholB.matrixL().conjugate().template marked<LowerTriangular>().solveTriangularInPlace(matC);
+//   matC = matC.transpose();
+  // FIXME: this should work: (currently it only does for small matrices)
+//   Transpose<MatrixType> trMatC(matC);
+//   cholB.matrixL().conjugate().eval().template marked<LowerTriangular>().solveTriangularInPlace(trMatC);
+
+  compute(matC, computeEigenvectors);
+
+  if (computeEigenvectors)
+  {
+    // transform back the eigen vectors: evecs = inv(U) * evecs
+    cholB.matrixL().adjoint().template marked<UpperTriangular>().solveTriangularInPlace(m_eivec);
+    for (int i=0; i<m_eivec.cols(); ++i)
+      m_eivec.col(i) = m_eivec.col(i).normalized();
+  }
+}
+
+#endif // EIGEN_HIDE_HEAVY_CODE
+
+/** \qr_module
+  *
+  * \returns a vector listing the eigenvalues of this matrix.
+  */
+template<typename Derived>
+inline Matrix<typename NumTraits<typename ei_traits<Derived>::Scalar>::Real, ei_traits<Derived>::ColsAtCompileTime, 1>
+MatrixBase<Derived>::eigenvalues() const
+{
+  ei_assert(Flags&SelfAdjointBit);
+  return SelfAdjointEigenSolver<typename Derived::PlainMatrixType>(eval(),false).eigenvalues();
+}
+
+template<typename Derived, bool IsSelfAdjoint>
+struct ei_operatorNorm_selector
+{
+  static inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
+  operatorNorm(const MatrixBase<Derived>& m)
+  {
+    // FIXME if it is really guaranteed that the eigenvalues are already sorted,
+    // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough.
+    return m.eigenvalues().cwise().abs().maxCoeff();
+  }
+};
+
+template<typename Derived> struct ei_operatorNorm_selector<Derived, false>
+{
+  static inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
+  operatorNorm(const MatrixBase<Derived>& m)
+  {
+    typename Derived::PlainMatrixType m_eval(m);
+    // FIXME if it is really guaranteed that the eigenvalues are already sorted,
+    // then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough.
+    return ei_sqrt(
+             (m_eval*m_eval.adjoint())
+             .template marked<SelfAdjoint>()
+             .eigenvalues()
+             .maxCoeff()
+           );
+  }
+};
+
+/** \qr_module
+  *
+  * \returns the matrix norm of this matrix.
+  */
+template<typename Derived>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
+MatrixBase<Derived>::operatorNorm() const
+{
+  return ei_operatorNorm_selector<Derived, Flags&SelfAdjointBit>
+       ::operatorNorm(derived());
+}
+
+#ifndef EIGEN_EXTERN_INSTANTIATIONS
+template<typename RealScalar, typename Scalar>
+static void ei_tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, int start, int end, Scalar* matrixQ, int n)
+{
+  RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
+  RealScalar e2 = ei_abs2(subdiag[end-1]);
+  RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * ei_sqrt(td*td + e2));
+  RealScalar x = diag[start] - mu;
+  RealScalar z = subdiag[start];
+
+  for (int k = start; k < end; ++k)
+  {
+    RealScalar c, s;
+    ei_givens_rotation(x, z, c, s);
+
+    // do T = G' T G
+    RealScalar sdk = s * diag[k] + c * subdiag[k];
+    RealScalar dkp1 = s * subdiag[k] + c * diag[k+1];
+
+    diag[k] = c * (c * diag[k] - s * subdiag[k]) - s * (c * subdiag[k] - s * diag[k+1]);
+    diag[k+1] = s * sdk + c * dkp1;
+    subdiag[k] = c * sdk - s * dkp1;
+
+    if (k > start)
+      subdiag[k - 1] = c * subdiag[k-1] - s * z;
+
+    x = subdiag[k];
+
+    if (k < end - 1)
+    {
+      z = -s * subdiag[k+1];
+      subdiag[k + 1] = c * subdiag[k+1];
+    }
+
+    // apply the givens rotation to the unit matrix Q = Q * G
+    // G only modifies the two columns k and k+1
+    if (matrixQ)
+    {
+      #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+      #else
+      int kn = k*n;
+      int kn1 = (k+1)*n;
+      #endif
+      // let's do the product manually to avoid the need of temporaries...
+      for (int i=0; i<n; ++i)
+      {
+        #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+        Scalar matrixQ_i_k = matrixQ[i*n+k];
+        matrixQ[i*n+k]   = c * matrixQ_i_k - s * matrixQ[i*n+k+1];
+        matrixQ[i*n+k+1] = s * matrixQ_i_k + c * matrixQ[i*n+k+1];
+        #else
+        Scalar matrixQ_i_k = matrixQ[i+kn];
+        matrixQ[i+kn]  = c * matrixQ_i_k - s * matrixQ[i+kn1];
+        matrixQ[i+kn1] = s * matrixQ_i_k + c * matrixQ[i+kn1];
+        #endif
+      }
+    }
+  }
+}
+#endif
+
+#endif // EIGEN_SELFADJOINTEIGENSOLVER_H
diff --git a/extern/Eigen2/Eigen/src/QR/Tridiagonalization.h b/extern/Eigen2/Eigen/src/QR/Tridiagonalization.h
new file mode 100644
index 00000000000..9ea39be717c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/QR/Tridiagonalization.h
@@ -0,0 +1,431 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_TRIDIAGONALIZATION_H
+#define EIGEN_TRIDIAGONALIZATION_H
+
+/** \ingroup QR_Module
+  * \nonstableyet
+  *
+  * \class Tridiagonalization
+  *
+  * \brief Trigiagonal decomposition of a selfadjoint matrix
+  *
+  * \param MatrixType the type of the matrix of which we are performing the tridiagonalization
+  *
+  * This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that:
+  * \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix.
+  *
+  * \sa MatrixBase::tridiagonalize()
+  */
+template<typename _MatrixType> class Tridiagonalization
+{
+  public:
+
+    typedef _MatrixType MatrixType;
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef typename ei_packet_traits<Scalar>::type Packet;
+
+    enum {
+      Size = MatrixType::RowsAtCompileTime,
+      SizeMinusOne = MatrixType::RowsAtCompileTime==Dynamic
+                   ? Dynamic
+                   : MatrixType::RowsAtCompileTime-1,
+      PacketSize = ei_packet_traits<Scalar>::size
+    };
+
+    typedef Matrix<Scalar, SizeMinusOne, 1> CoeffVectorType;
+    typedef Matrix<RealScalar, Size, 1> DiagonalType;
+    typedef Matrix<RealScalar, SizeMinusOne, 1> SubDiagonalType;
+
+    typedef typename NestByValue<DiagonalCoeffs<MatrixType> >::RealReturnType DiagonalReturnType;
+
+    typedef typename NestByValue<DiagonalCoeffs<
+        NestByValue<Block<MatrixType,SizeMinusOne,SizeMinusOne> > > >::RealReturnType SubDiagonalReturnType;
+
+    /** This constructor initializes a Tridiagonalization object for
+      * further use with Tridiagonalization::compute()
+      */
+    Tridiagonalization(int size = Size==Dynamic ? 2 : Size)
+      : m_matrix(size,size), m_hCoeffs(size-1)
+    {}
+
+    Tridiagonalization(const MatrixType& matrix)
+      : m_matrix(matrix),
+        m_hCoeffs(matrix.cols()-1)
+    {
+      _compute(m_matrix, m_hCoeffs);
+    }
+
+    /** Computes or re-compute the tridiagonalization for the matrix \a matrix.
+      *
+      * This method allows to re-use the allocated data.
+      */
+    void compute(const MatrixType& matrix)
+    {
+      m_matrix = matrix;
+      m_hCoeffs.resize(matrix.rows()-1, 1);
+      _compute(m_matrix, m_hCoeffs);
+    }
+
+    /** \returns the householder coefficients allowing to
+      * reconstruct the matrix Q from the packed data.
+      *
+      * \sa packedMatrix()
+      */
+    inline CoeffVectorType householderCoefficients(void) const { return m_hCoeffs; }
+
+    /** \returns the internal result of the decomposition.
+      *
+      * The returned matrix contains the following information:
+      *  - the strict upper part is equal to the input matrix A
+      *  - the diagonal and lower sub-diagonal represent the tridiagonal symmetric matrix (real).
+      *  - the rest of the lower part contains the Householder vectors that, combined with
+      *    Householder coefficients returned by householderCoefficients(),
+      *    allows to reconstruct the matrix Q as follow:
+      *       Q = H_{N-1} ... H_1 H_0
+      *    where the matrices H are the Householder transformations:
+      *       H_i = (I - h_i * v_i * v_i')
+      *    where h_i == householderCoefficients()[i] and v_i is a Householder vector:
+      *       v_i = [ 0, ..., 0, 1, M(i+2,i), ..., M(N-1,i) ]
+      *
+      * See LAPACK for further details on this packed storage.
+      */
+    inline const MatrixType& packedMatrix(void) const { return m_matrix; }
+
+    MatrixType matrixQ(void) const;
+    MatrixType matrixT(void) const;
+    const DiagonalReturnType diagonal(void) const;
+    const SubDiagonalReturnType subDiagonal(void) const;
+
+    static void decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
+
+  private:
+
+    static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs);
+
+    static void _decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
+
+  protected:
+    MatrixType m_matrix;
+    CoeffVectorType m_hCoeffs;
+};
+
+/** \returns an expression of the diagonal vector */
+template<typename MatrixType>
+const typename Tridiagonalization<MatrixType>::DiagonalReturnType
+Tridiagonalization<MatrixType>::diagonal(void) const
+{
+  return m_matrix.diagonal().nestByValue().real();
+}
+
+/** \returns an expression of the sub-diagonal vector */
+template<typename MatrixType>
+const typename Tridiagonalization<MatrixType>::SubDiagonalReturnType
+Tridiagonalization<MatrixType>::subDiagonal(void) const
+{
+  int n = m_matrix.rows();
+  return Block<MatrixType,SizeMinusOne,SizeMinusOne>(m_matrix, 1, 0, n-1,n-1)
+    .nestByValue().diagonal().nestByValue().real();
+}
+
+/** constructs and returns the tridiagonal matrix T.
+  * Note that the matrix T is equivalent to the diagonal and sub-diagonal of the packed matrix.
+  * Therefore, it might be often sufficient to directly use the packed matrix, or the vector
+  * expressions returned by diagonal() and subDiagonal() instead of creating a new matrix.
+  */
+template<typename MatrixType>
+typename Tridiagonalization<MatrixType>::MatrixType
+Tridiagonalization<MatrixType>::matrixT(void) const
+{
+  // FIXME should this function (and other similar ones) rather take a matrix as argument
+  // and fill it ? (to avoid temporaries)
+  int n = m_matrix.rows();
+  MatrixType matT = m_matrix;
+  matT.corner(TopRight,n-1, n-1).diagonal() = subDiagonal().template cast<Scalar>().conjugate();
+  if (n>2)
+  {
+    matT.corner(TopRight,n-2, n-2).template part<UpperTriangular>().setZero();
+    matT.corner(BottomLeft,n-2, n-2).template part<LowerTriangular>().setZero();
+  }
+  return matT;
+}
+
+#ifndef EIGEN_HIDE_HEAVY_CODE
+
+/** \internal
+  * Performs a tridiagonal decomposition of \a matA in place.
+  *
+  * \param matA the input selfadjoint matrix
+  * \param hCoeffs returned Householder coefficients
+  *
+  * The result is written in the lower triangular part of \a matA.
+  *
+  * Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
+  *
+  * \sa packedMatrix()
+  */
+template<typename MatrixType>
+void Tridiagonalization<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs)
+{
+  assert(matA.rows()==matA.cols());
+  int n = matA.rows();
+//   std::cerr << matA << "\n\n";
+  for (int i = 0; i<n-2; ++i)
+  {
+    // let's consider the vector v = i-th column starting at position i+1
+
+    // start of the householder transformation
+    // squared norm of the vector v skipping the first element
+    RealScalar v1norm2 = matA.col(i).end(n-(i+2)).squaredNorm();
+
+    // FIXME comparing against 1
+    if (ei_isMuchSmallerThan(v1norm2,static_cast<Scalar>(1)))
+    {
+      hCoeffs.coeffRef(i) = 0.;
+    }
+    else
+    {
+      Scalar v0 = matA.col(i).coeff(i+1);
+      RealScalar beta = ei_sqrt(ei_abs2(v0)+v1norm2);
+      if (ei_real(v0)>=0.)
+        beta = -beta;
+      matA.col(i).end(n-(i+2)) *= (Scalar(1)/(v0-beta));
+      matA.col(i).coeffRef(i+1) = beta;
+      Scalar h = (beta - v0) / beta;
+      // end of the householder transformation
+
+      // Apply similarity transformation to remaining columns,
+      // i.e., A = H' A H where H = I - h v v' and v = matA.col(i).end(n-i-1)
+
+      matA.col(i).coeffRef(i+1) = 1;
+
+      /* This is the initial algorithm which minimize operation counts and maximize
+       * the use of Eigen's expression. Unfortunately, the first matrix-vector product
+       * using Part<LowerTriangular|Selfadjoint>  is very very slow */
+      #ifdef EIGEN_NEVER_DEFINED
+      // matrix - vector product
+      hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular|SelfAdjoint>()
+                                * (h * matA.col(i).end(n-i-1))).lazy();
+      // simple axpy
+      hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
+                            * matA.col(i).end(n-i-1);
+      // rank-2 update
+      //Block<MatrixType,Dynamic,1> B(matA,i+1,i,n-i-1,1);
+      matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular>() -=
+            (matA.col(i).end(n-i-1) * hCoeffs.end(n-i-1).adjoint()).lazy()
+          + (hCoeffs.end(n-i-1) * matA.col(i).end(n-i-1).adjoint()).lazy();
+      #endif
+      /* end initial algorithm */
+
+      /* If we still want to minimize operation count (i.e., perform operation on the lower part only)
+       * then we could provide the following algorithm for selfadjoint - vector product. However, a full
+       * matrix-vector product is still faster (at least for dynamic size, and not too small, did not check
+       * small matrices). The algo performs block matrix-vector and transposed matrix vector products. */
+      #ifdef EIGEN_NEVER_DEFINED
+      int n4 = (std::max(0,n-4)/4)*4;
+      hCoeffs.end(n-i-1).setZero();
+      for (int b=i+1; b<n4; b+=4)
+      {
+        // the ?x4 part:
+        hCoeffs.end(b-4) +=
+            Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4) * matA.template block<4,1>(b,i);
+        // the respective transposed part:
+        Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
+            Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4).adjoint() * Block<MatrixType,Dynamic,1>(matA,b+4,i,n-b-4,1);
+        // the 4x4 block diagonal:
+        Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
+            (Block<MatrixType,4,4>(matA,b,b,4,4).template part<LowerTriangular|SelfAdjoint>()
+             * (h * Block<MatrixType,4,1>(matA,b,i,4,1))).lazy();
+      }
+      #endif
+      // todo: handle the remaining part
+      /* end optimized selfadjoint - vector product */
+
+      /* Another interesting note: the above rank-2 update is much slower than the following hand written loop.
+       * After an analyze of the ASM, it seems GCC (4.2) generate poor code because of the Block. Moreover,
+       * if we remove the specialization of Block for Matrix then it is even worse, much worse ! */
+      #ifdef EIGEN_NEVER_DEFINED
+      for (int j1=i+1; j1<n; ++j1)
+      for (int i1=j1;  i1<n; ++i1)
+        matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+                              + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
+      #endif
+      /* end hand writen partial rank-2 update */
+
+      /* The current fastest implementation: the full matrix is used, no "optimization" to use/compute
+       * only half of the matrix. Custom vectorization of the inner col -= alpha X + beta Y such that access
+       * to col are always aligned. Once we support that in Assign, then the algorithm could be rewriten as
+       * a single compact expression. This code is therefore a good benchmark when will do that. */
+
+      // let's use the end of hCoeffs to store temporary values:
+      hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1) * (h * matA.col(i).end(n-i-1))).lazy();
+      // FIXME in the above expr a temporary is created because of the scalar multiple by h
+
+      hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
+                            * matA.col(i).end(n-i-1);
+
+      const Scalar* EIGEN_RESTRICT pb = &matA.coeffRef(0,i);
+      const Scalar* EIGEN_RESTRICT pa = (&hCoeffs.coeffRef(0)) - 1;
+      for (int j1=i+1; j1<n; ++j1)
+      {
+        int starti = i+1;
+        int alignedEnd = starti;
+        if (PacketSize>1)
+        {
+          int alignedStart = (starti) + ei_alignmentOffset(&matA.coeffRef(starti,j1), n-starti);
+          alignedEnd = alignedStart + ((n-alignedStart)/PacketSize)*PacketSize;
+
+          for (int i1=starti; i1<alignedStart; ++i1)
+            matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+                                  + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
+
+          Packet tmp0 = ei_pset1(hCoeffs.coeff(j1-1));
+          Packet tmp1 = ei_pset1(matA.coeff(j1,i));
+          Scalar* pc = &matA.coeffRef(0,j1);
+          for (int i1=alignedStart ; i1<alignedEnd; i1+=PacketSize)
+            ei_pstore(pc+i1,ei_psub(ei_pload(pc+i1),
+              ei_padd(ei_pmul(tmp0, ei_ploadu(pb+i1)),
+                      ei_pmul(tmp1, ei_ploadu(pa+i1)))));
+        }
+        for (int i1=alignedEnd; i1<n; ++i1)
+          matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+                                + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
+      }
+      /* end optimized implementation */
+
+      // note: at that point matA(i+1,i+1) is the (i+1)-th element of the final diagonal
+      // note: the sequence of the beta values leads to the subdiagonal entries
+      matA.col(i).coeffRef(i+1) = beta;
+
+      hCoeffs.coeffRef(i) = h;
+    }
+  }
+  if (NumTraits<Scalar>::IsComplex)
+  {
+    // Householder transformation on the remaining single scalar
+    int i = n-2;
+    Scalar v0 = matA.col(i).coeff(i+1);
+    RealScalar beta = ei_abs(v0);
+    if (ei_real(v0)>=0.)
+      beta = -beta;
+    matA.col(i).coeffRef(i+1) = beta;
+    if(ei_isMuchSmallerThan(beta, Scalar(1))) hCoeffs.coeffRef(i) = Scalar(0);
+    else hCoeffs.coeffRef(i) = (beta - v0) / beta;
+  }
+  else
+  {
+    hCoeffs.coeffRef(n-2) = 0;
+  }
+}
+
+/** reconstructs and returns the matrix Q */
+template<typename MatrixType>
+typename Tridiagonalization<MatrixType>::MatrixType
+Tridiagonalization<MatrixType>::matrixQ(void) const
+{
+  int n = m_matrix.rows();
+  MatrixType matQ = MatrixType::Identity(n,n);
+  for (int i = n-2; i>=0; i--)
+  {
+    Scalar tmp = m_matrix.coeff(i+1,i);
+    m_matrix.const_cast_derived().coeffRef(i+1,i) = 1;
+
+    matQ.corner(BottomRight,n-i-1,n-i-1) -=
+      ((m_hCoeffs.coeff(i) * m_matrix.col(i).end(n-i-1)) *
+      (m_matrix.col(i).end(n-i-1).adjoint() * matQ.corner(BottomRight,n-i-1,n-i-1)).lazy()).lazy();
+
+    m_matrix.const_cast_derived().coeffRef(i+1,i) = tmp;
+  }
+  return matQ;
+}
+
+/** Performs a full decomposition in place */
+template<typename MatrixType>
+void Tridiagonalization<MatrixType>::decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
+{
+  int n = mat.rows();
+  ei_assert(mat.cols()==n && diag.size()==n && subdiag.size()==n-1);
+  if (n==3 && (!NumTraits<Scalar>::IsComplex) )
+  {
+    _decomposeInPlace3x3(mat, diag, subdiag, extractQ);
+  }
+  else
+  {
+    Tridiagonalization tridiag(mat);
+    diag = tridiag.diagonal();
+    subdiag = tridiag.subDiagonal();
+    if (extractQ)
+      mat = tridiag.matrixQ();
+  }
+}
+
+/** \internal
+  * Optimized path for 3x3 matrices.
+  * Especially useful for plane fitting.
+  */
+template<typename MatrixType>
+void Tridiagonalization<MatrixType>::_decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
+{
+  diag[0] = ei_real(mat(0,0));
+  RealScalar v1norm2 = ei_abs2(mat(0,2));
+  if (ei_isMuchSmallerThan(v1norm2, RealScalar(1)))
+  {
+    diag[1] = ei_real(mat(1,1));
+    diag[2] = ei_real(mat(2,2));
+    subdiag[0] = ei_real(mat(0,1));
+    subdiag[1] = ei_real(mat(1,2));
+    if (extractQ)
+      mat.setIdentity();
+  }
+  else
+  {
+    RealScalar beta = ei_sqrt(ei_abs2(mat(0,1))+v1norm2);
+    RealScalar invBeta = RealScalar(1)/beta;
+    Scalar m01 = mat(0,1) * invBeta;
+    Scalar m02 = mat(0,2) * invBeta;
+    Scalar q = RealScalar(2)*m01*mat(1,2) + m02*(mat(2,2) - mat(1,1));
+    diag[1] = ei_real(mat(1,1) + m02*q);
+    diag[2] = ei_real(mat(2,2) - m02*q);
+    subdiag[0] = beta;
+    subdiag[1] = ei_real(mat(1,2) - m01 * q);
+    if (extractQ)
+    {
+      mat(0,0) = 1;
+      mat(0,1) = 0;
+      mat(0,2) = 0;
+      mat(1,0) = 0;
+      mat(1,1) = m01;
+      mat(1,2) = m02;
+      mat(2,0) = 0;
+      mat(2,1) = m02;
+      mat(2,2) = -m01;
+    }
+  }
+}
+
+#endif // EIGEN_HIDE_HEAVY_CODE
+
+#endif // EIGEN_TRIDIAGONALIZATION_H
diff --git a/extern/Eigen2/Eigen/src/SVD/SVD.h b/extern/Eigen2/Eigen/src/SVD/SVD.h
new file mode 100644
index 00000000000..0a52acf3d5b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/SVD/SVD.h
@@ -0,0 +1,645 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SVD_H
+#define EIGEN_SVD_H
+
+/** \ingroup SVD_Module
+  * \nonstableyet
+  *
+  * \class SVD
+  *
+  * \brief Standard SVD decomposition of a matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the SVD decomposition
+  *
+  * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N
+  * with \c M \>= \c N.
+  *
+  *
+  * \sa MatrixBase::SVD()
+  */
+template<typename MatrixType> class SVD
+{
+  private:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+
+    enum {
+      PacketSize = ei_packet_traits<Scalar>::size,
+      AlignmentMask = int(PacketSize)-1,
+      MinSize = EIGEN_ENUM_MIN(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)
+    };
+
+    typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
+
+    typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
+    typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
+
+  public:
+
+    SVD(const MatrixType& matrix)
+      : m_matU(matrix.rows(), std::min(matrix.rows(), matrix.cols())),
+        m_matV(matrix.cols(),matrix.cols()),
+        m_sigma(std::min(matrix.rows(),matrix.cols()))
+    {
+      compute(matrix);
+    }
+
+    template<typename OtherDerived, typename ResultType>
+    bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
+
+    const MatrixUType& matrixU() const { return m_matU; }
+    const SingularValuesType& singularValues() const { return m_sigma; }
+    const MatrixVType& matrixV() const { return m_matV; }
+
+    void compute(const MatrixType& matrix);
+    SVD& sort();
+
+    template<typename UnitaryType, typename PositiveType>
+    void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const;
+    template<typename PositiveType, typename UnitaryType>
+    void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const;
+    template<typename RotationType, typename ScalingType>
+    void computeRotationScaling(RotationType *unitary, ScalingType *positive) const;
+    template<typename ScalingType, typename RotationType>
+    void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
+
+  protected:
+    /** \internal */
+    MatrixUType m_matU;
+    /** \internal */
+    MatrixVType m_matV;
+    /** \internal */
+    SingularValuesType m_sigma;
+};
+
+/** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix
+  *
+  * \note this code has been adapted from JAMA (public domain)
+  */
+template<typename MatrixType>
+void SVD<MatrixType>::compute(const MatrixType& matrix)
+{
+  const int m = matrix.rows();
+  const int n = matrix.cols();
+  const int nu = std::min(m,n);
+
+  m_matU.resize(m, nu);
+  m_matU.setZero();
+  m_sigma.resize(std::min(m,n));
+  m_matV.resize(n,n);
+
+  RowVector e(n);
+  ColVector work(m);
+  MatrixType matA(matrix);
+  const bool wantu = true;
+  const bool wantv = true;
+  int i=0, j=0, k=0;
+
+  // Reduce A to bidiagonal form, storing the diagonal elements
+  // in s and the super-diagonal elements in e.
+  int nct = std::min(m-1,n);
+  int nrt = std::max(0,std::min(n-2,m));
+  for (k = 0; k < std::max(nct,nrt); ++k)
+  {
+    if (k < nct)
+    {
+      // Compute the transformation for the k-th column and
+      // place the k-th diagonal in m_sigma[k].
+      m_sigma[k] = matA.col(k).end(m-k).norm();
+      if (m_sigma[k] != 0.0) // FIXME
+      {
+        if (matA(k,k) < 0.0)
+          m_sigma[k] = -m_sigma[k];
+        matA.col(k).end(m-k) /= m_sigma[k];
+        matA(k,k) += 1.0;
+      }
+      m_sigma[k] = -m_sigma[k];
+    }
+
+    for (j = k+1; j < n; ++j)
+    {
+      if ((k < nct) && (m_sigma[k] != 0.0))
+      {
+        // Apply the transformation.
+        Scalar t = matA.col(k).end(m-k).dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
+        t = -t/matA(k,k);
+        matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
+      }
+
+      // Place the k-th row of A into e for the
+      // subsequent calculation of the row transformation.
+      e[j] = matA(k,j);
+    }
+
+    // Place the transformation in U for subsequent back multiplication.
+    if (wantu & (k < nct))
+      m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
+
+    if (k < nrt)
+    {
+      // Compute the k-th row transformation and place the
+      // k-th super-diagonal in e[k].
+      e[k] = e.end(n-k-1).norm();
+      if (e[k] != 0.0)
+      {
+          if (e[k+1] < 0.0)
+            e[k] = -e[k];
+          e.end(n-k-1) /= e[k];
+          e[k+1] += 1.0;
+      }
+      e[k] = -e[k];
+      if ((k+1 < m) & (e[k] != 0.0))
+      {
+        // Apply the transformation.
+        work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
+        for (j = k+1; j < n; ++j)
+          matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
+      }
+
+      // Place the transformation in V for subsequent back multiplication.
+      if (wantv)
+        m_matV.col(k).end(n-k-1) = e.end(n-k-1);
+    }
+  }
+
+
+  // Set up the final bidiagonal matrix or order p.
+  int p = std::min(n,m+1);
+  if (nct < n)
+    m_sigma[nct] = matA(nct,nct);
+  if (m < p)
+    m_sigma[p-1] = 0.0;
+  if (nrt+1 < p)
+    e[nrt] = matA(nrt,p-1);
+  e[p-1] = 0.0;
+
+  // If required, generate U.
+  if (wantu)
+  {
+    for (j = nct; j < nu; ++j)
+    {
+      m_matU.col(j).setZero();
+      m_matU(j,j) = 1.0;
+    }
+    for (k = nct-1; k >= 0; k--)
+    {
+      if (m_sigma[k] != 0.0)
+      {
+        for (j = k+1; j < nu; ++j)
+        {
+          Scalar t = m_matU.col(k).end(m-k).dot(m_matU.col(j).end(m-k)); // FIXME is it really a dot product we want ?
+          t = -t/m_matU(k,k);
+          m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
+        }
+        m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
+        m_matU(k,k) = Scalar(1) + m_matU(k,k);
+        if (k-1>0)
+          m_matU.col(k).start(k-1).setZero();
+      }
+      else
+      {
+        m_matU.col(k).setZero();
+        m_matU(k,k) = 1.0;
+      }
+    }
+  }
+
+  // If required, generate V.
+  if (wantv)
+  {
+    for (k = n-1; k >= 0; k--)
+    {
+      if ((k < nrt) & (e[k] != 0.0))
+      {
+        for (j = k+1; j < nu; ++j)
+        {
+          Scalar t = m_matV.col(k).end(n-k-1).dot(m_matV.col(j).end(n-k-1)); // FIXME is it really a dot product we want ?
+          t = -t/m_matV(k+1,k);
+          m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
+        }
+      }
+      m_matV.col(k).setZero();
+      m_matV(k,k) = 1.0;
+    }
+  }
+
+  // Main iteration loop for the singular values.
+  int pp = p-1;
+  int iter = 0;
+  Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
+  while (p > 0)
+  {
+    int k=0;
+    int kase=0;
+
+    // Here is where a test for too many iterations would go.
+
+    // This section of the program inspects for
+    // negligible elements in the s and e arrays.  On
+    // completion the variables kase and k are set as follows.
+
+    // kase = 1     if s(p) and e[k-1] are negligible and k<p
+    // kase = 2     if s(k) is negligible and k<p
+    // kase = 3     if e[k-1] is negligible, k<p, and
+    //              s(k), ..., s(p) are not negligible (qr step).
+    // kase = 4     if e(p-1) is negligible (convergence).
+
+    for (k = p-2; k >= -1; --k)
+    {
+      if (k == -1)
+          break;
+      if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
+      {
+          e[k] = 0.0;
+          break;
+      }
+    }
+    if (k == p-2)
+    {
+      kase = 4;
+    }
+    else
+    {
+      int ks;
+      for (ks = p-1; ks >= k; --ks)
+      {
+        if (ks == k)
+          break;
+        Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
+        if (ei_abs(m_sigma[ks]) <= eps*t)
+        {
+          m_sigma[ks] = 0.0;
+          break;
+        }
+      }
+      if (ks == k)
+      {
+        kase = 3;
+      }
+      else if (ks == p-1)
+      {
+        kase = 1;
+      }
+      else
+      {
+        kase = 2;
+        k = ks;
+      }
+    }
+    ++k;
+
+    // Perform the task indicated by kase.
+    switch (kase)
+    {
+
+      // Deflate negligible s(p).
+      case 1:
+      {
+        Scalar f(e[p-2]);
+        e[p-2] = 0.0;
+        for (j = p-2; j >= k; --j)
+        {
+          Scalar t(ei_hypot(m_sigma[j],f));
+          Scalar cs(m_sigma[j]/t);
+          Scalar sn(f/t);
+          m_sigma[j] = t;
+          if (j != k)
+          {
+            f = -sn*e[j-1];
+            e[j-1] = cs*e[j-1];
+          }
+          if (wantv)
+          {
+            for (i = 0; i < n; ++i)
+            {
+              t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
+              m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
+              m_matV(i,j) = t;
+            }
+          }
+        }
+      }
+      break;
+
+      // Split at negligible s(k).
+      case 2:
+      {
+        Scalar f(e[k-1]);
+        e[k-1] = 0.0;
+        for (j = k; j < p; ++j)
+        {
+          Scalar t(ei_hypot(m_sigma[j],f));
+          Scalar cs( m_sigma[j]/t);
+          Scalar sn(f/t);
+          m_sigma[j] = t;
+          f = -sn*e[j];
+          e[j] = cs*e[j];
+          if (wantu)
+          {
+            for (i = 0; i < m; ++i)
+            {
+              t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
+              m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
+              m_matU(i,j) = t;
+            }
+          }
+        }
+      }
+      break;
+
+      // Perform one qr step.
+      case 3:
+      {
+        // Calculate the shift.
+        Scalar scale = std::max(std::max(std::max(std::max(
+                        ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
+                        ei_abs(m_sigma[k])),ei_abs(e[k]));
+        Scalar sp = m_sigma[p-1]/scale;
+        Scalar spm1 = m_sigma[p-2]/scale;
+        Scalar epm1 = e[p-2]/scale;
+        Scalar sk = m_sigma[k]/scale;
+        Scalar ek = e[k]/scale;
+        Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
+        Scalar c = (sp*epm1)*(sp*epm1);
+        Scalar shift = 0.0;
+        if ((b != 0.0) || (c != 0.0))
+        {
+          shift = ei_sqrt(b*b + c);
+          if (b < 0.0)
+            shift = -shift;
+          shift = c/(b + shift);
+        }
+        Scalar f = (sk + sp)*(sk - sp) + shift;
+        Scalar g = sk*ek;
+
+        // Chase zeros.
+
+        for (j = k; j < p-1; ++j)
+        {
+          Scalar t = ei_hypot(f,g);
+          Scalar cs = f/t;
+          Scalar sn = g/t;
+          if (j != k)
+            e[j-1] = t;
+          f = cs*m_sigma[j] + sn*e[j];
+          e[j] = cs*e[j] - sn*m_sigma[j];
+          g = sn*m_sigma[j+1];
+          m_sigma[j+1] = cs*m_sigma[j+1];
+          if (wantv)
+          {
+            for (i = 0; i < n; ++i)
+            {
+              t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
+              m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
+              m_matV(i,j) = t;
+            }
+          }
+          t = ei_hypot(f,g);
+          cs = f/t;
+          sn = g/t;
+          m_sigma[j] = t;
+          f = cs*e[j] + sn*m_sigma[j+1];
+          m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
+          g = sn*e[j+1];
+          e[j+1] = cs*e[j+1];
+          if (wantu && (j < m-1))
+          {
+            for (i = 0; i < m; ++i)
+            {
+              t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
+              m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
+              m_matU(i,j) = t;
+            }
+          }
+        }
+        e[p-2] = f;
+        iter = iter + 1;
+      }
+      break;
+
+      // Convergence.
+      case 4:
+      {
+        // Make the singular values positive.
+        if (m_sigma[k] <= 0.0)
+        {
+          m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
+          if (wantv)
+            m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
+        }
+
+        // Order the singular values.
+        while (k < pp)
+        {
+          if (m_sigma[k] >= m_sigma[k+1])
+            break;
+          Scalar t = m_sigma[k];
+          m_sigma[k] = m_sigma[k+1];
+          m_sigma[k+1] = t;
+          if (wantv && (k < n-1))
+            m_matV.col(k).swap(m_matV.col(k+1));
+          if (wantu && (k < m-1))
+            m_matU.col(k).swap(m_matU.col(k+1));
+          ++k;
+        }
+        iter = 0;
+        p--;
+      }
+      break;
+    } // end big switch
+  } // end iterations
+}
+
+template<typename MatrixType>
+SVD<MatrixType>& SVD<MatrixType>::sort()
+{
+  int mu = m_matU.rows();
+  int mv = m_matV.rows();
+  int n  = m_matU.cols();
+
+  for (int i=0; i<n; ++i)
+  {
+    int  k = i;
+    Scalar p = m_sigma.coeff(i);
+
+    for (int j=i+1; j<n; ++j)
+    {
+      if (m_sigma.coeff(j) > p)
+      {
+        k = j;
+        p = m_sigma.coeff(j);
+      }
+    }
+    if (k != i)
+    {
+      m_sigma.coeffRef(k) = m_sigma.coeff(i);  // i.e.
+      m_sigma.coeffRef(i) = p;                 // swaps the i-th and the k-th elements
+
+      int j = mu;
+      for(int s=0; j!=0; ++s, --j)
+        std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k));
+
+      j = mv;
+      for (int s=0; j!=0; ++s, --j)
+        std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k));
+    }
+  }
+  return *this;
+}
+
+/** \returns the solution of \f$ A x = b \f$ using the current SVD decomposition of A.
+  * The parts of the solution corresponding to zero singular values are ignored.
+  *
+  * \sa MatrixBase::svd(), LU::solve(), LLT::solve()
+  */
+template<typename MatrixType>
+template<typename OtherDerived, typename ResultType>
+bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
+{
+  const int rows = m_matU.rows();
+  ei_assert(b.rows() == rows);
+
+  Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
+  for (int j=0; j<b.cols(); ++j)
+  {
+    Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j);
+
+    for (int i = 0; i <m_matU.cols(); ++i)
+    {
+      Scalar si = m_sigma.coeff(i);
+      if (ei_isMuchSmallerThan(ei_abs(si),maxVal))
+        aux.coeffRef(i) = 0;
+      else
+        aux.coeffRef(i) /= si;
+    }
+
+    result->col(j) = m_matV * aux;
+  }
+  return true;
+}
+
+/** Computes the polar decomposition of the matrix, as a product unitary x positive.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * Only for square matrices.
+  *
+  * \sa computePositiveUnitary(), computeRotationScaling()
+  */
+template<typename MatrixType>
+template<typename UnitaryType, typename PositiveType>
+void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary,
+                                             PositiveType *positive) const
+{
+  ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices");
+  if(unitary) *unitary = m_matU * m_matV.adjoint();
+  if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint();
+}
+
+/** Computes the polar decomposition of the matrix, as a product positive x unitary.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * Only for square matrices.
+  *
+  * \sa computeUnitaryPositive(), computeRotationScaling()
+  */
+template<typename MatrixType>
+template<typename UnitaryType, typename PositiveType>
+void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive,
+                                             PositiveType *unitary) const
+{
+  ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
+  if(unitary) *unitary = m_matU * m_matV.adjoint();
+  if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint();
+}
+
+/** decomposes the matrix as a product rotation x scaling, the scaling being
+  * not necessarily positive.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * This method requires the Geometry module.
+  *
+  * \sa computeScalingRotation(), computeUnitaryPositive()
+  */
+template<typename MatrixType>
+template<typename RotationType, typename ScalingType>
+void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const
+{
+  ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
+  Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
+  Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
+  sv.coeffRef(0) *= x;
+  if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint());
+  if(rotation)
+  {
+    MatrixType m(m_matU);
+    m.col(0) /= x;
+    rotation->lazyAssign(m * m_matV.adjoint());
+  }
+}
+
+/** decomposes the matrix as a product scaling x rotation, the scaling being
+  * not necessarily positive.
+  *
+  * If either pointer is zero, the corresponding computation is skipped.
+  *
+  * This method requires the Geometry module.
+  *
+  * \sa computeRotationScaling(), computeUnitaryPositive()
+  */
+template<typename MatrixType>
+template<typename ScalingType, typename RotationType>
+void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const
+{
+  ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
+  Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
+  Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
+  sv.coeffRef(0) *= x;
+  if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint());
+  if(rotation)
+  {
+    MatrixType m(m_matU);
+    m.col(0) /= x;
+    rotation->lazyAssign(m * m_matV.adjoint());
+  }
+}
+
+
+/** \svd_module
+  * \returns the SVD decomposition of \c *this
+  */
+template<typename Derived>
+inline SVD<typename MatrixBase<Derived>::PlainMatrixType>
+MatrixBase<Derived>::svd() const
+{
+  return SVD<PlainMatrixType>(derived());
+}
+
+#endif // EIGEN_SVD_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/AmbiVector.h b/extern/Eigen2/Eigen/src/Sparse/AmbiVector.h
new file mode 100644
index 00000000000..75001a2fa25
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/AmbiVector.h
@@ -0,0 +1,371 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_AMBIVECTOR_H
+#define EIGEN_AMBIVECTOR_H
+
+/** \internal
+  * Hybrid sparse/dense vector class designed for intensive read-write operations.
+  *
+  * See BasicSparseLLT and SparseProduct for usage examples.
+  */
+template<typename _Scalar> class AmbiVector
+{
+  public:
+    typedef _Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+    AmbiVector(int size)
+      : m_buffer(0), m_size(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1)
+    {
+      resize(size);
+    }
+
+    void init(RealScalar estimatedDensity);
+    void init(int mode);
+
+    void nonZeros() const;
+
+    /** Specifies a sub-vector to work on */
+    void setBounds(int start, int end) { m_start = start; m_end = end; }
+
+    void setZero();
+
+    void restart();
+    Scalar& coeffRef(int i);
+    Scalar coeff(int i);
+
+    class Iterator;
+
+    ~AmbiVector() { delete[] m_buffer; }
+
+    void resize(int size)
+    {
+      if (m_allocatedSize < size)
+        reallocate(size);
+      m_size = size;
+    }
+
+    int size() const { return m_size; }
+
+  protected:
+
+    void reallocate(int size)
+    {
+      // if the size of the matrix is not too large, let's allocate a bit more than needed such
+      // that we can handle dense vector even in sparse mode.
+      delete[] m_buffer;
+      if (size<1000)
+      {
+        int allocSize = (size * sizeof(ListEl))/sizeof(Scalar);
+        m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl);
+        m_buffer = new Scalar[allocSize];
+      }
+      else
+      {
+        m_allocatedElements = (size*sizeof(Scalar))/sizeof(ListEl);
+        m_buffer = new Scalar[size];
+      }
+      m_size = size;
+      m_start = 0;
+      m_end = m_size;
+    }
+
+    void reallocateSparse()
+    {
+      int copyElements = m_allocatedElements;
+      m_allocatedElements = std::min(int(m_allocatedElements*1.5),m_size);
+      int allocSize = m_allocatedElements * sizeof(ListEl);
+      allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0);
+      Scalar* newBuffer = new Scalar[allocSize];
+      memcpy(newBuffer,  m_buffer,  copyElements * sizeof(ListEl));
+    }
+
+  protected:
+    // element type of the linked list
+    struct ListEl
+    {
+      int next;
+      int index;
+      Scalar value;
+    };
+
+    // used to store data in both mode
+    Scalar* m_buffer;
+    int m_size;
+    int m_start;
+    int m_end;
+    int m_allocatedSize;
+    int m_allocatedElements;
+    int m_mode;
+
+    // linked list mode
+    int m_llStart;
+    int m_llCurrent;
+    int m_llSize;
+
+  private:
+    AmbiVector(const AmbiVector&);
+
+};
+
+/** \returns the number of non zeros in the current sub vector */
+template<typename Scalar>
+void AmbiVector<Scalar>::nonZeros() const
+{
+  if (m_mode==IsSparse)
+    return m_llSize;
+  else
+    return m_end - m_start;
+}
+
+template<typename Scalar>
+void AmbiVector<Scalar>::init(RealScalar estimatedDensity)
+{
+  if (estimatedDensity>0.1)
+    init(IsDense);
+  else
+    init(IsSparse);
+}
+
+template<typename Scalar>
+void AmbiVector<Scalar>::init(int mode)
+{
+  m_mode = mode;
+  if (m_mode==IsSparse)
+  {
+    m_llSize = 0;
+    m_llStart = -1;
+  }
+}
+
+/** Must be called whenever we might perform a write access
+  * with an index smaller than the previous one.
+  *
+  * Don't worry, this function is extremely cheap.
+  */
+template<typename Scalar>
+void AmbiVector<Scalar>::restart()
+{
+  m_llCurrent = m_llStart;
+}
+
+/** Set all coefficients of current subvector to zero */
+template<typename Scalar>
+void AmbiVector<Scalar>::setZero()
+{
+  if (m_mode==IsDense)
+  {
+    for (int i=m_start; i<m_end; ++i)
+      m_buffer[i] = Scalar(0);
+  }
+  else
+  {
+    ei_assert(m_mode==IsSparse);
+    m_llSize = 0;
+    m_llStart = -1;
+  }
+}
+
+template<typename Scalar>
+Scalar& AmbiVector<Scalar>::coeffRef(int i)
+{
+  if (m_mode==IsDense)
+    return m_buffer[i];
+  else
+  {
+    ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+    // TODO factorize the following code to reduce code generation
+    ei_assert(m_mode==IsSparse);
+    if (m_llSize==0)
+    {
+      // this is the first element
+      m_llStart = 0;
+      m_llCurrent = 0;
+      ++m_llSize;
+      llElements[0].value = Scalar(0);
+      llElements[0].index = i;
+      llElements[0].next = -1;
+      return llElements[0].value;
+    }
+    else if (i<llElements[m_llStart].index)
+    {
+      // this is going to be the new first element of the list
+      ListEl& el = llElements[m_llSize];
+      el.value = Scalar(0);
+      el.index = i;
+      el.next = m_llStart;
+      m_llStart = m_llSize;
+      ++m_llSize;
+      m_llCurrent = m_llStart;
+      return el.value;
+    }
+    else
+    {
+      int nextel = llElements[m_llCurrent].next;
+      ei_assert(i>=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index");
+      while (nextel >= 0 && llElements[nextel].index<=i)
+      {
+        m_llCurrent = nextel;
+        nextel = llElements[nextel].next;
+      }
+
+      if (llElements[m_llCurrent].index==i)
+      {
+        // the coefficient already exists and we found it !
+        return llElements[m_llCurrent].value;
+      }
+      else
+      {
+        if (m_llSize>=m_allocatedElements)
+          reallocateSparse();
+        ei_internal_assert(m_llSize<m_size && "internal error: overflow in sparse mode");
+        // let's insert a new coefficient
+        ListEl& el = llElements[m_llSize];
+        el.value = Scalar(0);
+        el.index = i;
+        el.next = llElements[m_llCurrent].next;
+        llElements[m_llCurrent].next = m_llSize;
+        ++m_llSize;
+        return el.value;
+      }
+    }
+  }
+}
+
+template<typename Scalar>
+Scalar AmbiVector<Scalar>::coeff(int i)
+{
+  if (m_mode==IsDense)
+    return m_buffer[i];
+  else
+  {
+    ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
+    ei_assert(m_mode==IsSparse);
+    if ((m_llSize==0) || (i<llElements[m_llStart].index))
+    {
+      return Scalar(0);
+    }
+    else
+    {
+      int elid = m_llStart;
+      while (elid >= 0 && llElements[elid].index<i)
+        elid = llElements[elid].next;
+
+      if (llElements[elid].index==i)
+        return llElements[m_llCurrent].value;
+      else
+        return Scalar(0);
+    }
+  }
+}
+
+/** Iterator over the nonzero coefficients */
+template<typename _Scalar>
+class AmbiVector<_Scalar>::Iterator
+{
+  public:
+    typedef _Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    /** Default constructor
+      * \param vec the vector on which we iterate
+      * \param epsilon the minimal value used to prune zero coefficients.
+      * In practice, all coefficients having a magnitude smaller than \a epsilon
+      * are skipped.
+      */
+    Iterator(const AmbiVector& vec, RealScalar epsilon = RealScalar(0.1)*precision<RealScalar>())
+      : m_vector(vec)
+    {
+      m_epsilon = epsilon;
+      m_isDense = m_vector.m_mode==IsDense;
+      if (m_isDense)
+      {
+        m_cachedIndex = m_vector.m_start-1;
+        ++(*this);
+      }
+      else
+      {
+        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+        m_currentEl = m_vector.m_llStart;
+        while (m_currentEl>=0 && ei_abs(llElements[m_currentEl].value)<m_epsilon)
+          m_currentEl = llElements[m_currentEl].next;
+        if (m_currentEl<0)
+        {
+          m_cachedIndex = -1;
+        }
+        else
+        {
+          m_cachedIndex = llElements[m_currentEl].index;
+          m_cachedValue = llElements[m_currentEl].value;
+        }
+      }
+    }
+
+    int index() const { return m_cachedIndex; }
+    Scalar value() const { return m_cachedValue; }
+
+    operator bool() const { return m_cachedIndex>=0; }
+
+    Iterator& operator++()
+    {
+      if (m_isDense)
+      {
+        do {
+          ++m_cachedIndex;
+        } while (m_cachedIndex<m_vector.m_end && ei_abs(m_vector.m_buffer[m_cachedIndex])<m_epsilon);
+        if (m_cachedIndex<m_vector.m_end)
+          m_cachedValue = m_vector.m_buffer[m_cachedIndex];
+        else
+          m_cachedIndex=-1;
+      }
+      else
+      {
+        ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
+        do {
+          m_currentEl = llElements[m_currentEl].next;
+        } while (m_currentEl>=0 && ei_abs(llElements[m_currentEl].value)<m_epsilon);
+        if (m_currentEl<0)
+        {
+          m_cachedIndex = -1;
+        }
+        else
+        {
+          m_cachedIndex = llElements[m_currentEl].index;
+          m_cachedValue = llElements[m_currentEl].value;
+        }
+      }
+      return *this;
+    }
+
+  protected:
+    const AmbiVector& m_vector; // the target vector
+    int m_currentEl;            // the current element in sparse/linked-list mode
+    RealScalar m_epsilon;       // epsilon used to prune zero coefficients
+    int m_cachedIndex;          // current coordinate
+    Scalar m_cachedValue;       // current value
+    bool m_isDense;             // mode of the vector
+};
+
+
+#endif // EIGEN_AMBIVECTOR_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/CholmodSupport.h b/extern/Eigen2/Eigen/src/Sparse/CholmodSupport.h
new file mode 100644
index 00000000000..dfd9c787ae9
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/CholmodSupport.h
@@ -0,0 +1,236 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_CHOLMODSUPPORT_H
+#define EIGEN_CHOLMODSUPPORT_H
+
+template<typename Scalar, typename CholmodType>
+void ei_cholmod_configure_matrix(CholmodType& mat)
+{
+  if (ei_is_same_type<Scalar,float>::ret)
+  {
+    mat.xtype = CHOLMOD_REAL;
+    mat.dtype = 1;
+  }
+  else if (ei_is_same_type<Scalar,double>::ret)
+  {
+    mat.xtype = CHOLMOD_REAL;
+    mat.dtype = 0;
+  }
+  else if (ei_is_same_type<Scalar,std::complex<float> >::ret)
+  {
+    mat.xtype = CHOLMOD_COMPLEX;
+    mat.dtype = 1;
+  }
+  else if (ei_is_same_type<Scalar,std::complex<double> >::ret)
+  {
+    mat.xtype = CHOLMOD_COMPLEX;
+    mat.dtype = 0;
+  }
+  else
+  {
+    ei_assert(false && "Scalar type not supported by CHOLMOD");
+  }
+}
+
+template<typename Derived>
+cholmod_sparse SparseMatrixBase<Derived>::asCholmodMatrix()
+{
+  typedef typename Derived::Scalar Scalar;
+  cholmod_sparse res;
+  res.nzmax   = nonZeros();
+  res.nrow    = rows();;
+  res.ncol    = cols();
+  res.p       = derived()._outerIndexPtr();
+  res.i       = derived()._innerIndexPtr();
+  res.x       = derived()._valuePtr();
+  res.xtype = CHOLMOD_REAL;
+  res.itype = CHOLMOD_INT;
+  res.sorted = 1;
+  res.packed = 1;
+  res.dtype = 0;
+  res.stype = -1;
+
+  ei_cholmod_configure_matrix<Scalar>(res);
+
+  if (Derived::Flags & SelfAdjoint)
+  {
+    if (Derived::Flags & UpperTriangular)
+      res.stype = 1;
+    else if (Derived::Flags & LowerTriangular)
+      res.stype = -1;
+    else
+      res.stype = 0;
+  }
+  else
+    res.stype = 0;
+
+  return res;
+}
+
+template<typename Derived>
+cholmod_dense ei_cholmod_map_eigen_to_dense(MatrixBase<Derived>& mat)
+{
+  EIGEN_STATIC_ASSERT((ei_traits<Derived>::Flags&RowMajorBit)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  typedef typename Derived::Scalar Scalar;
+
+  cholmod_dense res;
+  res.nrow   = mat.rows();
+  res.ncol   = mat.cols();
+  res.nzmax  = res.nrow * res.ncol;
+  res.d      = mat.derived().stride();
+  res.x      = mat.derived().data();
+  res.z      = 0;
+
+  ei_cholmod_configure_matrix<Scalar>(res);
+
+  return res;
+}
+
+template<typename Scalar, int Flags>
+MappedSparseMatrix<Scalar,Flags>::MappedSparseMatrix(cholmod_sparse& cm)
+{
+  m_innerSize = cm.nrow;
+  m_outerSize = cm.ncol;
+  m_outerIndex = reinterpret_cast<int*>(cm.p);
+  m_innerIndices = reinterpret_cast<int*>(cm.i);
+  m_values = reinterpret_cast<Scalar*>(cm.x);
+  m_nnz = m_outerIndex[cm.ncol];
+}
+
+template<typename MatrixType>
+class SparseLLT<MatrixType,Cholmod> : public SparseLLT<MatrixType>
+{
+  protected:
+    typedef SparseLLT<MatrixType> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::RealScalar RealScalar;
+    using Base::MatrixLIsDirty;
+    using Base::SupernodalFactorIsDirty;
+    using Base::m_flags;
+    using Base::m_matrix;
+    using Base::m_status;
+
+  public:
+
+    SparseLLT(int flags = 0)
+      : Base(flags), m_cholmodFactor(0)
+    {
+      cholmod_start(&m_cholmod);
+    }
+
+    SparseLLT(const MatrixType& matrix, int flags = 0)
+      : Base(flags), m_cholmodFactor(0)
+    {
+      cholmod_start(&m_cholmod);
+      compute(matrix);
+    }
+
+    ~SparseLLT()
+    {
+      if (m_cholmodFactor)
+        cholmod_free_factor(&m_cholmodFactor, &m_cholmod);
+      cholmod_finish(&m_cholmod);
+    }
+
+    inline const typename Base::CholMatrixType& matrixL(void) const;
+
+    template<typename Derived>
+    void solveInPlace(MatrixBase<Derived> &b) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+    mutable cholmod_common m_cholmod;
+    cholmod_factor* m_cholmodFactor;
+};
+
+template<typename MatrixType>
+void SparseLLT<MatrixType,Cholmod>::compute(const MatrixType& a)
+{
+  if (m_cholmodFactor)
+  {
+    cholmod_free_factor(&m_cholmodFactor, &m_cholmod);
+    m_cholmodFactor = 0;
+  }
+
+  cholmod_sparse A = const_cast<MatrixType&>(a).asCholmodMatrix();
+  m_cholmod.supernodal = CHOLMOD_AUTO;
+  // TODO
+  if (m_flags&IncompleteFactorization)
+  {
+    m_cholmod.nmethods = 1;
+    m_cholmod.method[0].ordering = CHOLMOD_NATURAL;
+    m_cholmod.postorder = 0;
+  }
+  else
+  {
+    m_cholmod.nmethods = 1;
+    m_cholmod.method[0].ordering = CHOLMOD_NATURAL;
+    m_cholmod.postorder = 0;
+  }
+  m_cholmod.final_ll = 1;
+  m_cholmodFactor = cholmod_analyze(&A, &m_cholmod);
+  cholmod_factorize(&A, m_cholmodFactor, &m_cholmod);
+
+  m_status = (m_status & ~SupernodalFactorIsDirty) | MatrixLIsDirty;
+}
+
+template<typename MatrixType>
+inline const typename SparseLLT<MatrixType>::CholMatrixType&
+SparseLLT<MatrixType,Cholmod>::matrixL() const
+{
+  if (m_status & MatrixLIsDirty)
+  {
+    ei_assert(!(m_status & SupernodalFactorIsDirty));
+
+    cholmod_sparse* cmRes = cholmod_factor_to_sparse(m_cholmodFactor, &m_cholmod);
+    const_cast<typename Base::CholMatrixType&>(m_matrix) = MappedSparseMatrix<Scalar>(*cmRes);
+    free(cmRes);
+
+    m_status = (m_status & ~MatrixLIsDirty);
+  }
+  return m_matrix;
+}
+
+template<typename MatrixType>
+template<typename Derived>
+void SparseLLT<MatrixType,Cholmod>::solveInPlace(MatrixBase<Derived> &b) const
+{
+  const int size = m_cholmodFactor->n;
+  ei_assert(size==b.rows());
+
+  // this uses Eigen's triangular sparse solver
+//   if (m_status & MatrixLIsDirty)
+//     matrixL();
+//   Base::solveInPlace(b);
+  // as long as our own triangular sparse solver is not fully optimal,
+  // let's use CHOLMOD's one:
+  cholmod_dense cdb = ei_cholmod_map_eigen_to_dense(b);
+  cholmod_dense* x = cholmod_solve(CHOLMOD_LDLt, m_cholmodFactor, &cdb, &m_cholmod);
+  b = Matrix<typename Base::Scalar,Dynamic,1>::Map(reinterpret_cast<typename Base::Scalar*>(x->x),b.rows());
+  cholmod_free_dense(&x, &m_cholmod);
+}
+
+#endif // EIGEN_CHOLMODSUPPORT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/CompressedStorage.h b/extern/Eigen2/Eigen/src/Sparse/CompressedStorage.h
new file mode 100644
index 00000000000..4dbd3230985
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/CompressedStorage.h
@@ -0,0 +1,230 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_COMPRESSED_STORAGE_H
+#define EIGEN_COMPRESSED_STORAGE_H
+
+/** Stores a sparse set of values as a list of values and a list of indices.
+  *
+  */
+template<typename Scalar>
+class CompressedStorage
+{
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+  public:
+    CompressedStorage()
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {}
+
+    CompressedStorage(size_t size)
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {
+      resize(size);
+    }
+
+    CompressedStorage(const CompressedStorage& other)
+      : m_values(0), m_indices(0), m_size(0), m_allocatedSize(0)
+    {
+      *this = other;
+    }
+
+    CompressedStorage& operator=(const CompressedStorage& other)
+    {
+      resize(other.size());
+      memcpy(m_values, other.m_values, m_size * sizeof(Scalar));
+      memcpy(m_indices, other.m_indices, m_size * sizeof(int));
+      return *this;
+    }
+
+    void swap(CompressedStorage& other)
+    {
+      std::swap(m_values, other.m_values);
+      std::swap(m_indices, other.m_indices);
+      std::swap(m_size, other.m_size);
+      std::swap(m_allocatedSize, other.m_allocatedSize);
+    }
+
+    ~CompressedStorage()
+    {
+      delete[] m_values;
+      delete[] m_indices;
+    }
+
+    void reserve(size_t size)
+    {
+      size_t newAllocatedSize = m_size + size;
+      if (newAllocatedSize > m_allocatedSize)
+        reallocate(newAllocatedSize);
+    }
+
+    void squeeze()
+    {
+      if (m_allocatedSize>m_size)
+        reallocate(m_size);
+    }
+
+    void resize(size_t size, float reserveSizeFactor = 0)
+    {
+      if (m_allocatedSize<size)
+        reallocate(size + size_t(reserveSizeFactor*size));
+      m_size = size;
+    }
+
+    void append(const Scalar& v, int i)
+    {
+      int id = m_size;
+      resize(m_size+1, 1);
+      m_values[id] = v;
+      m_indices[id] = i;
+    }
+
+    inline size_t size() const { return m_size; }
+    inline size_t allocatedSize() const { return m_allocatedSize; }
+    inline void clear() { m_size = 0; }
+
+    inline Scalar& value(size_t i) { return m_values[i]; }
+    inline const Scalar& value(size_t i) const { return m_values[i]; }
+
+    inline int& index(size_t i) { return m_indices[i]; }
+    inline const int& index(size_t i) const { return m_indices[i]; }
+
+    static CompressedStorage Map(int* indices, Scalar* values, size_t size)
+    {
+      CompressedStorage res;
+      res.m_indices = indices;
+      res.m_values = values;
+      res.m_allocatedSize = res.m_size = size;
+      return res;
+    }
+    
+    /** \returns the largest \c k such that for all \c j in [0,k) index[\c j]\<\a key */
+    inline int searchLowerIndex(int key) const
+    {
+      return searchLowerIndex(0, m_size, key);
+    }
+    
+    /** \returns the largest \c k in [start,end) such that for all \c j in [start,k) index[\c j]\<\a key */
+    inline int searchLowerIndex(size_t start, size_t end, int key) const
+    {
+      while(end>start)
+      {
+        size_t mid = (end+start)>>1;
+        if (m_indices[mid]<key)
+          start = mid+1;
+        else
+          end = mid;
+      }
+      return start;
+    }
+    
+    /** \returns the stored value at index \a key
+      * If the value does not exist, then the value \a defaultValue is returned without any insertion. */
+    inline Scalar at(int key, Scalar defaultValue = Scalar(0)) const
+    {
+      if (m_size==0)
+        return defaultValue;
+      else if (key==m_indices[m_size-1])
+        return m_values[m_size-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+      const size_t id = searchLowerIndex(0,m_size-1,key);
+      return ((id<m_size) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+    }
+    
+    /** Like at(), but the search is performed in the range [start,end) */
+    inline Scalar atInRange(size_t start, size_t end, int key, Scalar defaultValue = Scalar(0)) const
+    {
+      if (start==end)
+        return Scalar(0);
+      else if (end>start && key==m_indices[end-1])
+        return m_values[end-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+      const size_t id = searchLowerIndex(start,end-1,key);
+      return ((id<end) && (m_indices[id]==key)) ? m_values[id] : defaultValue;
+    }
+    
+    /** \returns a reference to the value at index \a key
+      * If the value does not exist, then the value \a defaultValue is inserted
+      * such that the keys are sorted. */
+    inline Scalar& atWithInsertion(int key, Scalar defaultValue = Scalar(0))
+    {
+      size_t id = searchLowerIndex(0,m_size,key);
+      if (id>=m_size || m_indices[id]!=key)
+      {
+        resize(m_size+1,1);
+        for (size_t j=m_size-1; j>id; --j)
+        {
+          m_indices[j] = m_indices[j-1];
+          m_values[j] = m_values[j-1];
+        }
+        m_indices[id] = key;
+        m_values[id] = defaultValue;
+      }
+      return m_values[id];
+    }
+    
+    void prune(Scalar reference, RealScalar epsilon = precision<RealScalar>())
+    {
+      size_t k = 0;
+      size_t n = size();
+      for (size_t i=0; i<n; ++i)
+      {
+        if (!ei_isMuchSmallerThan(value(i), reference, epsilon))
+        {
+          value(k) = value(i);
+          index(k) = index(i);
+          ++k;
+        }
+      }
+      resize(k,0);
+    }
+
+  protected:
+
+    inline void reallocate(size_t size)
+    {
+      Scalar* newValues  = new Scalar[size];
+      int* newIndices = new int[size];
+      size_t copySize = std::min(size, m_size);
+      // copy
+      memcpy(newValues,  m_values,  copySize * sizeof(Scalar));
+      memcpy(newIndices, m_indices, copySize * sizeof(int));
+      // delete old stuff
+      delete[] m_values;
+      delete[] m_indices;
+      m_values = newValues;
+      m_indices = newIndices;
+      m_allocatedSize = size;
+    }
+
+  protected:
+    Scalar* m_values;
+    int* m_indices;
+    size_t m_size;
+    size_t m_allocatedSize;
+
+};
+
+#endif // EIGEN_COMPRESSED_STORAGE_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/CoreIterators.h b/extern/Eigen2/Eigen/src/Sparse/CoreIterators.h
new file mode 100644
index 00000000000..f1520a585ca
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/CoreIterators.h
@@ -0,0 +1,68 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_COREITERATORS_H
+#define EIGEN_COREITERATORS_H
+
+/* This file contains the respective InnerIterator definition of the expressions defined in Eigen/Core
+ */
+
+/** \class InnerIterator
+  * \brief An InnerIterator allows to loop over the element of a sparse (or dense) matrix or expression
+  *
+  * todo
+  */
+
+// generic version for dense matrix and expressions
+template<typename Derived> class MatrixBase<Derived>::InnerIterator
+{
+    typedef typename Derived::Scalar Scalar;
+    enum { IsRowMajor = (Derived::Flags&RowMajorBit)==RowMajorBit };
+  public:
+    EIGEN_STRONG_INLINE InnerIterator(const Derived& expr, int outer)
+      : m_expression(expr), m_inner(0), m_outer(outer), m_end(expr.rows())
+    {}
+
+    EIGEN_STRONG_INLINE Scalar value() const
+    {
+      return (IsRowMajor) ? m_expression.coeff(m_outer, m_inner)
+                          : m_expression.coeff(m_inner, m_outer);
+    }
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++() { m_inner++; return *this; }
+
+    EIGEN_STRONG_INLINE int index() const { return m_inner; }
+    inline int row() const { return IsRowMajor ? m_outer : index(); }
+    inline int col() const { return IsRowMajor ? index() : m_outer; }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_inner < m_end && m_inner>=0; }
+
+  protected:
+    const Derived& m_expression;
+    int m_inner;
+    const int m_outer;
+    const int m_end;
+};
+
+#endif // EIGEN_COREITERATORS_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/DynamicSparseMatrix.h b/extern/Eigen2/Eigen/src/Sparse/DynamicSparseMatrix.h
new file mode 100644
index 00000000000..7119a84bd51
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/DynamicSparseMatrix.h
@@ -0,0 +1,297 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_DYNAMIC_SPARSEMATRIX_H
+#define EIGEN_DYNAMIC_SPARSEMATRIX_H
+
+/** \class DynamicSparseMatrix
+  *
+  * \brief A sparse matrix class designed for matrix assembly purpose
+  *
+  * \param _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * Unlike SparseMatrix, this class provides a much higher degree of flexibility. In particular, it allows
+  * random read/write accesses in log(rho*outer_size) where \c rho is the probability that a coefficient is
+  * nonzero and outer_size is the number of columns if the matrix is column-major and the number of rows
+  * otherwise.
+  * 
+  * Internally, the data are stored as a std::vector of compressed vector. The performances of random writes might
+  * decrease as the number of nonzeros per inner-vector increase. In practice, we observed very good performance
+  * till about 100 nonzeros/vector, and the performance remains relatively good till 500 nonzeros/vectors.
+  *
+  * \see SparseMatrix
+  */
+template<typename _Scalar, int _Flags>
+struct ei_traits<DynamicSparseMatrix<_Scalar, _Flags> >
+{
+  typedef _Scalar Scalar;
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = Dynamic,
+    MaxRowsAtCompileTime = Dynamic,
+    MaxColsAtCompileTime = Dynamic,
+    Flags = SparseBit | _Flags,
+    CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    SupportedAccessPatterns = OuterRandomAccessPattern
+  };
+};
+
+template<typename _Scalar, int _Flags>
+class DynamicSparseMatrix
+  : public SparseMatrixBase<DynamicSparseMatrix<_Scalar, _Flags> >
+{
+  public:
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(DynamicSparseMatrix)
+    // FIXME: why are these operator already alvailable ???
+    // EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(DynamicSparseMatrix, +=)
+    // EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(DynamicSparseMatrix, -=)
+    typedef MappedSparseMatrix<Scalar,Flags> Map;
+
+  protected:
+
+    enum { IsRowMajor = Base::IsRowMajor };
+    typedef DynamicSparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
+
+    int m_innerSize;
+    std::vector<CompressedStorage<Scalar> > m_data;
+
+  public:
+  
+    inline int rows() const { return IsRowMajor ? outerSize() : m_innerSize; }
+    inline int cols() const { return IsRowMajor ? m_innerSize : outerSize(); }
+    inline int innerSize() const { return m_innerSize; }
+    inline int outerSize() const { return m_data.size(); }
+    inline int innerNonZeros(int j) const { return m_data[j].size(); }
+    
+    std::vector<CompressedStorage<Scalar> >& _data() { return m_data; }
+    const std::vector<CompressedStorage<Scalar> >& _data() const { return m_data; }
+
+    /** \returns the coefficient value at given position \a row, \a col
+      * This operation involes a log(rho*outer_size) binary search.
+      */
+    inline Scalar coeff(int row, int col) const
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      return m_data[outer].at(inner);
+    }
+
+    /** \returns a reference to the coefficient value at given position \a row, \a col
+      * This operation involes a log(rho*outer_size) binary search. If the coefficient does not
+      * exist yet, then a sorted insertion into a sequential buffer is performed.
+      */
+    inline Scalar& coeffRef(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      return m_data[outer].atWithInsertion(inner);
+    }
+
+    class InnerIterator;
+
+    inline void setZero()
+    {
+      for (int j=0; j<outerSize(); ++j)
+        m_data[j].clear();
+    }
+
+    /** \returns the number of non zero coefficients */
+    inline int nonZeros() const
+    {
+      int res = 0;
+      for (int j=0; j<outerSize(); ++j)
+        res += m_data[j].size();
+      return res;
+    }
+
+    /** Set the matrix to zero and reserve the memory for \a reserveSize nonzero coefficients. */
+    inline void startFill(int reserveSize = 1000)
+    {
+      if (outerSize()>0)
+      {
+        int reserveSizePerVector = std::max(reserveSize/outerSize(),4);
+        for (int j=0; j<outerSize(); ++j)
+        {
+          m_data[j].clear();
+          m_data[j].reserve(reserveSizePerVector);
+        }
+      }
+    }
+
+    /** inserts a nonzero coefficient at given coordinates \a row, \a col and returns its reference assuming that:
+      *  1 - the coefficient does not exist yet
+      *  2 - this the coefficient with greater inner coordinate for the given outer coordinate.
+      * In other words, assuming \c *this is column-major, then there must not exists any nonzero coefficient of coordinates
+      * \c i \c x \a col such that \c i >= \a row. Otherwise the matrix is invalid.
+      * 
+      * \see fillrand(), coeffRef()
+      */
+    inline Scalar& fill(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      ei_assert(outer<int(m_data.size()) && inner<m_innerSize);
+      ei_assert((m_data[outer].size()==0) || (m_data[outer].index(m_data[outer].size()-1)<inner));
+      m_data[outer].append(0, inner);
+      return m_data[outer].value(m_data[outer].size()-1);
+    }
+
+    /** Like fill() but with random inner coordinates.
+      * Compared to the generic coeffRef(), the unique limitation is that we assume
+      * the coefficient does not exist yet.
+      */
+    inline Scalar& fillrand(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      
+      int startId = 0;
+      int id = m_data[outer].size() - 1;
+      m_data[outer].resize(id+2,1);
+
+      while ( (id >= startId) && (m_data[outer].index(id) > inner) )
+      {
+        m_data[outer].index(id+1) = m_data[outer].index(id);
+        m_data[outer].value(id+1) = m_data[outer].value(id);
+        --id;
+      }
+      m_data[outer].index(id+1) = inner;
+      m_data[outer].value(id+1) = 0;
+      return m_data[outer].value(id+1);
+    }
+
+    /** Does nothing. Provided for compatibility with SparseMatrix. */
+    inline void endFill() {}
+    
+    void prune(Scalar reference, RealScalar epsilon = precision<RealScalar>())
+    {
+      for (int j=0; j<outerSize(); ++j)
+        m_data[j].prune(reference,epsilon);
+    }
+
+    /** Resize the matrix without preserving the data (the matrix is set to zero)
+      */
+    void resize(int rows, int cols)
+    {
+      const int outerSize = IsRowMajor ? rows : cols;
+      m_innerSize = IsRowMajor ? cols : rows;
+      setZero();
+      if (int(m_data.size()) != outerSize)
+      {
+        m_data.resize(outerSize);
+      }
+    }
+    
+    void resizeAndKeepData(int rows, int cols)
+    {
+      const int outerSize = IsRowMajor ? rows : cols;
+      const int innerSize = IsRowMajor ? cols : rows;
+      if (m_innerSize>innerSize)
+      {
+        // remove all coefficients with innerCoord>=innerSize
+        // TODO
+        std::cerr << "not implemented yet\n";
+        exit(2);
+      }
+      if (m_data.size() != outerSize)
+      {
+        m_data.resize(outerSize);
+      }
+    }
+
+    inline DynamicSparseMatrix()
+      : m_innerSize(0), m_data(0)
+    {
+      ei_assert(innerSize()==0 && outerSize()==0);
+    }
+
+    inline DynamicSparseMatrix(int rows, int cols)
+      : m_innerSize(0)
+    {
+      resize(rows, cols);
+    }
+
+    template<typename OtherDerived>
+    inline DynamicSparseMatrix(const SparseMatrixBase<OtherDerived>& other)
+      : m_innerSize(0)
+    {
+      *this = other.derived();
+    }
+
+    inline DynamicSparseMatrix(const DynamicSparseMatrix& other)
+      : Base(), m_innerSize(0)
+    {
+      *this = other.derived();
+    }
+
+    inline void swap(DynamicSparseMatrix& other)
+    {
+      //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
+      std::swap(m_innerSize, other.m_innerSize);
+      //std::swap(m_outerSize, other.m_outerSize);
+      m_data.swap(other.m_data);
+    }
+
+    inline DynamicSparseMatrix& operator=(const DynamicSparseMatrix& other)
+    {
+      if (other.isRValue())
+      {
+        swap(other.const_cast_derived());
+      }
+      else
+      {
+        resize(other.rows(), other.cols());
+        m_data = other.m_data;
+      }
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    inline DynamicSparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      return SparseMatrixBase<DynamicSparseMatrix>::operator=(other.derived());
+    }
+
+    /** Destructor */
+    inline ~DynamicSparseMatrix() {}
+};
+
+template<typename Scalar, int _Flags>
+class DynamicSparseMatrix<Scalar,_Flags>::InnerIterator : public SparseVector<Scalar,_Flags>::InnerIterator
+{
+    typedef typename SparseVector<Scalar,_Flags>::InnerIterator Base;
+  public:
+    InnerIterator(const DynamicSparseMatrix& mat, int outer)
+      : Base(mat.m_data[outer]), m_outer(outer)
+    {}
+    
+    inline int row() const { return IsRowMajor ? m_outer : Base::index(); }
+    inline int col() const { return IsRowMajor ? Base::index() : m_outer; }
+
+
+  protected:
+    const int m_outer;
+};
+
+#endif // EIGEN_DYNAMIC_SPARSEMATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/MappedSparseMatrix.h b/extern/Eigen2/Eigen/src/Sparse/MappedSparseMatrix.h
new file mode 100644
index 00000000000..f4935d8344e
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/MappedSparseMatrix.h
@@ -0,0 +1,175 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_MAPPED_SPARSEMATRIX_H
+#define EIGEN_MAPPED_SPARSEMATRIX_H
+
+/** \class MappedSparseMatrix
+  *
+  * \brief Sparse matrix
+  *
+  * \param _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+  *
+  */
+template<typename _Scalar, int _Flags>
+struct ei_traits<MappedSparseMatrix<_Scalar, _Flags> > : ei_traits<SparseMatrix<_Scalar, _Flags> >
+{};
+
+template<typename _Scalar, int _Flags>
+class MappedSparseMatrix
+  : public SparseMatrixBase<MappedSparseMatrix<_Scalar, _Flags> >
+{
+  public:
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(MappedSparseMatrix)
+
+  protected:
+    enum { IsRowMajor = Base::IsRowMajor };
+
+    int m_outerSize;
+    int m_innerSize;
+    int m_nnz;
+    int* m_outerIndex;
+    int* m_innerIndices;
+    Scalar* m_values;
+
+  public:
+
+    inline int rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+    inline int cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+    inline int innerSize() const { return m_innerSize; }
+    inline int outerSize() const { return m_outerSize; }
+    inline int innerNonZeros(int j) const { return m_outerIndex[j+1]-m_outerIndex[j]; }
+
+    //----------------------------------------
+    // direct access interface
+    inline const Scalar* _valuePtr() const { return m_values; }
+    inline Scalar* _valuePtr() { return m_values; }
+
+    inline const int* _innerIndexPtr() const { return m_innerIndices; }
+    inline int* _innerIndexPtr() { return m_innerIndices; }
+
+    inline const int* _outerIndexPtr() const { return m_outerIndex; }
+    inline int* _outerIndexPtr() { return m_outerIndex; }
+    //----------------------------------------
+
+    inline Scalar coeff(int row, int col) const
+    {
+      const int outer = RowMajor ? row : col;
+      const int inner = RowMajor ? col : row;
+
+      int start = m_outerIndex[outer];
+      int end = m_outerIndex[outer+1];
+      if (start==end)
+        return Scalar(0);
+      else if (end>0 && inner==m_innerIndices[end-1])
+        return m_values[end-1];
+      // ^^  optimization: let's first check if it is the last coefficient
+      // (very common in high level algorithms)
+
+      const int* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end-1],inner);
+      const int id = r-&m_innerIndices[0];
+      return ((*r==inner) && (id<end)) ? m_values[id] : Scalar(0);
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      const int outer = RowMajor ? row : col;
+      const int inner = RowMajor ? col : row;
+
+      int start = m_outerIndex[outer];
+      int end = m_outerIndex[outer+1];
+      ei_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+      ei_assert(end>start && "coeffRef cannot be called on a zero coefficient");
+      int* r = std::lower_bound(&m_innerIndices[start],&m_innerIndices[end],inner);
+      const int id = r-&m_innerIndices[0];
+      ei_assert((*r==inner) && (id<end) && "coeffRef cannot be called on a zero coefficient");
+      return m_values[id];
+    }
+
+    class InnerIterator;
+
+    /** \returns the number of non zero coefficients */
+    inline int nonZeros() const  { return m_nnz; }
+
+    inline MappedSparseMatrix(int rows, int cols, int nnz, int* outerIndexPtr, int* innerIndexPtr, Scalar* valuePtr)
+      : m_outerSize(IsRowMajor?rows:cols), m_innerSize(IsRowMajor?cols:rows), m_nnz(nnz), m_outerIndex(outerIndexPtr),
+        m_innerIndices(innerIndexPtr), m_values(valuePtr)
+    {}
+
+    #ifdef EIGEN_TAUCS_SUPPORT
+    explicit MappedSparseMatrix(taucs_ccs_matrix& taucsMatrix);
+    #endif
+
+    #ifdef EIGEN_CHOLMOD_SUPPORT
+    explicit MappedSparseMatrix(cholmod_sparse& cholmodMatrix);
+    #endif
+
+    #ifdef EIGEN_SUPERLU_SUPPORT
+    explicit MappedSparseMatrix(SluMatrix& sluMatrix);
+    #endif
+
+    /** Empty destructor */
+    inline ~MappedSparseMatrix() {}
+};
+
+template<typename Scalar, int _Flags>
+class MappedSparseMatrix<Scalar,_Flags>::InnerIterator
+{
+  public:
+    InnerIterator(const MappedSparseMatrix& mat, int outer)
+      : m_matrix(mat),
+        m_outer(outer),
+        m_id(mat._outerIndexPtr()[outer]),
+        m_start(m_id),
+        m_end(mat._outerIndexPtr()[outer+1])
+    {}
+
+    template<unsigned int Added, unsigned int Removed>
+    InnerIterator(const Flagged<MappedSparseMatrix,Added,Removed>& mat, int outer)
+      : m_matrix(mat._expression()), m_id(m_matrix._outerIndexPtr()[outer]),
+        m_start(m_id), m_end(m_matrix._outerIndexPtr()[outer+1])
+    {}
+
+    inline InnerIterator& operator++() { m_id++; return *this; }
+
+    inline Scalar value() const { return m_matrix._valuePtr()[m_id]; }
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix._valuePtr()[m_id]); }
+
+    inline int index() const { return m_matrix._innerIndexPtr()[m_id]; }
+    inline int row() const { return IsRowMajor ? m_outer : index(); }
+    inline int col() const { return IsRowMajor ? index() : m_outer; }
+
+    inline operator bool() const { return (m_id < m_end) && (m_id>=m_start); }
+
+  protected:
+    const MappedSparseMatrix& m_matrix;
+    const int m_outer;
+    int m_id;
+    const int m_start;
+    const int m_end;
+};
+
+#endif // EIGEN_MAPPED_SPARSEMATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/RandomSetter.h b/extern/Eigen2/Eigen/src/Sparse/RandomSetter.h
new file mode 100644
index 00000000000..d908e315f3b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/RandomSetter.h
@@ -0,0 +1,330 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_RANDOMSETTER_H
+#define EIGEN_RANDOMSETTER_H
+
+/** Represents a std::map
+  *
+  * \see RandomSetter
+  */
+template<typename Scalar> struct StdMapTraits
+{
+  typedef int KeyType;
+  typedef std::map<KeyType,Scalar> Type;
+  enum {
+    IsSorted = 1
+  };
+
+  static void setInvalidKey(Type&, const KeyType&) {}
+};
+
+#ifdef EIGEN_UNORDERED_MAP_SUPPORT
+/** Represents a std::unordered_map
+  *
+  * To use it you need to both define EIGEN_UNORDERED_MAP_SUPPORT and include the unordered_map header file
+  * yourself making sure that unordered_map is defined in the std namespace.
+  * 
+  * For instance, with current version of gcc you can either enable C++0x standard (-std=c++0x) or do:
+  * \code
+  * #include <tr1/unordered_map>
+  * #define EIGEN_UNORDERED_MAP_SUPPORT
+  * namespace std {
+  *   using std::tr1::unordered_map;
+  * }
+  * \endcode
+  *
+  * \see RandomSetter
+  */
+template<typename Scalar> struct StdUnorderedMapTraits
+{
+  typedef int KeyType;
+  typedef std::unordered_map<KeyType,Scalar> Type;
+  enum {
+    IsSorted = 0
+  };
+
+  static void setInvalidKey(Type&, const KeyType&) {}
+};
+#endif // EIGEN_UNORDERED_MAP_SUPPORT
+
+#ifdef _DENSE_HASH_MAP_H_
+/** Represents a google::dense_hash_map
+  *
+  * \see RandomSetter
+  */
+template<typename Scalar> struct GoogleDenseHashMapTraits
+{
+  typedef int KeyType;
+  typedef google::dense_hash_map<KeyType,Scalar> Type;
+  enum {
+    IsSorted = 0
+  };
+
+  static void setInvalidKey(Type& map, const KeyType& k)
+  { map.set_empty_key(k); }
+};
+#endif
+
+#ifdef _SPARSE_HASH_MAP_H_
+/** Represents a google::sparse_hash_map
+  *
+  * \see RandomSetter
+  */
+template<typename Scalar> struct GoogleSparseHashMapTraits
+{
+  typedef int KeyType;
+  typedef google::sparse_hash_map<KeyType,Scalar> Type;
+  enum {
+    IsSorted = 0
+  };
+
+  static void setInvalidKey(Type&, const KeyType&) {}
+};
+#endif
+
+/** \class RandomSetter
+  *
+  * \brief The RandomSetter is a wrapper object allowing to set/update a sparse matrix with random access
+  *
+  * \param SparseMatrixType the type of the sparse matrix we are updating
+  * \param MapTraits a traits class representing the map implementation used for the temporary sparse storage.
+  *                  Its default value depends on the system.
+  * \param OuterPacketBits defines the number of rows (or columns) manage by a single map object
+  *                        as a power of two exponent.
+  *
+  * This class temporarily represents a sparse matrix object using a generic map implementation allowing for
+  * efficient random access. The conversion from the compressed representation to a hash_map object is performed
+  * in the RandomSetter constructor, while the sparse matrix is updated back at destruction time. This strategy
+  * suggest the use of nested blocks as in this example:
+  *
+  * \code
+  * SparseMatrix<double> m(rows,cols);
+  * {
+  *   RandomSetter<SparseMatrix<double> > w(m);
+  *   // don't use m but w instead with read/write random access to the coefficients:
+  *   for(;;)
+  *     w(rand(),rand()) = rand;
+  * }
+  * // when w is deleted, the data are copied back to m
+  * // and m is ready to use.
+  * \endcode
+  *
+  * Since hash_map objects are not fully sorted, representing a full matrix as a single hash_map would
+  * involve a big and costly sort to update the compressed matrix back. To overcome this issue, a RandomSetter
+  * use multiple hash_map, each representing 2^OuterPacketBits columns or rows according to the storage order.
+  * To reach optimal performance, this value should be adjusted according to the average number of nonzeros
+  * per rows/columns.
+  *
+  * The possible values for the template parameter MapTraits are:
+  *  - \b StdMapTraits: corresponds to std::map. (does not perform very well)
+  *  - \b GnuHashMapTraits: corresponds to __gnu_cxx::hash_map (available only with GCC)
+  *  - \b GoogleDenseHashMapTraits: corresponds to google::dense_hash_map (best efficiency, reasonable memory consumption)
+  *  - \b GoogleSparseHashMapTraits: corresponds to google::sparse_hash_map (best memory consumption, relatively good performance)
+  *
+  * The default map implementation depends on the availability, and the preferred order is:
+  * GoogleSparseHashMapTraits, GnuHashMapTraits, and finally StdMapTraits.
+  *
+  * For performance and memory consumption reasons it is highly recommended to use one of
+  * the Google's hash_map implementation. To enable the support for them, you have two options:
+  *  - \#include <google/dense_hash_map> yourself \b before Eigen/Sparse header
+  *  - define EIGEN_GOOGLEHASH_SUPPORT
+  * In the later case the inclusion of <google/dense_hash_map> is made for you.
+  * 
+  * \see http://code.google.com/p/google-sparsehash/
+  */
+template<typename SparseMatrixType,
+         template <typename T> class MapTraits =
+#if defined _DENSE_HASH_MAP_H_
+          GoogleDenseHashMapTraits
+#elif defined _HASH_MAP
+          GnuHashMapTraits
+#else
+          StdMapTraits
+#endif
+         ,int OuterPacketBits = 6>
+class RandomSetter
+{
+    typedef typename ei_traits<SparseMatrixType>::Scalar Scalar;
+    struct ScalarWrapper
+    {
+      ScalarWrapper() : value(0) {}
+      Scalar value;
+    };
+    typedef typename MapTraits<ScalarWrapper>::KeyType KeyType;
+    typedef typename MapTraits<ScalarWrapper>::Type HashMapType;
+    static const int OuterPacketMask = (1 << OuterPacketBits) - 1;
+    enum {
+      SwapStorage = 1 - MapTraits<ScalarWrapper>::IsSorted,
+      TargetRowMajor = (SparseMatrixType::Flags & RowMajorBit) ? 1 : 0,
+      SetterRowMajor = SwapStorage ? 1-TargetRowMajor : TargetRowMajor,
+      IsUpperTriangular = SparseMatrixType::Flags & UpperTriangularBit,
+      IsLowerTriangular = SparseMatrixType::Flags & LowerTriangularBit
+    };
+
+  public:
+
+    /** Constructs a random setter object from the sparse matrix \a target
+      *
+      * Note that the initial value of \a target are imported. If you want to re-set
+      * a sparse matrix from scratch, then you must set it to zero first using the
+      * setZero() function.
+      */
+    inline RandomSetter(SparseMatrixType& target)
+      : mp_target(&target)
+    {
+      const int outerSize = SwapStorage ? target.innerSize() : target.outerSize();
+      const int innerSize = SwapStorage ? target.outerSize() : target.innerSize();
+      m_outerPackets = outerSize >> OuterPacketBits;
+      if (outerSize&OuterPacketMask)
+        m_outerPackets += 1;
+      m_hashmaps = new HashMapType[m_outerPackets];
+      // compute number of bits needed to store inner indices
+      int aux = innerSize - 1;
+      m_keyBitsOffset = 0;
+      while (aux)
+      {
+        ++m_keyBitsOffset;
+        aux = aux >> 1;
+      }
+      KeyType ik = (1<<(OuterPacketBits+m_keyBitsOffset));
+      for (int k=0; k<m_outerPackets; ++k)
+        MapTraits<ScalarWrapper>::setInvalidKey(m_hashmaps[k],ik);
+
+      // insert current coeffs
+      for (int j=0; j<mp_target->outerSize(); ++j)
+        for (typename SparseMatrixType::InnerIterator it(*mp_target,j); it; ++it)
+          (*this)(TargetRowMajor?j:it.index(), TargetRowMajor?it.index():j) = it.value();
+    }
+
+    /** Destructor updating back the sparse matrix target */
+    ~RandomSetter()
+    {
+      KeyType keyBitsMask = (1<<m_keyBitsOffset)-1;
+      if (!SwapStorage) // also means the map is sorted
+      {
+        mp_target->startFill(nonZeros());
+        for (int k=0; k<m_outerPackets; ++k)
+        {
+          const int outerOffset = (1<<OuterPacketBits) * k;
+          typename HashMapType::iterator end = m_hashmaps[k].end();
+          for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it)
+          {
+            const int outer = (it->first >> m_keyBitsOffset) + outerOffset;
+            const int inner = it->first & keyBitsMask;
+            mp_target->fill(TargetRowMajor ? outer : inner, TargetRowMajor ? inner : outer) = it->second.value;
+          }
+        }
+        mp_target->endFill();
+      }
+      else
+      {
+        VectorXi positions(mp_target->outerSize());
+        positions.setZero();
+        // pass 1
+        for (int k=0; k<m_outerPackets; ++k)
+        {
+          typename HashMapType::iterator end = m_hashmaps[k].end();
+          for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it)
+          {
+            const int outer = it->first & keyBitsMask;
+            ++positions[outer];
+          }
+        }
+        // prefix sum
+        int count = 0;
+        for (int j=0; j<mp_target->outerSize(); ++j)
+        {
+          int tmp = positions[j];
+          mp_target->_outerIndexPtr()[j] = count;
+          positions[j] = count;
+          count += tmp;
+        }
+        mp_target->_outerIndexPtr()[mp_target->outerSize()] = count;
+        mp_target->resizeNonZeros(count);
+        // pass 2
+        for (int k=0; k<m_outerPackets; ++k)
+        {
+          const int outerOffset = (1<<OuterPacketBits) * k;
+          typename HashMapType::iterator end = m_hashmaps[k].end();
+          for (typename HashMapType::iterator it = m_hashmaps[k].begin(); it!=end; ++it)
+          {
+            const int inner = (it->first >> m_keyBitsOffset) + outerOffset;
+            const int outer = it->first & keyBitsMask;
+            // sorted insertion
+            // Note that we have to deal with at most 2^OuterPacketBits unsorted coefficients,
+            // moreover those 2^OuterPacketBits coeffs are likely to be sparse, an so only a
+            // small fraction of them have to be sorted, whence the following simple procedure:
+            int posStart = mp_target->_outerIndexPtr()[outer];
+            int i = (positions[outer]++) - 1;
+            while ( (i >= posStart) && (mp_target->_innerIndexPtr()[i] > inner) )
+            {
+              mp_target->_valuePtr()[i+1] = mp_target->_valuePtr()[i];
+              mp_target->_innerIndexPtr()[i+1] = mp_target->_innerIndexPtr()[i];
+              --i;
+            }
+            mp_target->_innerIndexPtr()[i+1] = inner;
+            mp_target->_valuePtr()[i+1] = it->second.value;
+          }
+        }
+      }
+      delete[] m_hashmaps;
+    }
+
+    /** \returns a reference to the coefficient at given coordinates \a row, \a col */
+    Scalar& operator() (int row, int col)
+    {
+      ei_assert(((!IsUpperTriangular) || (row<=col)) && "Invalid access to an upper triangular matrix");
+      ei_assert(((!IsLowerTriangular) || (col<=row)) && "Invalid access to an upper triangular matrix");
+      const int outer = SetterRowMajor ? row : col;
+      const int inner = SetterRowMajor ? col : row;
+      const int outerMajor = outer >> OuterPacketBits; // index of the packet/map
+      const int outerMinor = outer & OuterPacketMask;  // index of the inner vector in the packet
+      const KeyType key = (KeyType(outerMinor)<<m_keyBitsOffset) | inner;
+      return m_hashmaps[outerMajor][key].value;
+    }
+
+    /** \returns the number of non zero coefficients 
+      *
+      * \note According to the underlying map/hash_map implementation,
+      * this function might be quite expensive.
+      */
+    int nonZeros() const
+    {
+      int nz = 0;
+      for (int k=0; k<m_outerPackets; ++k)
+        nz += m_hashmaps[k].size();
+      return nz;
+    }
+
+
+  protected:
+
+    HashMapType* m_hashmaps;
+    SparseMatrixType* mp_target;
+    int m_outerPackets;
+    unsigned char m_keyBitsOffset;
+};
+
+#endif // EIGEN_RANDOMSETTER_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseAssign.h b/extern/Eigen2/Eigen/src/Sparse/SparseAssign.h
new file mode 100644
index 00000000000..e69de29bb2d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseAssign.h
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseBlock.h b/extern/Eigen2/Eigen/src/Sparse/SparseBlock.h
new file mode 100644
index 00000000000..c39066676b6
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseBlock.h
@@ -0,0 +1,449 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Daniel Gomez Ferro <dgomezferro@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_BLOCK_H
+#define EIGEN_SPARSE_BLOCK_H
+
+template<typename MatrixType, int Size>
+struct ei_traits<SparseInnerVectorSet<MatrixType, Size> >
+{
+  typedef typename ei_traits<MatrixType>::Scalar Scalar;
+  enum {
+    IsRowMajor = (int(MatrixType::Flags)&RowMajorBit)==RowMajorBit,
+    Flags = MatrixType::Flags,
+    RowsAtCompileTime = IsRowMajor ? Size : MatrixType::RowsAtCompileTime,
+    ColsAtCompileTime = IsRowMajor ? MatrixType::ColsAtCompileTime : Size,
+    CoeffReadCost = MatrixType::CoeffReadCost
+  };
+};
+
+template<typename MatrixType, int Size>
+class SparseInnerVectorSet : ei_no_assignment_operator,
+  public SparseMatrixBase<SparseInnerVectorSet<MatrixType, Size> >
+{
+    enum { IsRowMajor = ei_traits<SparseInnerVectorSet>::IsRowMajor };
+  public:
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseInnerVectorSet)
+    class InnerIterator: public MatrixType::InnerIterator
+    {
+      public:
+        inline InnerIterator(const SparseInnerVectorSet& xpr, int outer)
+          : MatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer)
+        {}
+    };
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outerStart, int outerSize)
+      : m_matrix(matrix), m_outerStart(outerStart), m_outerSize(outerSize)
+    {
+      ei_assert( (outerStart>=0) && ((outerStart+outerSize)<=matrix.outerSize()) );
+    }
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outer)
+      : m_matrix(matrix), m_outerStart(outer), m_outerSize(Size)
+    {
+      ei_assert(Size!=Dynamic);
+      ei_assert( (outer>=0) && (outer<matrix.outerSize()) );
+    }
+
+//     template<typename OtherDerived>
+//     inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+//     {
+//       return *this;
+//     }
+
+//     template<typename Sparse>
+//     inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+//     {
+//       return *this;
+//     }
+
+    EIGEN_STRONG_INLINE int rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+    int m_outerStart;
+    const ei_int_if_dynamic<Size> m_outerSize;
+
+};
+
+/***************************************************************************
+* specialisation for DynamicSparseMatrix
+***************************************************************************/
+
+template<typename _Scalar, int _Options, int Size>
+class SparseInnerVectorSet<DynamicSparseMatrix<_Scalar, _Options>, Size>
+  : public SparseMatrixBase<SparseInnerVectorSet<DynamicSparseMatrix<_Scalar, _Options>, Size> >
+{
+    typedef DynamicSparseMatrix<_Scalar, _Options> MatrixType;
+    enum { IsRowMajor = ei_traits<SparseInnerVectorSet>::IsRowMajor };
+  public:
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseInnerVectorSet)
+    class InnerIterator: public MatrixType::InnerIterator
+    {
+      public:
+        inline InnerIterator(const SparseInnerVectorSet& xpr, int outer)
+          : MatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer)
+        {}
+    };
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outerStart, int outerSize)
+      : m_matrix(matrix), m_outerStart(outerStart), m_outerSize(outerSize)
+    {
+      ei_assert( (outerStart>=0) && ((outerStart+outerSize)<=matrix.outerSize()) );
+    }
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outer)
+      : m_matrix(matrix), m_outerStart(outer), m_outerSize(Size)
+    {
+      ei_assert(Size!=Dynamic);
+      ei_assert( (outer>=0) && (outer<matrix.outerSize()) );
+    }
+
+    template<typename OtherDerived>
+    inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      if (IsRowMajor != ((OtherDerived::Flags&RowMajorBit)==RowMajorBit))
+      {
+        // need to transpose => perform a block evaluation followed by a big swap
+        DynamicSparseMatrix<Scalar,IsRowMajor?RowMajorBit:0> aux(other);
+        *this = aux.markAsRValue();
+      }
+      else
+      {
+        // evaluate/copy vector per vector
+        for (int j=0; j<m_outerSize.value(); ++j)
+        {
+          SparseVector<Scalar,IsRowMajor ? RowMajorBit : 0> aux(other.innerVector(j));
+          m_matrix.const_cast_derived()._data()[m_outerStart+j].swap(aux._data());
+        }
+      }
+      return *this;
+    }
+
+    inline SparseInnerVectorSet& operator=(const SparseInnerVectorSet& other)
+    {
+      return operator=<SparseInnerVectorSet>(other);
+    }
+
+//     template<typename Sparse>
+//     inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+//     {
+//       return *this;
+//     }
+
+    EIGEN_STRONG_INLINE int rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+    int m_outerStart;
+    const ei_int_if_dynamic<Size> m_outerSize;
+
+};
+
+
+/***************************************************************************
+* specialisation for SparseMatrix
+***************************************************************************/
+/*
+template<typename _Scalar, int _Options, int Size>
+class SparseInnerVectorSet<SparseMatrix<_Scalar, _Options>, Size>
+  : public SparseMatrixBase<SparseInnerVectorSet<SparseMatrix<_Scalar, _Options>, Size> >
+{
+    typedef DynamicSparseMatrix<_Scalar, _Options> MatrixType;
+    enum { IsRowMajor = ei_traits<SparseInnerVectorSet>::IsRowMajor };
+  public:
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseInnerVectorSet)
+    class InnerIterator: public MatrixType::InnerIterator
+    {
+      public:
+        inline InnerIterator(const SparseInnerVectorSet& xpr, int outer)
+          : MatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer)
+        {}
+    };
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outerStart, int outerSize)
+      : m_matrix(matrix), m_outerStart(outerStart), m_outerSize(outerSize)
+    {
+      ei_assert( (outerStart>=0) && ((outerStart+outerSize)<=matrix.outerSize()) );
+    }
+
+    inline SparseInnerVectorSet(const MatrixType& matrix, int outer)
+      : m_matrix(matrix), m_outerStart(outer)
+    {
+      ei_assert(Size==1);
+      ei_assert( (outer>=0) && (outer<matrix.outerSize()) );
+    }
+
+    template<typename OtherDerived>
+    inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      if (IsRowMajor != ((OtherDerived::Flags&RowMajorBit)==RowMajorBit))
+      {
+        // need to transpose => perform a block evaluation followed by a big swap
+        DynamicSparseMatrix<Scalar,IsRowMajor?RowMajorBit:0> aux(other);
+        *this = aux.markAsRValue();
+      }
+      else
+      {
+        // evaluate/copy vector per vector
+        for (int j=0; j<m_outerSize.value(); ++j)
+        {
+          SparseVector<Scalar,IsRowMajor ? RowMajorBit : 0> aux(other.innerVector(j));
+          m_matrix.const_cast_derived()._data()[m_outerStart+j].swap(aux._data());
+        }
+      }
+      return *this;
+    }
+
+    inline SparseInnerVectorSet& operator=(const SparseInnerVectorSet& other)
+    {
+      return operator=<SparseInnerVectorSet>(other);
+    }
+
+    inline const Scalar* _valuePtr() const
+    { return m_matrix._valuePtr() + m_matrix._outerIndexPtr()[m_outerStart]; }
+    inline const int* _innerIndexPtr() const
+    { return m_matrix._innerIndexPtr() + m_matrix._outerIndexPtr()[m_outerStart]; }
+    inline const int* _outerIndexPtr() const { return m_matrix._outerIndexPtr() + m_outerStart; }
+
+//     template<typename Sparse>
+//     inline SparseInnerVectorSet& operator=(const SparseMatrixBase<OtherDerived>& other)
+//     {
+//       return *this;
+//     }
+
+    EIGEN_STRONG_INLINE int rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+    int m_outerStart;
+    const ei_int_if_dynamic<Size> m_outerSize;
+
+};
+*/
+//----------
+
+/** \returns the i-th row of the matrix \c *this. For row-major matrix only. */
+template<typename Derived>
+SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::row(int i)
+{
+  EIGEN_STATIC_ASSERT(IsRowMajor,THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES);
+  return innerVector(i);
+}
+
+/** \returns the i-th row of the matrix \c *this. For row-major matrix only.
+  * (read-only version) */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::row(int i) const
+{
+  EIGEN_STATIC_ASSERT(IsRowMajor,THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES);
+  return innerVector(i);
+}
+
+/** \returns the i-th column of the matrix \c *this. For column-major matrix only. */
+template<typename Derived>
+SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::col(int i)
+{
+  EIGEN_STATIC_ASSERT(!IsRowMajor,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  return innerVector(i);
+}
+
+/** \returns the i-th column of the matrix \c *this. For column-major matrix only.
+  * (read-only version) */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::col(int i) const
+{
+  EIGEN_STATIC_ASSERT(!IsRowMajor,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  return innerVector(i);
+}
+
+/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+  * is col-major (resp. row-major).
+  */
+template<typename Derived>
+SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::innerVector(int outer)
+{ return SparseInnerVectorSet<Derived,1>(derived(), outer); }
+
+/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+  * is col-major (resp. row-major). Read-only.
+  */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,1> SparseMatrixBase<Derived>::innerVector(int outer) const
+{ return SparseInnerVectorSet<Derived,1>(derived(), outer); }
+
+//----------
+
+/** \returns the i-th row of the matrix \c *this. For row-major matrix only. */
+template<typename Derived>
+SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::subrows(int start, int size)
+{
+  EIGEN_STATIC_ASSERT(IsRowMajor,THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES);
+  return innerVectors(start, size);
+}
+
+/** \returns the i-th row of the matrix \c *this. For row-major matrix only.
+  * (read-only version) */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::subrows(int start, int size) const
+{
+  EIGEN_STATIC_ASSERT(IsRowMajor,THIS_METHOD_IS_ONLY_FOR_ROW_MAJOR_MATRICES);
+  return innerVectors(start, size);
+}
+
+/** \returns the i-th column of the matrix \c *this. For column-major matrix only. */
+template<typename Derived>
+SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::subcols(int start, int size)
+{
+  EIGEN_STATIC_ASSERT(!IsRowMajor,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  return innerVectors(start, size);
+}
+
+/** \returns the i-th column of the matrix \c *this. For column-major matrix only.
+  * (read-only version) */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::subcols(int start, int size) const
+{
+  EIGEN_STATIC_ASSERT(!IsRowMajor,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+  return innerVectors(start, size);
+}
+
+/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+  * is col-major (resp. row-major).
+  */
+template<typename Derived>
+SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::innerVectors(int outerStart, int outerSize)
+{ return SparseInnerVectorSet<Derived,Dynamic>(derived(), outerStart, outerSize); }
+
+/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+  * is col-major (resp. row-major). Read-only.
+  */
+template<typename Derived>
+const SparseInnerVectorSet<Derived,Dynamic> SparseMatrixBase<Derived>::innerVectors(int outerStart, int outerSize) const
+{ return SparseInnerVectorSet<Derived,Dynamic>(derived(), outerStart, outerSize); }
+
+# if 0
+template<typename MatrixType, int BlockRows, int BlockCols, int PacketAccess>
+class Block<MatrixType,BlockRows,BlockCols,PacketAccess,IsSparse>
+  : public SparseMatrixBase<Block<MatrixType,BlockRows,BlockCols,PacketAccess,IsSparse> >
+{
+public:
+
+    _EIGEN_GENERIC_PUBLIC_INTERFACE(Block, SparseMatrixBase<Block>)
+    class InnerIterator;
+
+    /** Column or Row constructor
+      */
+    inline Block(const MatrixType& matrix, int i)
+      : m_matrix(matrix),
+        // It is a row if and only if BlockRows==1 and BlockCols==MatrixType::ColsAtCompileTime,
+        // and it is a column if and only if BlockRows==MatrixType::RowsAtCompileTime and BlockCols==1,
+        // all other cases are invalid.
+        // The case a 1x1 matrix seems ambiguous, but the result is the same anyway.
+        m_startRow( (BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) ? i : 0),
+        m_startCol( (BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
+        m_blockRows(matrix.rows()), // if it is a row, then m_blockRows has a fixed-size of 1, so no pb to try to overwrite it
+        m_blockCols(matrix.cols())  // same for m_blockCols
+    {
+      ei_assert( (i>=0) && (
+          ((BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) && i<matrix.rows())
+        ||((BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) && i<matrix.cols())));
+    }
+
+    /** Fixed-size constructor
+      */
+    inline Block(const MatrixType& matrix, int startRow, int startCol)
+      : m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
+        m_blockRows(matrix.rows()), m_blockCols(matrix.cols())
+    {
+      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && RowsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+      ei_assert(startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= matrix.rows()
+          && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= matrix.cols());
+    }
+
+    /** Dynamic-size constructor
+      */
+    inline Block(const MatrixType& matrix,
+          int startRow, int startCol,
+          int blockRows, int blockCols)
+      : m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
+                          m_blockRows(blockRows), m_blockCols(blockCols)
+    {
+      ei_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
+          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
+      ei_assert(startRow >= 0 && blockRows >= 1 && startRow + blockRows <= matrix.rows()
+          && startCol >= 0 && blockCols >= 1 && startCol + blockCols <= matrix.cols());
+    }
+
+    inline int rows() const { return m_blockRows.value(); }
+    inline int cols() const { return m_blockCols.value(); }
+
+    inline int stride(void) const { return m_matrix.stride(); }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived()
+               .coeffRef(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
+    }
+
+    inline Scalar& coeffRef(int index)
+    {
+      return m_matrix.const_cast_derived()
+             .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                       m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix
+             .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
+                    m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
+    }
+
+  protected:
+
+    const typename MatrixType::Nested m_matrix;
+    const ei_int_if_dynamic<MatrixType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
+    const ei_int_if_dynamic<MatrixType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
+    const ei_int_if_dynamic<RowsAtCompileTime> m_blockRows;
+    const ei_int_if_dynamic<ColsAtCompileTime> m_blockCols;
+
+};
+#endif
+
+#endif // EIGEN_SPARSE_BLOCK_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseCwise.h b/extern/Eigen2/Eigen/src/Sparse/SparseCwise.h
new file mode 100644
index 00000000000..2206883cc76
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseCwise.h
@@ -0,0 +1,175 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_CWISE_H
+#define EIGEN_SPARSE_CWISE_H
+
+/** \internal
+  * convenient macro to defined the return type of a cwise binary operation */
+#define EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(OP) \
+    CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, OtherDerived>
+
+#define EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE \
+    SparseCwiseBinaryOp< \
+      ei_scalar_product_op< \
+        typename ei_scalar_product_traits< \
+          typename ei_traits<ExpressionType>::Scalar, \
+          typename ei_traits<OtherDerived>::Scalar \
+        >::ReturnType \
+      >, \
+      ExpressionType, \
+      OtherDerived \
+    >
+
+/** \internal
+  * convenient macro to defined the return type of a cwise unary operation */
+#define EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(OP) \
+    SparseCwiseUnaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType>
+
+/** \internal
+  * convenient macro to defined the return type of a cwise comparison to a scalar */
+/*#define EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(OP) \
+    CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, \
+        NestByValue<typename ExpressionType::ConstantReturnType> >*/
+
+template<typename ExpressionType> class SparseCwise
+{
+  public:
+
+    typedef typename ei_traits<ExpressionType>::Scalar Scalar;
+    typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
+        ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
+    typedef CwiseUnaryOp<ei_scalar_add_op<Scalar>, ExpressionType> ScalarAddReturnType;
+
+    inline SparseCwise(const ExpressionType& matrix) : m_matrix(matrix) {}
+
+    /** \internal */
+    inline const ExpressionType& _expression() const { return m_matrix; }
+
+    template<typename OtherDerived>
+    const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE
+    operator*(const SparseMatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE
+    operator*(const MatrixBase<OtherDerived> &other) const;
+
+//     template<typename OtherDerived>
+//     const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+//     operator/(const SparseMatrixBase<OtherDerived> &other) const;
+//
+//     template<typename OtherDerived>
+//     const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+//     operator/(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
+    min(const SparseMatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
+    max(const SparseMatrixBase<OtherDerived> &other) const;
+
+    const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op)      abs() const;
+    const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op)     abs2() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_square_op)   square() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_cube_op)     cube() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_inverse_op)  inverse() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_sqrt_op)     sqrt() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_exp_op)      exp() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_log_op)      log() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_cos_op)      cos() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_sin_op)      sin() const;
+//     const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)      pow(const Scalar& exponent) const;
+
+    template<typename OtherDerived>
+    inline ExpressionType& operator*=(const SparseMatrixBase<OtherDerived> &other);
+
+//     template<typename OtherDerived>
+//     inline ExpressionType& operator/=(const SparseMatrixBase<OtherDerived> &other);
+
+    /*
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)
+    operator<(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)
+    operator<=(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)
+    operator>(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)
+    operator>=(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)
+    operator==(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)
+    operator!=(const MatrixBase<OtherDerived>& other) const;
+
+    // comparisons to a scalar value
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)
+    operator<(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)
+    operator<=(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)
+    operator>(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)
+    operator>=(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)
+    operator==(Scalar s) const;
+
+    const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)
+    operator!=(Scalar s) const;
+    */
+
+    // allow to extend SparseCwise outside Eigen
+    #ifdef EIGEN_SPARSE_CWISE_PLUGIN
+    #include EIGEN_SPARSE_CWISE_PLUGIN
+    #endif
+
+  protected:
+    ExpressionTypeNested m_matrix;
+};
+
+template<typename Derived>
+inline const SparseCwise<Derived>
+SparseMatrixBase<Derived>::cwise() const
+{
+  return derived();
+}
+
+template<typename Derived>
+inline SparseCwise<Derived>
+SparseMatrixBase<Derived>::cwise()
+{
+  return derived();
+}
+
+#endif // EIGEN_SPARSE_CWISE_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseCwiseBinaryOp.h b/extern/Eigen2/Eigen/src/Sparse/SparseCwiseBinaryOp.h
new file mode 100644
index 00000000000..d19970efcb1
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseCwiseBinaryOp.h
@@ -0,0 +1,442 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_CWISE_BINARY_OP_H
+#define EIGEN_SPARSE_CWISE_BINARY_OP_H
+
+// Here we have to handle 3 cases:
+//  1 - sparse op dense
+//  2 - dense op sparse
+//  3 - sparse op sparse
+// We also need to implement a 4th iterator for:
+//  4 - dense op dense
+// Finally, we also need to distinguish between the product and other operations :
+//                configuration      returned mode
+//  1 - sparse op dense    product      sparse
+//                         generic      dense
+//  2 - dense op sparse    product      sparse
+//                         generic      dense
+//  3 - sparse op sparse   product      sparse
+//                         generic      sparse
+//  4 - dense op dense     product      dense
+//                         generic      dense
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+struct ei_traits<SparseCwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  typedef typename ei_result_of<
+                     BinaryOp(
+                       typename Lhs::Scalar,
+                       typename Rhs::Scalar
+                     )
+                   >::type Scalar;
+  typedef typename Lhs::Nested LhsNested;
+  typedef typename Rhs::Nested RhsNested;
+  typedef typename ei_unref<LhsNested>::type _LhsNested;
+  typedef typename ei_unref<RhsNested>::type _RhsNested;
+  enum {
+    LhsCoeffReadCost = _LhsNested::CoeffReadCost,
+    RhsCoeffReadCost = _RhsNested::CoeffReadCost,
+    LhsFlags = _LhsNested::Flags,
+    RhsFlags = _RhsNested::Flags,
+    RowsAtCompileTime = Lhs::RowsAtCompileTime,
+    ColsAtCompileTime = Lhs::ColsAtCompileTime,
+    MaxRowsAtCompileTime = Lhs::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = Lhs::MaxColsAtCompileTime,
+    Flags = (int(LhsFlags) | int(RhsFlags)) & HereditaryBits,
+    CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + ei_functor_traits<BinaryOp>::Cost
+  };
+};
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+class SparseCwiseBinaryOp : ei_no_assignment_operator,
+  public SparseMatrixBase<SparseCwiseBinaryOp<BinaryOp, Lhs, Rhs> >
+{
+  public:
+
+    class InnerIterator;
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseCwiseBinaryOp)
+    typedef typename ei_traits<SparseCwiseBinaryOp>::LhsNested LhsNested;
+    typedef typename ei_traits<SparseCwiseBinaryOp>::RhsNested RhsNested;
+    typedef typename ei_unref<LhsNested>::type _LhsNested;
+    typedef typename ei_unref<RhsNested>::type _RhsNested;
+
+    EIGEN_STRONG_INLINE SparseCwiseBinaryOp(const Lhs& lhs, const Rhs& rhs, const BinaryOp& func = BinaryOp())
+      : m_lhs(lhs), m_rhs(rhs), m_functor(func)
+    {
+      EIGEN_STATIC_ASSERT((_LhsNested::Flags&RowMajorBit)==(_RhsNested::Flags&RowMajorBit),
+        BOTH_MATRICES_MUST_HAVE_THE_SAME_STORAGE_ORDER)
+      EIGEN_STATIC_ASSERT((ei_functor_allows_mixing_real_and_complex<BinaryOp>::ret
+                           ? int(ei_is_same_type<typename Lhs::RealScalar, typename Rhs::RealScalar>::ret)
+                           : int(ei_is_same_type<typename Lhs::Scalar, typename Rhs::Scalar>::ret)),
+        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+      // require the sizes to match
+      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
+      ei_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_lhs.cols(); }
+
+    EIGEN_STRONG_INLINE const _LhsNested& lhs() const { return m_lhs; }
+    EIGEN_STRONG_INLINE const _RhsNested& rhs() const { return m_rhs; }
+    EIGEN_STRONG_INLINE const BinaryOp& functor() const { return m_functor; }
+
+  protected:
+    const LhsNested m_lhs;
+    const RhsNested m_rhs;
+    const BinaryOp m_functor;
+};
+
+template<typename BinaryOp, typename Lhs, typename Rhs, typename Derived,
+  int _LhsStorageMode = int(Lhs::Flags) & SparseBit,
+  int _RhsStorageMode = int(Rhs::Flags) & SparseBit>
+class ei_sparse_cwise_binary_op_inner_iterator_selector;
+
+template<typename BinaryOp, typename Lhs, typename Rhs>
+class SparseCwiseBinaryOp<BinaryOp,Lhs,Rhs>::InnerIterator
+  : public ei_sparse_cwise_binary_op_inner_iterator_selector<BinaryOp,Lhs,Rhs, typename SparseCwiseBinaryOp<BinaryOp,Lhs,Rhs>::InnerIterator>
+{
+  public:
+    typedef ei_sparse_cwise_binary_op_inner_iterator_selector<
+      BinaryOp,Lhs,Rhs, InnerIterator> Base;
+
+    EIGEN_STRONG_INLINE InnerIterator(const SparseCwiseBinaryOp& binOp, int outer)
+      : Base(binOp,outer)
+    {}
+};
+
+/***************************************************************************
+* Implementation of inner-iterators
+***************************************************************************/
+
+// template<typename T> struct ei_func_is_conjunction { enum { ret = false }; };
+// template<typename T> struct ei_func_is_conjunction<ei_scalar_product_op<T> > { enum { ret = true }; };
+
+// TODO generalize the ei_scalar_product_op specialization to all conjunctions if any !
+
+// sparse - sparse  (generic)
+template<typename BinaryOp, typename Lhs, typename Rhs, typename Derived>
+class ei_sparse_cwise_binary_op_inner_iterator_selector<BinaryOp, Lhs, Rhs, Derived, IsSparse, IsSparse>
+{
+    typedef SparseCwiseBinaryOp<BinaryOp, Lhs, Rhs> CwiseBinaryXpr;
+    typedef typename ei_traits<CwiseBinaryXpr>::Scalar Scalar;
+    typedef typename ei_traits<CwiseBinaryXpr>::_LhsNested _LhsNested;
+    typedef typename ei_traits<CwiseBinaryXpr>::_RhsNested _RhsNested;
+    typedef typename _LhsNested::InnerIterator LhsIterator;
+    typedef typename _RhsNested::InnerIterator RhsIterator;
+  public:
+
+    EIGEN_STRONG_INLINE ei_sparse_cwise_binary_op_inner_iterator_selector(const CwiseBinaryXpr& xpr, int outer)
+      : m_lhsIter(xpr.lhs(),outer), m_rhsIter(xpr.rhs(),outer), m_functor(xpr.functor())
+    {
+      this->operator++();
+    }
+
+    EIGEN_STRONG_INLINE Derived& operator++()
+    {
+      if (m_lhsIter && m_rhsIter && (m_lhsIter.index() == m_rhsIter.index()))
+      {
+        m_id = m_lhsIter.index();
+        m_value = m_functor(m_lhsIter.value(), m_rhsIter.value());
+        ++m_lhsIter;
+        ++m_rhsIter;
+      }
+      else if (m_lhsIter && (!m_rhsIter || (m_lhsIter.index() < m_rhsIter.index())))
+      {
+        m_id = m_lhsIter.index();
+        m_value = m_functor(m_lhsIter.value(), Scalar(0));
+        ++m_lhsIter;
+      }
+      else if (m_rhsIter && (!m_lhsIter || (m_lhsIter.index() > m_rhsIter.index())))
+      {
+        m_id = m_rhsIter.index();
+        m_value = m_functor(Scalar(0), m_rhsIter.value());
+        ++m_rhsIter;
+      }
+      else
+      {
+        m_id = -1;
+      }
+      return *static_cast<Derived*>(this);
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_value; }
+
+    EIGEN_STRONG_INLINE int index() const { return m_id; }
+    EIGEN_STRONG_INLINE int row() const { return m_lhsIter.row(); }
+    EIGEN_STRONG_INLINE int col() const { return m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_id>=0; }
+
+  protected:
+    LhsIterator m_lhsIter;
+    RhsIterator m_rhsIter;
+    const BinaryOp& m_functor;
+    Scalar m_value;
+    int m_id;
+};
+
+// sparse - sparse  (product)
+template<typename T, typename Lhs, typename Rhs, typename Derived>
+class ei_sparse_cwise_binary_op_inner_iterator_selector<ei_scalar_product_op<T>, Lhs, Rhs, Derived, IsSparse, IsSparse>
+{
+    typedef ei_scalar_product_op<T> BinaryFunc;
+    typedef SparseCwiseBinaryOp<BinaryFunc, Lhs, Rhs> CwiseBinaryXpr;
+    typedef typename CwiseBinaryXpr::Scalar Scalar;
+    typedef typename ei_traits<CwiseBinaryXpr>::_LhsNested _LhsNested;
+    typedef typename _LhsNested::InnerIterator LhsIterator;
+    typedef typename ei_traits<CwiseBinaryXpr>::_RhsNested _RhsNested;
+    typedef typename _RhsNested::InnerIterator RhsIterator;
+  public:
+
+    EIGEN_STRONG_INLINE ei_sparse_cwise_binary_op_inner_iterator_selector(const CwiseBinaryXpr& xpr, int outer)
+      : m_lhsIter(xpr.lhs(),outer), m_rhsIter(xpr.rhs(),outer), m_functor(xpr.functor())
+    {
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+      {
+        if (m_lhsIter.index() < m_rhsIter.index())
+          ++m_lhsIter;
+        else
+          ++m_rhsIter;
+      }
+    }
+
+    EIGEN_STRONG_INLINE Derived& operator++()
+    {
+      ++m_lhsIter;
+      ++m_rhsIter;
+      while (m_lhsIter && m_rhsIter && (m_lhsIter.index() != m_rhsIter.index()))
+      {
+        if (m_lhsIter.index() < m_rhsIter.index())
+          ++m_lhsIter;
+        else
+          ++m_rhsIter;
+      }
+      return *static_cast<Derived*>(this);
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_lhsIter.value(), m_rhsIter.value()); }
+
+    EIGEN_STRONG_INLINE int index() const { return m_lhsIter.index(); }
+    EIGEN_STRONG_INLINE int row() const { return m_lhsIter.row(); }
+    EIGEN_STRONG_INLINE int col() const { return m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return (m_lhsIter && m_rhsIter); }
+
+  protected:
+    LhsIterator m_lhsIter;
+    RhsIterator m_rhsIter;
+    const BinaryFunc& m_functor;
+};
+
+// sparse - dense  (product)
+template<typename T, typename Lhs, typename Rhs, typename Derived>
+class ei_sparse_cwise_binary_op_inner_iterator_selector<ei_scalar_product_op<T>, Lhs, Rhs, Derived, IsSparse, IsDense>
+{
+    typedef ei_scalar_product_op<T> BinaryFunc;
+    typedef SparseCwiseBinaryOp<BinaryFunc, Lhs, Rhs> CwiseBinaryXpr;
+    typedef typename CwiseBinaryXpr::Scalar Scalar;
+    typedef typename ei_traits<CwiseBinaryXpr>::_LhsNested _LhsNested;
+    typedef typename ei_traits<CwiseBinaryXpr>::RhsNested RhsNested;
+    typedef typename _LhsNested::InnerIterator LhsIterator;
+    enum { IsRowMajor = (int(Lhs::Flags)&RowMajorBit)==RowMajorBit };
+  public:
+
+    EIGEN_STRONG_INLINE ei_sparse_cwise_binary_op_inner_iterator_selector(const CwiseBinaryXpr& xpr, int outer)
+      : m_rhs(xpr.rhs()), m_lhsIter(xpr.lhs(),outer), m_functor(xpr.functor()), m_outer(outer)
+    {}
+
+    EIGEN_STRONG_INLINE Derived& operator++()
+    {
+      ++m_lhsIter;
+      return *static_cast<Derived*>(this);
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const
+    { return m_functor(m_lhsIter.value(),
+                       m_rhs.coeff(IsRowMajor?m_outer:m_lhsIter.index(),IsRowMajor?m_lhsIter.index():m_outer)); }
+
+    EIGEN_STRONG_INLINE int index() const { return m_lhsIter.index(); }
+    EIGEN_STRONG_INLINE int row() const { return m_lhsIter.row(); }
+    EIGEN_STRONG_INLINE int col() const { return m_lhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_lhsIter; }
+
+  protected:
+    const RhsNested m_rhs;
+    LhsIterator m_lhsIter;
+    const BinaryFunc m_functor;
+    const int m_outer;
+};
+
+// sparse - dense  (product)
+template<typename T, typename Lhs, typename Rhs, typename Derived>
+class ei_sparse_cwise_binary_op_inner_iterator_selector<ei_scalar_product_op<T>, Lhs, Rhs, Derived, IsDense, IsSparse>
+{
+    typedef ei_scalar_product_op<T> BinaryFunc;
+    typedef SparseCwiseBinaryOp<BinaryFunc, Lhs, Rhs> CwiseBinaryXpr;
+    typedef typename CwiseBinaryXpr::Scalar Scalar;
+    typedef typename ei_traits<CwiseBinaryXpr>::_RhsNested _RhsNested;
+    typedef typename _RhsNested::InnerIterator RhsIterator;
+    enum { IsRowMajor = (int(Rhs::Flags)&RowMajorBit)==RowMajorBit };
+  public:
+
+    EIGEN_STRONG_INLINE ei_sparse_cwise_binary_op_inner_iterator_selector(const CwiseBinaryXpr& xpr, int outer)
+      : m_xpr(xpr), m_rhsIter(xpr.rhs(),outer), m_functor(xpr.functor()), m_outer(outer)
+    {}
+
+    EIGEN_STRONG_INLINE Derived& operator++()
+    {
+      ++m_rhsIter;
+      return *static_cast<Derived*>(this);
+    }
+
+    EIGEN_STRONG_INLINE Scalar value() const
+    { return m_functor(m_xpr.lhs().coeff(IsRowMajor?m_outer:m_rhsIter.index(),IsRowMajor?m_rhsIter.index():m_outer), m_rhsIter.value()); }
+
+    EIGEN_STRONG_INLINE int index() const { return m_rhsIter.index(); }
+    EIGEN_STRONG_INLINE int row() const { return m_rhsIter.row(); }
+    EIGEN_STRONG_INLINE int col() const { return m_rhsIter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_rhsIter; }
+
+  protected:
+    const CwiseBinaryXpr& m_xpr;
+    RhsIterator m_rhsIter;
+    const BinaryFunc& m_functor;
+    const int m_outer;
+};
+
+
+/***************************************************************************
+* Implementation of SparseMatrixBase and SparseCwise functions/operators
+***************************************************************************/
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const SparseCwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>,
+                                 Derived, OtherDerived>
+SparseMatrixBase<Derived>::operator-(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return SparseCwiseBinaryOp<ei_scalar_difference_op<Scalar>,
+                       Derived, OtherDerived>(derived(), other.derived());
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator-=(const SparseMatrixBase<OtherDerived> &other)
+{
+  return *this = derived() - other.derived();
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const SparseCwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+SparseMatrixBase<Derived>::operator+(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return SparseCwiseBinaryOp<ei_scalar_sum_op<Scalar>, Derived, OtherDerived>(derived(), other.derived());
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE Derived &
+SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& other)
+{
+  return *this = derived() + other.derived();
+}
+
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE
+SparseCwise<ExpressionType>::operator*(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE(_expression(), other.derived());
+}
+
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE
+SparseCwise<ExpressionType>::operator*(const MatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE(_expression(), other.derived());
+}
+
+// template<typename ExpressionType>
+// template<typename OtherDerived>
+// EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+// SparseCwise<ExpressionType>::operator/(const SparseMatrixBase<OtherDerived> &other) const
+// {
+//   return EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)(_expression(), other.derived());
+// }
+//
+// template<typename ExpressionType>
+// template<typename OtherDerived>
+// EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
+// SparseCwise<ExpressionType>::operator/(const MatrixBase<OtherDerived> &other) const
+// {
+//   return EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)(_expression(), other.derived());
+// }
+
+template<typename ExpressionType>
+template<typename OtherDerived>
+inline ExpressionType& SparseCwise<ExpressionType>::operator*=(const SparseMatrixBase<OtherDerived> &other)
+{
+  return m_matrix.const_cast_derived() = _expression() * other.derived();
+}
+
+// template<typename ExpressionType>
+// template<typename OtherDerived>
+// inline ExpressionType& SparseCwise<ExpressionType>::operator/=(const SparseMatrixBase<OtherDerived> &other)
+// {
+//   return m_matrix.const_cast_derived() = *this / other;
+// }
+
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
+SparseCwise<ExpressionType>::min(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)(_expression(), other.derived());
+}
+
+template<typename ExpressionType>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
+SparseCwise<ExpressionType>::max(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return EIGEN_SPARSE_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)(_expression(), other.derived());
+}
+
+// template<typename Derived>
+// template<typename CustomBinaryOp, typename OtherDerived>
+// EIGEN_STRONG_INLINE const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
+// SparseMatrixBase<Derived>::binaryExpr(const SparseMatrixBase<OtherDerived> &other, const CustomBinaryOp& func) const
+// {
+//   return CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>(derived(), other.derived(), func);
+// }
+
+#endif // EIGEN_SPARSE_CWISE_BINARY_OP_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseCwiseUnaryOp.h b/extern/Eigen2/Eigen/src/Sparse/SparseCwiseUnaryOp.h
new file mode 100644
index 00000000000..b11c0f8a377
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseCwiseUnaryOp.h
@@ -0,0 +1,183 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_CWISE_UNARY_OP_H
+#define EIGEN_SPARSE_CWISE_UNARY_OP_H
+
+template<typename UnaryOp, typename MatrixType>
+struct ei_traits<SparseCwiseUnaryOp<UnaryOp, MatrixType> > : ei_traits<MatrixType>
+{
+  typedef typename ei_result_of<
+                     UnaryOp(typename MatrixType::Scalar)
+                   >::type Scalar;
+  typedef typename MatrixType::Nested MatrixTypeNested;
+  typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
+  enum {
+    CoeffReadCost = _MatrixTypeNested::CoeffReadCost + ei_functor_traits<UnaryOp>::Cost
+  };
+};
+
+template<typename UnaryOp, typename MatrixType>
+class SparseCwiseUnaryOp : ei_no_assignment_operator,
+  public SparseMatrixBase<SparseCwiseUnaryOp<UnaryOp, MatrixType> >
+{
+  public:
+
+    class InnerIterator;
+//     typedef typename ei_unref<LhsNested>::type _LhsNested;
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseCwiseUnaryOp)
+
+    inline SparseCwiseUnaryOp(const MatrixType& mat, const UnaryOp& func = UnaryOp())
+      : m_matrix(mat), m_functor(func) {}
+
+    EIGEN_STRONG_INLINE int rows() const { return m_matrix.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_matrix.cols(); }
+
+//     EIGEN_STRONG_INLINE const typename MatrixType::Nested& _matrix() const { return m_matrix; }
+//     EIGEN_STRONG_INLINE const UnaryOp& _functor() const { return m_functor; }
+
+  protected:
+    const typename MatrixType::Nested m_matrix;
+    const UnaryOp m_functor;
+};
+
+
+template<typename UnaryOp, typename MatrixType>
+class SparseCwiseUnaryOp<UnaryOp,MatrixType>::InnerIterator
+{
+    typedef typename SparseCwiseUnaryOp::Scalar Scalar;
+    typedef typename ei_traits<SparseCwiseUnaryOp>::_MatrixTypeNested _MatrixTypeNested;
+    typedef typename _MatrixTypeNested::InnerIterator MatrixTypeIterator;
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const SparseCwiseUnaryOp& unaryOp, int outer)
+      : m_iter(unaryOp.m_matrix,outer), m_functor(unaryOp.m_functor)
+    {}
+
+    EIGEN_STRONG_INLINE InnerIterator& operator++()
+    { ++m_iter; return *this; }
+
+    EIGEN_STRONG_INLINE Scalar value() const { return m_functor(m_iter.value()); }
+
+    EIGEN_STRONG_INLINE int index() const { return m_iter.index(); }
+    EIGEN_STRONG_INLINE int row() const { return m_iter.row(); }
+    EIGEN_STRONG_INLINE int col() const { return m_iter.col(); }
+
+    EIGEN_STRONG_INLINE operator bool() const { return m_iter; }
+
+  protected:
+    MatrixTypeIterator m_iter;
+    const UnaryOp m_functor;
+};
+
+template<typename Derived>
+template<typename CustomUnaryOp>
+EIGEN_STRONG_INLINE const SparseCwiseUnaryOp<CustomUnaryOp, Derived>
+SparseMatrixBase<Derived>::unaryExpr(const CustomUnaryOp& func) const
+{
+  return SparseCwiseUnaryOp<CustomUnaryOp, Derived>(derived(), func);
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE const SparseCwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived>
+SparseMatrixBase<Derived>::operator-() const
+{
+  return derived();
+}
+
+template<typename ExpressionType>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op)
+SparseCwise<ExpressionType>::abs() const
+{
+  return _expression();
+}
+
+template<typename ExpressionType>
+EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op)
+SparseCwise<ExpressionType>::abs2() const
+{
+  return _expression();
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE typename SparseMatrixBase<Derived>::ConjugateReturnType
+SparseMatrixBase<Derived>::conjugate() const
+{
+  return ConjugateReturnType(derived());
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::RealReturnType
+SparseMatrixBase<Derived>::real() const { return derived(); }
+
+template<typename Derived>
+EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::ImagReturnType
+SparseMatrixBase<Derived>::imag() const { return derived(); }
+
+template<typename Derived>
+template<typename NewType>
+EIGEN_STRONG_INLINE const SparseCwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived>
+SparseMatrixBase<Derived>::cast() const
+{
+  return derived();
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE const SparseCwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<Derived>::Scalar>, Derived>
+SparseMatrixBase<Derived>::operator*(const Scalar& scalar) const
+{
+  return SparseCwiseUnaryOp<ei_scalar_multiple_op<Scalar>, Derived>
+    (derived(), ei_scalar_multiple_op<Scalar>(scalar));
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE const SparseCwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
+SparseMatrixBase<Derived>::operator/(const Scalar& scalar) const
+{
+  return SparseCwiseUnaryOp<ei_scalar_quotient1_op<Scalar>, Derived>
+    (derived(), ei_scalar_quotient1_op<Scalar>(scalar));
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator*=(const Scalar& other)
+{
+  for (int j=0; j<outerSize(); ++j)
+    for (typename Derived::InnerIterator i(derived(),j); i; ++i)
+      i.valueRef() *= other;
+  return derived();
+}
+
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+SparseMatrixBase<Derived>::operator/=(const Scalar& other)
+{
+  for (int j=0; j<outerSize(); ++j)
+    for (typename Derived::InnerIterator i(derived(),j); i; ++i)
+      i.valueRef() /= other;
+  return derived();
+}
+
+#endif // EIGEN_SPARSE_CWISE_UNARY_OP_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseDiagonalProduct.h b/extern/Eigen2/Eigen/src/Sparse/SparseDiagonalProduct.h
new file mode 100644
index 00000000000..932daf220b9
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseDiagonalProduct.h
@@ -0,0 +1,157 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_DIAGONAL_PRODUCT_H
+#define EIGEN_SPARSE_DIAGONAL_PRODUCT_H
+
+// the product a diagonal matrix with a sparse matrix can be easily
+// implemented using expression template. We have two very different cases:
+// 1 - diag * row-major sparse
+//     => each inner vector <=> scalar * sparse vector product
+//     => so we can reuse CwiseUnaryOp::InnerIterator
+// 2 - diag * col-major sparse
+//     => each inner vector <=> densevector * sparse vector cwise product
+//     => again, we can reuse specialization of CwiseBinaryOp::InnerIterator
+//        for that particular case
+// The two other cases are symmetric.
+
+template<typename Lhs, typename Rhs>
+struct ei_traits<SparseDiagonalProduct<Lhs, Rhs> > : ei_traits<SparseProduct<Lhs, Rhs, DiagonalProduct> >
+{
+  typedef typename ei_cleantype<Lhs>::type _Lhs;
+  typedef typename ei_cleantype<Rhs>::type _Rhs;
+  enum {
+    SparseFlags = ((int(_Lhs::Flags)&Diagonal)==Diagonal) ? int(_Rhs::Flags) : int(_Lhs::Flags),
+    Flags = SparseBit | (SparseFlags&RowMajorBit)
+  };
+};
+
+enum {SDP_IsDiagonal, SDP_IsSparseRowMajor, SDP_IsSparseColMajor};
+template<typename Lhs, typename Rhs, typename SparseDiagonalProductType, int RhsMode, int LhsMode> 
+class ei_sparse_diagonal_product_inner_iterator_selector;
+
+template<typename LhsNested, typename RhsNested>
+class SparseDiagonalProduct : public SparseMatrixBase<SparseDiagonalProduct<LhsNested,RhsNested> >, ei_no_assignment_operator
+{
+    typedef typename ei_traits<SparseDiagonalProduct>::_LhsNested _LhsNested;
+    typedef typename ei_traits<SparseDiagonalProduct>::_RhsNested _RhsNested;
+    
+    enum {
+      LhsMode = (_LhsNested::Flags&Diagonal)==Diagonal ? SDP_IsDiagonal
+              : (_LhsNested::Flags&RowMajorBit) ? SDP_IsSparseRowMajor : SDP_IsSparseColMajor,
+      RhsMode = (_RhsNested::Flags&Diagonal)==Diagonal ? SDP_IsDiagonal
+              : (_RhsNested::Flags&RowMajorBit) ? SDP_IsSparseRowMajor : SDP_IsSparseColMajor
+    };
+
+  public:
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseDiagonalProduct)
+    
+    typedef ei_sparse_diagonal_product_inner_iterator_selector
+                <_LhsNested,_RhsNested,SparseDiagonalProduct,LhsMode,RhsMode> InnerIterator;
+
+    template<typename Lhs, typename Rhs>
+    EIGEN_STRONG_INLINE SparseDiagonalProduct(const Lhs& lhs, const Rhs& rhs)
+      : m_lhs(lhs), m_rhs(rhs)
+    {
+      ei_assert(lhs.cols() == rhs.rows() && "invalid sparse matrix * diagonal matrix product");
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_rhs.cols(); }
+
+    EIGEN_STRONG_INLINE const _LhsNested& lhs() const { return m_lhs; }
+    EIGEN_STRONG_INLINE const _RhsNested& rhs() const { return m_rhs; }
+
+  protected:
+    LhsNested m_lhs;
+    RhsNested m_rhs;
+};
+
+
+template<typename Lhs, typename Rhs, typename SparseDiagonalProductType>
+class ei_sparse_diagonal_product_inner_iterator_selector
+<Lhs,Rhs,SparseDiagonalProductType,SDP_IsDiagonal,SDP_IsSparseRowMajor>
+  : public SparseCwiseUnaryOp<ei_scalar_multiple_op<typename Lhs::Scalar>,Rhs>::InnerIterator
+{
+    typedef typename SparseCwiseUnaryOp<ei_scalar_multiple_op<typename Lhs::Scalar>,Rhs>::InnerIterator Base;
+  public:
+    inline ei_sparse_diagonal_product_inner_iterator_selector(
+              const SparseDiagonalProductType& expr, int outer)
+      : Base(expr.rhs()*(expr.lhs().diagonal().coeff(outer)), outer)
+    {}
+};
+
+template<typename Lhs, typename Rhs, typename SparseDiagonalProductType>
+class ei_sparse_diagonal_product_inner_iterator_selector
+<Lhs,Rhs,SparseDiagonalProductType,SDP_IsDiagonal,SDP_IsSparseColMajor>
+  : public SparseCwiseBinaryOp<
+      ei_scalar_product_op<typename Lhs::Scalar>,
+      SparseInnerVectorSet<Rhs,1>,
+      typename Lhs::_CoeffsVectorType>::InnerIterator
+{
+    typedef typename SparseCwiseBinaryOp<
+      ei_scalar_product_op<typename Lhs::Scalar>,
+      SparseInnerVectorSet<Rhs,1>,
+      typename Lhs::_CoeffsVectorType>::InnerIterator Base;
+  public:
+    inline ei_sparse_diagonal_product_inner_iterator_selector(
+              const SparseDiagonalProductType& expr, int outer)
+      : Base(expr.rhs().innerVector(outer) .cwise()* expr.lhs().diagonal(), 0)
+    {}
+};
+
+template<typename Lhs, typename Rhs, typename SparseDiagonalProductType>
+class ei_sparse_diagonal_product_inner_iterator_selector
+<Lhs,Rhs,SparseDiagonalProductType,SDP_IsSparseColMajor,SDP_IsDiagonal>
+  : public SparseCwiseUnaryOp<ei_scalar_multiple_op<typename Rhs::Scalar>,Lhs>::InnerIterator
+{
+    typedef typename SparseCwiseUnaryOp<ei_scalar_multiple_op<typename Rhs::Scalar>,Lhs>::InnerIterator Base;
+  public:
+    inline ei_sparse_diagonal_product_inner_iterator_selector(
+              const SparseDiagonalProductType& expr, int outer)
+      : Base(expr.lhs()*expr.rhs().diagonal().coeff(outer), outer)
+    {}
+};
+
+template<typename Lhs, typename Rhs, typename SparseDiagonalProductType>
+class ei_sparse_diagonal_product_inner_iterator_selector
+<Lhs,Rhs,SparseDiagonalProductType,SDP_IsSparseRowMajor,SDP_IsDiagonal>
+  : public SparseCwiseBinaryOp<
+      ei_scalar_product_op<typename Rhs::Scalar>,
+      SparseInnerVectorSet<Lhs,1>,
+      NestByValue<Transpose<typename Rhs::_CoeffsVectorType> > >::InnerIterator
+{
+    typedef typename SparseCwiseBinaryOp<
+      ei_scalar_product_op<typename Rhs::Scalar>,
+      SparseInnerVectorSet<Lhs,1>,
+      NestByValue<Transpose<typename Rhs::_CoeffsVectorType> > >::InnerIterator Base;
+  public:
+    inline ei_sparse_diagonal_product_inner_iterator_selector(
+              const SparseDiagonalProductType& expr, int outer)
+      : Base(expr.lhs().innerVector(outer) .cwise()* expr.rhs().diagonal().transpose().nestByValue(), 0)
+    {}
+};
+
+#endif // EIGEN_SPARSE_DIAGONAL_PRODUCT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseDot.h b/extern/Eigen2/Eigen/src/Sparse/SparseDot.h
new file mode 100644
index 00000000000..7a26e0f4ba5
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseDot.h
@@ -0,0 +1,97 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_DOT_H
+#define EIGEN_SPARSE_DOT_H
+
+template<typename Derived>
+template<typename OtherDerived>
+typename ei_traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+
+  ei_assert(size() == other.size());
+  ei_assert(other.size()>0 && "you are using a non initialized vector");
+  
+  typename Derived::InnerIterator i(derived(),0);
+  Scalar res = 0;
+  while (i)
+  {
+    res += i.value() * ei_conj(other.coeff(i.index()));
+    ++i;
+  }
+  return res;
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+typename ei_traits<Derived>::Scalar
+SparseMatrixBase<Derived>::dot(const SparseMatrixBase<OtherDerived>& other) const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
+  EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
+    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+  
+  ei_assert(size() == other.size());
+  
+  typename Derived::InnerIterator i(derived(),0);
+  typename OtherDerived::InnerIterator j(other.derived(),0);
+  Scalar res = 0;
+  while (i && j)
+  {
+    if (i.index()==j.index())
+    {
+      res += i.value() * ei_conj(j.value());
+      ++i; ++j;
+    }
+    else if (i.index()<j.index())
+      ++i;
+    else
+      ++j;
+  }
+  return res;
+}
+
+template<typename Derived>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::squaredNorm() const
+{
+  return ei_real((*this).cwise().abs2().sum());
+}
+
+template<typename Derived>
+inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
+SparseMatrixBase<Derived>::norm() const
+{
+  return ei_sqrt(squaredNorm());
+}
+
+#endif // EIGEN_SPARSE_DOT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseFlagged.h b/extern/Eigen2/Eigen/src/Sparse/SparseFlagged.h
new file mode 100644
index 00000000000..c47e162f538
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseFlagged.h
@@ -0,0 +1,97 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_FLAGGED_H
+#define EIGEN_SPARSE_FLAGGED_H
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed>
+struct ei_traits<SparseFlagged<ExpressionType, Added, Removed> > : ei_traits<ExpressionType>
+{
+  enum { Flags = (ExpressionType::Flags | Added) & ~Removed };
+};
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed> class SparseFlagged
+  : public SparseMatrixBase<SparseFlagged<ExpressionType, Added, Removed> >
+{
+  public:
+
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseFlagged)
+    class InnerIterator;
+    class ReverseInnerIterator;
+    
+    typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
+        ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
+
+    inline SparseFlagged(const ExpressionType& matrix) : m_matrix(matrix) {}
+
+    inline int rows() const { return m_matrix.rows(); }
+    inline int cols() const { return m_matrix.cols(); }
+    
+    // FIXME should be keep them ?
+    inline Scalar& coeffRef(int row, int col)
+    { return m_matrix.const_cast_derived().coeffRef(col, row); }
+
+    inline const Scalar coeff(int row, int col) const
+    { return m_matrix.coeff(col, row); }
+
+    inline const Scalar coeff(int index) const
+    { return m_matrix.coeff(index); }
+
+    inline Scalar& coeffRef(int index)
+    { return m_matrix.const_cast_derived().coeffRef(index); }
+
+  protected:
+    ExpressionTypeNested m_matrix;
+};
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed>
+  class SparseFlagged<ExpressionType,Added,Removed>::InnerIterator : public ExpressionType::InnerIterator
+{
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const SparseFlagged& xpr, int outer)
+      : ExpressionType::InnerIterator(xpr.m_matrix, outer)
+    {}
+};
+
+template<typename ExpressionType, unsigned int Added, unsigned int Removed>
+  class SparseFlagged<ExpressionType,Added,Removed>::ReverseInnerIterator : public ExpressionType::ReverseInnerIterator
+{
+  public:
+
+    EIGEN_STRONG_INLINE ReverseInnerIterator(const SparseFlagged& xpr, int outer)
+      : ExpressionType::ReverseInnerIterator(xpr.m_matrix, outer)
+    {}
+};
+
+template<typename Derived>
+template<unsigned int Added>
+inline const SparseFlagged<Derived, Added, 0>
+SparseMatrixBase<Derived>::marked() const
+{
+  return derived();
+}
+
+#endif // EIGEN_SPARSE_FLAGGED_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseFuzzy.h b/extern/Eigen2/Eigen/src/Sparse/SparseFuzzy.h
new file mode 100644
index 00000000000..355f4d52eab
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseFuzzy.h
@@ -0,0 +1,41 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSE_FUZZY_H
+#define EIGEN_SPARSE_FUZZY_H
+
+// template<typename Derived>
+// template<typename OtherDerived>
+// bool SparseMatrixBase<Derived>::isApprox(
+//   const OtherDerived& other,
+//   typename NumTraits<Scalar>::Real prec
+// ) const
+// {
+//   const typename ei_nested<Derived,2>::type nested(derived());
+//   const typename ei_nested<OtherDerived,2>::type otherNested(other.derived());
+//   return    (nested - otherNested).cwise().abs2().sum()
+//          <= prec * prec * std::min(nested.cwise().abs2().sum(), otherNested.cwise().abs2().sum());
+// }
+
+#endif // EIGEN_SPARSE_FUZZY_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseLDLT.h b/extern/Eigen2/Eigen/src/Sparse/SparseLDLT.h
new file mode 100644
index 00000000000..a1bac4d084d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseLDLT.h
@@ -0,0 +1,346 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+/*
+
+NOTE: the _symbolic, and _numeric functions has been adapted from
+      the LDL library:
+
+LDL Copyright (c) 2005 by Timothy A. Davis.  All Rights Reserved.
+
+LDL License:
+
+    Your use or distribution of LDL or any modified version of
+    LDL implies that you agree to this License.
+
+    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
+
+    Permission is hereby granted to use or copy this program under the
+    terms of the GNU LGPL, provided that the Copyright, this License,
+    and the Availability of the original version is retained on all copies.
+    User documentation of any code that uses this code or any modified
+    version of this code must cite the Copyright, this License, the
+    Availability note, and "Used by permission." Permission to modify
+    the code and to distribute modified code is granted, provided the
+    Copyright, this License, and the Availability note are retained,
+    and a notice that the code was modified is included.
+ */
+
+#ifndef EIGEN_SPARSELDLT_H
+#define EIGEN_SPARSELDLT_H
+
+/** \ingroup Sparse_Module
+  *
+  * \class SparseLDLT
+  *
+  * \brief LDLT Cholesky decomposition of a sparse matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LDLT Cholesky decomposition
+  *
+  * \sa class LDLT, class LDLT
+  */
+template<typename MatrixType, int Backend = DefaultBackend>
+class SparseLDLT
+{
+  protected:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef SparseMatrix<Scalar,LowerTriangular|UnitDiagBit> CholMatrixType;
+    typedef Matrix<Scalar,MatrixType::ColsAtCompileTime,1> VectorType;
+
+    enum {
+      SupernodalFactorIsDirty      = 0x10000,
+      MatrixLIsDirty               = 0x20000
+    };
+
+  public:
+
+    /** Creates a dummy LDLT factorization object with flags \a flags. */
+    SparseLDLT(int flags = 0)
+      : m_flags(flags), m_status(0)
+    {
+      ei_assert((MatrixType::Flags&RowMajorBit)==0);
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+    }
+
+    /** Creates a LDLT object and compute the respective factorization of \a matrix using
+      * flags \a flags. */
+    SparseLDLT(const MatrixType& matrix, int flags = 0)
+      : m_matrix(matrix.rows(), matrix.cols()), m_flags(flags), m_status(0)
+    {
+      ei_assert((MatrixType::Flags&RowMajorBit)==0);
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+      compute(matrix);
+    }
+
+    /** Sets the relative threshold value used to prune zero coefficients during the decomposition.
+      *
+      * Setting a value greater than zero speeds up computation, and yields to an imcomplete
+      * factorization with fewer non zero coefficients. Such approximate factors are especially
+      * useful to initialize an iterative solver.
+      *
+      * \warning if precision is greater that zero, the LDLT factorization is not guaranteed to succeed
+      * even if the matrix is positive definite.
+      *
+      * Note that the exact meaning of this parameter might depends on the actual
+      * backend. Moreover, not all backends support this feature.
+      *
+      * \sa precision() */
+    void setPrecision(RealScalar v) { m_precision = v; }
+
+    /** \returns the current precision.
+      *
+      * \sa setPrecision() */
+    RealScalar precision() const { return m_precision; }
+
+    /** Sets the flags. Possible values are:
+      *  - CompleteFactorization
+      *  - IncompleteFactorization
+      *  - MemoryEfficient          (hint to use the memory most efficient method offered by the backend)
+      *  - SupernodalMultifrontal   (implies a complete factorization if supported by the backend,
+      *                              overloads the MemoryEfficient flags)
+      *  - SupernodalLeftLooking    (implies a complete factorization  if supported by the backend,
+      *                              overloads the MemoryEfficient flags)
+      *
+      * \sa flags() */
+    void settagss(int f) { m_flags = f; }
+    /** \returns the current flags */
+    int flags() const { return m_flags; }
+
+    /** Computes/re-computes the LDLT factorization */
+    void compute(const MatrixType& matrix);
+
+    /** Perform a symbolic factorization */
+    void _symbolic(const MatrixType& matrix);
+    /** Perform the actual factorization using the previously
+      * computed symbolic factorization */
+    bool _numeric(const MatrixType& matrix);
+
+    /** \returns the lower triangular matrix L */
+    inline const CholMatrixType& matrixL(void) const { return m_matrix; }
+
+    /** \returns the coefficients of the diagonal matrix D */
+    inline VectorType vectorD(void) const { return m_diag; }
+
+    template<typename Derived>
+    bool solveInPlace(MatrixBase<Derived> &b) const;
+
+    /** \returns true if the factorization succeeded */
+    inline bool succeeded(void) const { return m_succeeded; }
+
+  protected:
+    CholMatrixType m_matrix;
+    VectorType m_diag;
+    VectorXi m_parent; // elimination tree
+    VectorXi m_nonZerosPerCol;
+//     VectorXi m_w; // workspace
+    RealScalar m_precision;
+    int m_flags;
+    mutable int m_status;
+    bool m_succeeded;
+};
+
+/** Computes / recomputes the LDLT decomposition of matrix \a a
+  * using the default algorithm.
+  */
+template<typename MatrixType, int Backend>
+void SparseLDLT<MatrixType,Backend>::compute(const MatrixType& a)
+{
+  _symbolic(a);
+  m_succeeded = _numeric(a);
+}
+
+template<typename MatrixType, int Backend>
+void SparseLDLT<MatrixType,Backend>::_symbolic(const MatrixType& a)
+{
+  assert(a.rows()==a.cols());
+  const int size = a.rows();
+  m_matrix.resize(size, size);
+  m_parent.resize(size);
+  m_nonZerosPerCol.resize(size);
+  int * tags = ei_aligned_stack_new(int, size);
+
+  const int* Ap = a._outerIndexPtr();
+  const int* Ai = a._innerIndexPtr();
+  int* Lp = m_matrix._outerIndexPtr();
+  const int* P = 0;
+  int* Pinv = 0;
+
+  if (P)
+  {
+    /* If P is present then compute Pinv, the inverse of P */
+    for (int k = 0; k < size; ++k)
+      Pinv[P[k]] = k;
+  }
+  for (int k = 0; k < size; ++k)
+  {
+    /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */
+    m_parent[k] = -1;             /* parent of k is not yet known */
+    tags[k] = k;                  /* mark node k as visited */
+    m_nonZerosPerCol[k] = 0;      /* count of nonzeros in column k of L */
+    int kk = P ? P[k] : k;  /* kth original, or permuted, column */
+    int p2 = Ap[kk+1];
+    for (int p = Ap[kk]; p < p2; ++p)
+    {
+      /* A (i,k) is nonzero (original or permuted A) */
+      int i = Pinv ? Pinv[Ai[p]] : Ai[p];
+      if (i < k)
+      {
+        /* follow path from i to root of etree, stop at flagged node */
+        for (; tags[i] != k; i = m_parent[i])
+        {
+          /* find parent of i if not yet determined */
+          if (m_parent[i] == -1)
+            m_parent[i] = k;
+          ++m_nonZerosPerCol[i];        /* L (k,i) is nonzero */
+          tags[i] = k;                  /* mark i as visited */
+        }
+      }
+    }
+  }
+  /* construct Lp index array from m_nonZerosPerCol column counts */
+  Lp[0] = 0;
+  for (int k = 0; k < size; ++k)
+    Lp[k+1] = Lp[k] + m_nonZerosPerCol[k];
+
+  m_matrix.resizeNonZeros(Lp[size]);
+  ei_aligned_stack_delete(int, tags, size);
+}
+
+template<typename MatrixType, int Backend>
+bool SparseLDLT<MatrixType,Backend>::_numeric(const MatrixType& a)
+{
+  assert(a.rows()==a.cols());
+  const int size = a.rows();
+  assert(m_parent.size()==size);
+  assert(m_nonZerosPerCol.size()==size);
+
+  const int* Ap = a._outerIndexPtr();
+  const int* Ai = a._innerIndexPtr();
+  const Scalar* Ax = a._valuePtr();
+  const int* Lp = m_matrix._outerIndexPtr();
+  int* Li = m_matrix._innerIndexPtr();
+  Scalar* Lx = m_matrix._valuePtr();
+  m_diag.resize(size);
+
+  Scalar * y = ei_aligned_stack_new(Scalar, size);
+  int * pattern = ei_aligned_stack_new(int, size);
+  int * tags = ei_aligned_stack_new(int, size);
+
+  const int* P = 0;
+  const int* Pinv = 0;
+  bool ok = true;
+
+  for (int k = 0; k < size; ++k)
+  {
+    /* compute nonzero pattern of kth row of L, in topological order */
+    y[k] = 0.0;                   /* Y(0:k) is now all zero */
+    int top = size;               /* stack for pattern is empty */
+    tags[k] = k;                  /* mark node k as visited */
+    m_nonZerosPerCol[k] = 0;      /* count of nonzeros in column k of L */
+    int kk = (P) ? (P[k]) : (k);  /* kth original, or permuted, column */
+    int p2 = Ap[kk+1];
+    for (int p = Ap[kk]; p < p2; ++p)
+    {
+      int i = Pinv ? Pinv[Ai[p]] : Ai[p]; /* get A(i,k) */
+      if (i <= k)
+      {
+        y[i] += Ax[p];            /* scatter A(i,k) into Y (sum duplicates) */
+        int len;
+        for (len = 0; tags[i] != k; i = m_parent[i])
+        {
+          pattern[len++] = i;     /* L(k,i) is nonzero */
+          tags[i] = k;            /* mark i as visited */
+        }
+        while (len > 0)
+          pattern[--top] = pattern[--len];
+      }
+    }
+    /* compute numerical values kth row of L (a sparse triangular solve) */
+    m_diag[k] = y[k];                       /* get D(k,k) and clear Y(k) */
+    y[k] = 0.0;
+    for (; top < size; ++top)
+    {
+      int i = pattern[top];      /* pattern[top:n-1] is pattern of L(:,k) */
+      Scalar yi = y[i];          /* get and clear Y(i) */
+      y[i] = 0.0;
+      int p2 = Lp[i] + m_nonZerosPerCol[i];
+      int p;
+      for (p = Lp[i]; p < p2; ++p)
+        y[Li[p]] -= Lx[p] * yi;
+      Scalar l_ki = yi / m_diag[i];       /* the nonzero entry L(k,i) */
+      m_diag[k] -= l_ki * yi;
+      Li[p] = k;                          /* store L(k,i) in column form of L */
+      Lx[p] = l_ki;
+      ++m_nonZerosPerCol[i];              /* increment count of nonzeros in col i */
+    }
+    if (m_diag[k] == 0.0)
+    {
+      ok = false;                         /* failure, D(k,k) is zero */
+      break;
+    }
+  }
+
+  ei_aligned_stack_delete(Scalar, y, size);
+  ei_aligned_stack_delete(int, pattern, size);
+  ei_aligned_stack_delete(int, tags, size);
+
+  return ok;  /* success, diagonal of D is all nonzero */
+}
+
+/** Computes b = L^-T L^-1 b */
+template<typename MatrixType, int Backend>
+template<typename Derived>
+bool SparseLDLT<MatrixType, Backend>::solveInPlace(MatrixBase<Derived> &b) const
+{
+  const int size = m_matrix.rows();
+  ei_assert(size==b.rows());
+  if (!m_succeeded)
+    return false;
+
+  if (m_matrix.nonZeros()>0) // otherwise L==I
+    m_matrix.solveTriangularInPlace(b);
+  b = b.cwise() / m_diag;
+  // FIXME should be .adjoint() but it fails to compile...
+
+  if (m_matrix.nonZeros()>0) // otherwise L==I
+  m_matrix.transpose().solveTriangularInPlace(b);
+
+  return true;
+}
+
+#endif // EIGEN_SPARSELDLT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseLLT.h b/extern/Eigen2/Eigen/src/Sparse/SparseLLT.h
new file mode 100644
index 00000000000..e7c314c2cad
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseLLT.h
@@ -0,0 +1,205 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSELLT_H
+#define EIGEN_SPARSELLT_H
+
+/** \ingroup Sparse_Module
+  *
+  * \class SparseLLT
+  *
+  * \brief LLT Cholesky decomposition of a sparse matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LLT Cholesky decomposition
+  *
+  * \sa class LLT, class LDLT
+  */
+template<typename MatrixType, int Backend = DefaultBackend>
+class SparseLLT
+{
+  protected:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef SparseMatrix<Scalar,LowerTriangular> CholMatrixType;
+
+    enum {
+      SupernodalFactorIsDirty      = 0x10000,
+      MatrixLIsDirty               = 0x20000
+    };
+
+  public:
+
+    /** Creates a dummy LLT factorization object with flags \a flags. */
+    SparseLLT(int flags = 0)
+      : m_flags(flags), m_status(0)
+    {
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+    }
+
+    /** Creates a LLT object and compute the respective factorization of \a matrix using
+      * flags \a flags. */
+    SparseLLT(const MatrixType& matrix, int flags = 0)
+      : m_matrix(matrix.rows(), matrix.cols()), m_flags(flags), m_status(0)
+    {
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+      compute(matrix);
+    }
+
+    /** Sets the relative threshold value used to prune zero coefficients during the decomposition.
+      *
+      * Setting a value greater than zero speeds up computation, and yields to an imcomplete
+      * factorization with fewer non zero coefficients. Such approximate factors are especially
+      * useful to initialize an iterative solver.
+      *
+      * \warning if precision is greater that zero, the LLT factorization is not guaranteed to succeed
+      * even if the matrix is positive definite.
+      *
+      * Note that the exact meaning of this parameter might depends on the actual
+      * backend. Moreover, not all backends support this feature.
+      *
+      * \sa precision() */
+    void setPrecision(RealScalar v) { m_precision = v; }
+
+    /** \returns the current precision.
+      *
+      * \sa setPrecision() */
+    RealScalar precision() const { return m_precision; }
+
+    /** Sets the flags. Possible values are:
+      *  - CompleteFactorization
+      *  - IncompleteFactorization
+      *  - MemoryEfficient          (hint to use the memory most efficient method offered by the backend)
+      *  - SupernodalMultifrontal   (implies a complete factorization if supported by the backend,
+      *                              overloads the MemoryEfficient flags)
+      *  - SupernodalLeftLooking    (implies a complete factorization  if supported by the backend,
+      *                              overloads the MemoryEfficient flags)
+      *
+      * \sa flags() */
+    void setFlags(int f) { m_flags = f; }
+    /** \returns the current flags */
+    int flags() const { return m_flags; }
+
+    /** Computes/re-computes the LLT factorization */
+    void compute(const MatrixType& matrix);
+
+    /** \returns the lower triangular matrix L */
+    inline const CholMatrixType& matrixL(void) const { return m_matrix; }
+
+    template<typename Derived>
+    bool solveInPlace(MatrixBase<Derived> &b) const;
+
+    /** \returns true if the factorization succeeded */
+    inline bool succeeded(void) const { return m_succeeded; }
+
+  protected:
+    CholMatrixType m_matrix;
+    RealScalar m_precision;
+    int m_flags;
+    mutable int m_status;
+    bool m_succeeded;
+};
+
+/** Computes / recomputes the LLT decomposition of matrix \a a
+  * using the default algorithm.
+  */
+template<typename MatrixType, int Backend>
+void SparseLLT<MatrixType,Backend>::compute(const MatrixType& a)
+{
+  assert(a.rows()==a.cols());
+  const int size = a.rows();
+  m_matrix.resize(size, size);
+
+  // allocate a temporary vector for accumulations
+  AmbiVector<Scalar> tempVector(size);
+  RealScalar density = a.nonZeros()/RealScalar(size*size);
+
+  // TODO estimate the number of non zeros
+  m_matrix.startFill(a.nonZeros()*2);
+  for (int j = 0; j < size; ++j)
+  {
+    Scalar x = ei_real(a.coeff(j,j));
+
+    // TODO better estimate of the density !
+    tempVector.init(density>0.001? IsDense : IsSparse);
+    tempVector.setBounds(j+1,size);
+    tempVector.setZero();
+    // init with current matrix a
+    {
+      typename MatrixType::InnerIterator it(a,j);
+      ++it; // skip diagonal element
+      for (; it; ++it)
+        tempVector.coeffRef(it.index()) = it.value();
+    }
+    for (int k=0; k<j+1; ++k)
+    {
+      typename CholMatrixType::InnerIterator it(m_matrix, k);
+      while (it && it.index()<j)
+        ++it;
+      if (it && it.index()==j)
+      {
+        Scalar y = it.value();
+        x -= ei_abs2(y);
+        ++it; // skip j-th element, and process remaining column coefficients
+        tempVector.restart();
+        for (; it; ++it)
+        {
+          tempVector.coeffRef(it.index()) -= it.value() * y;
+        }
+      }
+    }
+    // copy the temporary vector to the respective m_matrix.col()
+    // while scaling the result by 1/real(x)
+    RealScalar rx = ei_sqrt(ei_real(x));
+    m_matrix.fill(j,j) = rx;
+    Scalar y = Scalar(1)/rx;
+    for (typename AmbiVector<Scalar>::Iterator it(tempVector, m_precision*rx); it; ++it)
+    {
+      m_matrix.fill(it.index(), j) = it.value() * y;
+    }
+  }
+  m_matrix.endFill();
+}
+
+/** Computes b = L^-T L^-1 b */
+template<typename MatrixType, int Backend>
+template<typename Derived>
+bool SparseLLT<MatrixType, Backend>::solveInPlace(MatrixBase<Derived> &b) const
+{
+  const int size = m_matrix.rows();
+  ei_assert(size==b.rows());
+
+  m_matrix.solveTriangularInPlace(b);
+  // FIXME should be simply .adjoint() but it fails to compile...
+  if (NumTraits<Scalar>::IsComplex)
+  {
+    CholMatrixType aux = m_matrix.conjugate();
+    aux.transpose().solveTriangularInPlace(b);
+  }
+  else
+    m_matrix.transpose().solveTriangularInPlace(b);
+
+  return true;
+}
+
+#endif // EIGEN_SPARSELLT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseLU.h b/extern/Eigen2/Eigen/src/Sparse/SparseLU.h
new file mode 100644
index 00000000000..1425920509f
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseLU.h
@@ -0,0 +1,148 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSELU_H
+#define EIGEN_SPARSELU_H
+
+/** \ingroup Sparse_Module
+  *
+  * \class SparseLU
+  *
+  * \brief LU decomposition of a sparse matrix and associated features
+  *
+  * \param MatrixType the type of the matrix of which we are computing the LU factorization
+  *
+  * \sa class LU, class SparseLLT
+  */
+template<typename MatrixType, int Backend = DefaultBackend>
+class SparseLU
+{
+  protected:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    typedef SparseMatrix<Scalar,LowerTriangular> LUMatrixType;
+
+    enum {
+      MatrixLUIsDirty             = 0x10000
+    };
+
+  public:
+
+    /** Creates a dummy LU factorization object with flags \a flags. */
+    SparseLU(int flags = 0)
+      : m_flags(flags), m_status(0)
+    {
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+    }
+
+    /** Creates a LU object and compute the respective factorization of \a matrix using
+      * flags \a flags. */
+    SparseLU(const MatrixType& matrix, int flags = 0)
+      : /*m_matrix(matrix.rows(), matrix.cols()),*/ m_flags(flags), m_status(0)
+    {
+      m_precision = RealScalar(0.1) * Eigen::precision<RealScalar>();
+      compute(matrix);
+    }
+
+    /** Sets the relative threshold value used to prune zero coefficients during the decomposition.
+      *
+      * Setting a value greater than zero speeds up computation, and yields to an imcomplete
+      * factorization with fewer non zero coefficients. Such approximate factors are especially
+      * useful to initialize an iterative solver.
+      *
+      * Note that the exact meaning of this parameter might depends on the actual
+      * backend. Moreover, not all backends support this feature.
+      *
+      * \sa precision() */
+    void setPrecision(RealScalar v) { m_precision = v; }
+
+    /** \returns the current precision.
+      *
+      * \sa setPrecision() */
+    RealScalar precision() const { return m_precision; }
+
+    /** Sets the flags. Possible values are:
+      *  - CompleteFactorization
+      *  - IncompleteFactorization
+      *  - MemoryEfficient
+      *  - one of the ordering methods
+      *  - etc...
+      *
+      * \sa flags() */
+    void setFlags(int f) { m_flags = f; }
+    /** \returns the current flags */
+    int flags() const { return m_flags; }
+
+    void setOrderingMethod(int m)
+    {
+      ei_assert(m&~OrderingMask == 0 && m!=0 && "invalid ordering method");
+      m_flags = m_flags&~OrderingMask | m&OrderingMask;
+    }
+
+    int orderingMethod() const
+    {
+      return m_flags&OrderingMask;
+    }
+
+    /** Computes/re-computes the LU factorization */
+    void compute(const MatrixType& matrix);
+
+    /** \returns the lower triangular matrix L */
+    //inline const MatrixType& matrixL() const { return m_matrixL; }
+
+    /** \returns the upper triangular matrix U */
+    //inline const MatrixType& matrixU() const { return m_matrixU; }
+
+    template<typename BDerived, typename XDerived>
+    bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x) const;
+
+    /** \returns true if the factorization succeeded */
+    inline bool succeeded(void) const { return m_succeeded; }
+
+  protected:
+    RealScalar m_precision;
+    int m_flags;
+    mutable int m_status;
+    bool m_succeeded;
+};
+
+/** Computes / recomputes the LU decomposition of matrix \a a
+  * using the default algorithm.
+  */
+template<typename MatrixType, int Backend>
+void SparseLU<MatrixType,Backend>::compute(const MatrixType& a)
+{
+  ei_assert(false && "not implemented yet");
+}
+
+/** Computes *x = U^-1 L^-1 b */
+template<typename MatrixType, int Backend>
+template<typename BDerived, typename XDerived>
+bool SparseLU<MatrixType,Backend>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x) const
+{
+  ei_assert(false && "not implemented yet");
+  return false;
+}
+
+#endif // EIGEN_SPARSELU_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseMatrix.h b/extern/Eigen2/Eigen/src/Sparse/SparseMatrix.h
new file mode 100644
index 00000000000..3f09596bc64
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseMatrix.h
@@ -0,0 +1,447 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEMATRIX_H
+#define EIGEN_SPARSEMATRIX_H
+
+/** \class SparseMatrix
+  *
+  * \brief Sparse matrix
+  *
+  * \param _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+  *
+  */
+template<typename _Scalar, int _Flags>
+struct ei_traits<SparseMatrix<_Scalar, _Flags> >
+{
+  typedef _Scalar Scalar;
+  enum {
+    RowsAtCompileTime = Dynamic,
+    ColsAtCompileTime = Dynamic,
+    MaxRowsAtCompileTime = Dynamic,
+    MaxColsAtCompileTime = Dynamic,
+    Flags = SparseBit | _Flags,
+    CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    SupportedAccessPatterns = InnerRandomAccessPattern
+  };
+};
+
+
+
+template<typename _Scalar, int _Flags>
+class SparseMatrix
+  : public SparseMatrixBase<SparseMatrix<_Scalar, _Flags> >
+{
+  public:
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseMatrix)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseMatrix, +=)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseMatrix, -=)
+    // FIXME: why are these operator already alvailable ???
+    // EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(SparseMatrix, *=)
+    // EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(SparseMatrix, /=)
+
+    typedef MappedSparseMatrix<Scalar,Flags> Map;
+
+  protected:
+
+    enum { IsRowMajor = Base::IsRowMajor };
+    typedef SparseMatrix<Scalar,(Flags&~RowMajorBit)|(IsRowMajor?RowMajorBit:0)> TransposedSparseMatrix;
+
+    int m_outerSize;
+    int m_innerSize;
+    int* m_outerIndex;
+    CompressedStorage<Scalar> m_data;
+
+  public:
+
+    inline int rows() const { return IsRowMajor ? m_outerSize : m_innerSize; }
+    inline int cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
+
+    inline int innerSize() const { return m_innerSize; }
+    inline int outerSize() const { return m_outerSize; }
+    inline int innerNonZeros(int j) const { return m_outerIndex[j+1]-m_outerIndex[j]; }
+
+    inline const Scalar* _valuePtr() const { return &m_data.value(0); }
+    inline Scalar* _valuePtr() { return &m_data.value(0); }
+
+    inline const int* _innerIndexPtr() const { return &m_data.index(0); }
+    inline int* _innerIndexPtr() { return &m_data.index(0); }
+
+    inline const int* _outerIndexPtr() const { return m_outerIndex; }
+    inline int* _outerIndexPtr() { return m_outerIndex; }
+
+    inline Scalar coeff(int row, int col) const
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      return m_data.atInRange(m_outerIndex[outer], m_outerIndex[outer+1], inner);
+    }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+
+      int start = m_outerIndex[outer];
+      int end = m_outerIndex[outer+1];
+      ei_assert(end>=start && "you probably called coeffRef on a non finalized matrix");
+      ei_assert(end>start && "coeffRef cannot be called on a zero coefficient");
+      const int id = m_data.searchLowerIndex(start,end-1,inner);
+      ei_assert((id<end) && (m_data.index(id)==inner) && "coeffRef cannot be called on a zero coefficient");
+      return m_data.value(id);
+    }
+
+  public:
+
+    class InnerIterator;
+
+    inline void setZero()
+    {
+      m_data.clear();
+      //if (m_outerSize)
+      memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(int));
+//       for (int i=0; i<m_outerSize; ++i)
+//         m_outerIndex[i] = 0;
+//       if (m_outerSize)
+//         m_outerIndex[i] = 0;
+    }
+
+    /** \returns the number of non zero coefficients */
+    inline int nonZeros() const  { return m_data.size(); }
+
+    /** Initializes the filling process of \c *this.
+      * \param reserveSize approximate number of nonzeros
+      * Note that the matrix \c *this is zero-ed.
+      */
+    inline void startFill(int reserveSize = 1000)
+    {
+      setZero();
+      m_data.reserve(reserveSize);
+    }
+
+    /**
+      */
+    inline Scalar& fill(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+
+      if (m_outerIndex[outer+1]==0)
+      {
+        // we start a new inner vector
+        int i = outer;
+        while (i>=0 && m_outerIndex[i]==0)
+        {
+          m_outerIndex[i] = m_data.size();
+          --i;
+        }
+        m_outerIndex[outer+1] = m_outerIndex[outer];
+      }
+      else
+      {
+        ei_assert(m_data.index(m_data.size()-1)<inner && "wrong sorted insertion");
+      }
+      assert(size_t(m_outerIndex[outer+1]) == m_data.size());
+      int id = m_outerIndex[outer+1];
+      ++m_outerIndex[outer+1];
+
+      m_data.append(0, inner);
+      return m_data.value(id);
+    }
+
+    /** Like fill() but with random inner coordinates.
+      */
+    inline Scalar& fillrand(int row, int col)
+    {
+      const int outer = IsRowMajor ? row : col;
+      const int inner = IsRowMajor ? col : row;
+      if (m_outerIndex[outer+1]==0)
+      {
+        // we start a new inner vector
+        // nothing special to do here
+        int i = outer;
+        while (i>=0 && m_outerIndex[i]==0)
+        {
+          m_outerIndex[i] = m_data.size();
+          --i;
+        }
+        m_outerIndex[outer+1] = m_outerIndex[outer];
+      }
+      assert(size_t(m_outerIndex[outer+1]) == m_data.size() && "invalid outer index");
+      size_t startId = m_outerIndex[outer];
+      // FIXME let's make sure sizeof(long int) == sizeof(size_t)
+      size_t id = m_outerIndex[outer+1];
+      ++m_outerIndex[outer+1];
+
+      float reallocRatio = 1;
+      if (m_data.allocatedSize()<id+1)
+      {
+        // we need to reallocate the data, to reduce multiple reallocations
+        // we use a smart resize algorithm based on the current filling ratio
+        // we use float to avoid overflows
+        float nnzEstimate = float(m_outerIndex[outer])*float(m_outerSize)/float(outer);
+        reallocRatio = (nnzEstimate-float(m_data.size()))/float(m_data.size());
+        // let's bounds the realloc ratio to
+        //   1) reduce multiple minor realloc when the matrix is almost filled
+        //   2) avoid to allocate too much memory when the matrix is almost empty
+        reallocRatio = std::min(std::max(reallocRatio,1.5f),8.f);
+      }
+      m_data.resize(id+1,reallocRatio);
+
+      while ( (id > startId) && (m_data.index(id-1) > inner) )
+      {
+        m_data.index(id) = m_data.index(id-1);
+        m_data.value(id) = m_data.value(id-1);
+        --id;
+      }
+
+      m_data.index(id) = inner;
+      return (m_data.value(id) = 0);
+    }
+
+    inline void endFill()
+    {
+      int size = m_data.size();
+      int i = m_outerSize;
+      // find the last filled column
+      while (i>=0 && m_outerIndex[i]==0)
+        --i;
+      ++i;
+      while (i<=m_outerSize)
+      {
+        m_outerIndex[i] = size;
+        ++i;
+      }
+    }
+
+    void prune(Scalar reference, RealScalar epsilon = precision<RealScalar>())
+    {
+      int k = 0;
+      for (int j=0; j<m_outerSize; ++j)
+      {
+        int previousStart = m_outerIndex[j];
+        m_outerIndex[j] = k;
+        int end = m_outerIndex[j+1];
+        for (int i=previousStart; i<end; ++i)
+        {
+          if (!ei_isMuchSmallerThan(m_data.value(i), reference, epsilon))
+          {
+            m_data.value(k) = m_data.value(i);
+            m_data.index(k) = m_data.index(i);
+            ++k;
+          }
+        }
+      }
+      m_outerIndex[m_outerSize] = k;
+      m_data.resize(k,0);
+    }
+
+    void resize(int rows, int cols)
+    {
+//       std::cerr << this << " resize " << rows << "x" << cols << "\n";
+      const int outerSize = IsRowMajor ? rows : cols;
+      m_innerSize = IsRowMajor ? cols : rows;
+      m_data.clear();
+      if (m_outerSize != outerSize)
+      {
+        delete[] m_outerIndex;
+        m_outerIndex = new int [outerSize+1];
+        m_outerSize = outerSize;
+        memset(m_outerIndex, 0, (m_outerSize+1)*sizeof(int));
+      }
+    }
+    void resizeNonZeros(int size)
+    {
+      m_data.resize(size);
+    }
+
+    inline SparseMatrix()
+      : m_outerSize(-1), m_innerSize(0), m_outerIndex(0)
+    {
+      resize(0, 0);
+    }
+
+    inline SparseMatrix(int rows, int cols)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0)
+    {
+      resize(rows, cols);
+    }
+
+    template<typename OtherDerived>
+    inline SparseMatrix(const SparseMatrixBase<OtherDerived>& other)
+      : m_outerSize(0), m_innerSize(0), m_outerIndex(0)
+    {
+      *this = other.derived();
+    }
+
+    inline SparseMatrix(const SparseMatrix& other)
+      : Base(), m_outerSize(0), m_innerSize(0), m_outerIndex(0)
+    {
+      *this = other.derived();
+    }
+
+    inline void swap(SparseMatrix& other)
+    {
+      //EIGEN_DBG_SPARSE(std::cout << "SparseMatrix:: swap\n");
+      std::swap(m_outerIndex, other.m_outerIndex);
+      std::swap(m_innerSize, other.m_innerSize);
+      std::swap(m_outerSize, other.m_outerSize);
+      m_data.swap(other.m_data);
+    }
+
+    inline SparseMatrix& operator=(const SparseMatrix& other)
+    {
+//       std::cout << "SparseMatrix& operator=(const SparseMatrix& other)\n";
+      if (other.isRValue())
+      {
+        swap(other.const_cast_derived());
+      }
+      else
+      {
+        resize(other.rows(), other.cols());
+        memcpy(m_outerIndex, other.m_outerIndex, (m_outerSize+1)*sizeof(int));
+        m_data = other.m_data;
+      }
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    inline SparseMatrix& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      const bool needToTranspose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit);
+      if (needToTranspose)
+      {
+        // two passes algorithm:
+        //  1 - compute the number of coeffs per dest inner vector
+        //  2 - do the actual copy/eval
+        // Since each coeff of the rhs has to be evaluated twice, let's evauluate it if needed
+        //typedef typename ei_nested<OtherDerived,2>::type OtherCopy;
+        typedef typename ei_eval<OtherDerived>::type OtherCopy;
+        typedef typename ei_cleantype<OtherCopy>::type _OtherCopy;
+        OtherCopy otherCopy(other.derived());
+
+        resize(other.rows(), other.cols());
+        Eigen::Map<VectorXi>(m_outerIndex,outerSize()).setZero();
+        // pass 1
+        // FIXME the above copy could be merged with that pass
+        for (int j=0; j<otherCopy.outerSize(); ++j)
+          for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
+            ++m_outerIndex[it.index()];
+
+        // prefix sum
+        int count = 0;
+        VectorXi positions(outerSize());
+        for (int j=0; j<outerSize(); ++j)
+        {
+          int tmp = m_outerIndex[j];
+          m_outerIndex[j] = count;
+          positions[j] = count;
+          count += tmp;
+        }
+        m_outerIndex[outerSize()] = count;
+        // alloc
+        m_data.resize(count);
+        // pass 2
+        for (int j=0; j<otherCopy.outerSize(); ++j)
+          for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
+          {
+            int pos = positions[it.index()]++;
+            m_data.index(pos) = j;
+            m_data.value(pos) = it.value();
+          }
+
+        return *this;
+      }
+      else
+      {
+        // there is no special optimization
+        return SparseMatrixBase<SparseMatrix>::operator=(other.derived());
+      }
+    }
+
+    friend std::ostream & operator << (std::ostream & s, const SparseMatrix& m)
+    {
+      EIGEN_DBG_SPARSE(
+        s << "Nonzero entries:\n";
+        for (int i=0; i<m.nonZeros(); ++i)
+        {
+          s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+        }
+        s << std::endl;
+        s << std::endl;
+        s << "Column pointers:\n";
+        for (int i=0; i<m.outerSize(); ++i)
+        {
+          s << m.m_outerIndex[i] << " ";
+        }
+        s << " $" << std::endl;
+        s << std::endl;
+      );
+      s << static_cast<const SparseMatrixBase<SparseMatrix>&>(m);
+      return s;
+    }
+
+    /** Destructor */
+    inline ~SparseMatrix()
+    {
+      delete[] m_outerIndex;
+    }
+};
+
+template<typename Scalar, int _Flags>
+class SparseMatrix<Scalar,_Flags>::InnerIterator
+{
+  public:
+    InnerIterator(const SparseMatrix& mat, int outer)
+      : m_matrix(mat), m_outer(outer), m_id(mat.m_outerIndex[outer]), m_start(m_id), m_end(mat.m_outerIndex[outer+1])
+    {}
+
+    template<unsigned int Added, unsigned int Removed>
+    InnerIterator(const Flagged<SparseMatrix,Added,Removed>& mat, int outer)
+      : m_matrix(mat._expression()), m_outer(outer), m_id(m_matrix.m_outerIndex[outer]),
+        m_start(m_id), m_end(m_matrix.m_outerIndex[outer+1])
+    {}
+
+    inline InnerIterator& operator++() { m_id++; return *this; }
+
+    inline Scalar value() const { return m_matrix.m_data.value(m_id); }
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_matrix.m_data.value(m_id)); }
+
+    inline int index() const { return m_matrix.m_data.index(m_id); }
+    inline int row() const { return IsRowMajor ? m_outer : index(); }
+    inline int col() const { return IsRowMajor ? index() : m_outer; }
+
+    inline operator bool() const { return (m_id < m_end) && (m_id>=m_start); }
+
+  protected:
+    const SparseMatrix& m_matrix;
+    const int m_outer;
+    int m_id;
+    const int m_start;
+    const int m_end;
+};
+
+#endif // EIGEN_SPARSEMATRIX_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseMatrixBase.h b/extern/Eigen2/Eigen/src/Sparse/SparseMatrixBase.h
new file mode 100644
index 00000000000..468bc9e227c
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseMatrixBase.h
@@ -0,0 +1,626 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEMATRIXBASE_H
+#define EIGEN_SPARSEMATRIXBASE_H
+
+template<typename Derived> class SparseMatrixBase
+{
+  public:
+
+    typedef typename ei_traits<Derived>::Scalar Scalar;
+//     typedef typename Derived::InnerIterator InnerIterator;
+
+    enum {
+
+      RowsAtCompileTime = ei_traits<Derived>::RowsAtCompileTime,
+        /**< The number of rows at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
+
+      ColsAtCompileTime = ei_traits<Derived>::ColsAtCompileTime,
+        /**< The number of columns at compile-time. This is just a copy of the value provided
+          * by the \a Derived type. If a value is not known at compile-time,
+          * it is set to the \a Dynamic constant.
+          * \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
+
+
+      SizeAtCompileTime = (ei_size_at_compile_time<ei_traits<Derived>::RowsAtCompileTime,
+                                                   ei_traits<Derived>::ColsAtCompileTime>::ret),
+        /**< This is equal to the number of coefficients, i.e. the number of
+          * rows times the number of columns, or to \a Dynamic if this is not
+          * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
+
+      MaxRowsAtCompileTime = RowsAtCompileTime,
+      MaxColsAtCompileTime = ColsAtCompileTime,
+
+      MaxSizeAtCompileTime = (ei_size_at_compile_time<MaxRowsAtCompileTime,
+                                                      MaxColsAtCompileTime>::ret),
+
+      IsVectorAtCompileTime = RowsAtCompileTime == 1 || ColsAtCompileTime == 1,
+        /**< This is set to true if either the number of rows or the number of
+          * columns is known at compile-time to be equal to 1. Indeed, in that case,
+          * we are dealing with a column-vector (if there is only one column) or with
+          * a row-vector (if there is only one row). */
+
+      Flags = ei_traits<Derived>::Flags,
+        /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
+          * constructed from this one. See the \ref flags "list of flags".
+          */
+      
+      CoeffReadCost = ei_traits<Derived>::CoeffReadCost,
+        /**< This is a rough measure of how expensive it is to read one coefficient from
+          * this expression.
+          */
+
+      IsRowMajor = Flags&RowMajorBit ? 1 : 0
+    };
+
+    /** \internal the return type of MatrixBase::conjugate() */
+    typedef typename ei_meta_if<NumTraits<Scalar>::IsComplex,
+                        const SparseCwiseUnaryOp<ei_scalar_conjugate_op<Scalar>, Derived>,
+                        const Derived&
+                     >::ret ConjugateReturnType;
+    /** \internal the return type of MatrixBase::real() */
+    typedef CwiseUnaryOp<ei_scalar_real_op<Scalar>, Derived> RealReturnType;
+    /** \internal the return type of MatrixBase::imag() */
+    typedef CwiseUnaryOp<ei_scalar_imag_op<Scalar>, Derived> ImagReturnType;
+    /** \internal the return type of MatrixBase::adjoint() */
+    typedef SparseTranspose</*NestByValue<*/typename ei_cleantype<ConjugateReturnType>::type> /*>*/
+            AdjointReturnType;
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** This is the "real scalar" type; if the \a Scalar type is already real numbers
+      * (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
+      * \a Scalar is \a std::complex<T> then RealScalar is \a T.
+      *
+      * \sa class NumTraits
+      */
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    /** type of the equivalent square matrix */
+    typedef Matrix<Scalar,EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime),
+                          EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
+
+    inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
+    inline Derived& derived() { return *static_cast<Derived*>(this); }
+    inline Derived& const_cast_derived() const
+    { return *static_cast<Derived*>(const_cast<SparseMatrixBase*>(this)); }
+#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    /** \returns the number of rows. \sa cols(), RowsAtCompileTime */
+    inline int rows() const { return derived().rows(); }
+    /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/
+    inline int cols() const { return derived().cols(); }
+    /** \returns the number of coefficients, which is \a rows()*cols().
+      * \sa rows(), cols(), SizeAtCompileTime. */
+    inline int size() const { return rows() * cols(); }
+    /** \returns the number of nonzero coefficients which is in practice the number
+      * of stored coefficients. */
+    inline int nonZeros() const { return derived().nonZeros(); }
+    /** \returns true if either the number of rows or the number of columns is equal to 1.
+      * In other words, this function returns
+      * \code rows()==1 || cols()==1 \endcode
+      * \sa rows(), cols(), IsVectorAtCompileTime. */
+    inline bool isVector() const { return rows()==1 || cols()==1; }
+    /** \returns the size of the storage major dimension,
+      * i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
+    int outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
+    /** \returns the size of the inner dimension according to the storage order,
+      * i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
+    int innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
+
+    bool isRValue() const { return m_isRValue; }
+    Derived& markAsRValue() { m_isRValue = true; return derived(); }
+
+    SparseMatrixBase() : m_isRValue(false) { /* TODO check flags */ }
+
+    inline Derived& operator=(const Derived& other)
+    {
+//       std::cout << "Derived& operator=(const Derived& other)\n";
+//       if (other.isRValue())
+//         derived().swap(other.const_cast_derived());
+//       else
+        this->operator=<Derived>(other);
+      return derived();
+    }
+
+
+    template<typename OtherDerived>
+    inline void assignGeneric(const OtherDerived& other)
+    {
+//       std::cout << "Derived& operator=(const MatrixBase<OtherDerived>& other)\n";
+      //const bool transpose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit);
+      ei_assert(( ((ei_traits<Derived>::SupportedAccessPatterns&OuterRandomAccessPattern)==OuterRandomAccessPattern) ||
+                  (!((Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit)))) &&
+                  "the transpose operation is supposed to be handled in SparseMatrix::operator=");
+
+      const int outerSize = other.outerSize();
+      //typedef typename ei_meta_if<transpose, LinkedVectorMatrix<Scalar,Flags&RowMajorBit>, Derived>::ret TempType;
+      // thanks to shallow copies, we always eval to a tempary
+      Derived temp(other.rows(), other.cols());
+
+      temp.startFill(std::max(this->rows(),this->cols())*2);
+      for (int j=0; j<outerSize; ++j)
+      {
+        for (typename OtherDerived::InnerIterator it(other.derived(), j); it; ++it)
+        {
+          Scalar v = it.value();
+          if (v!=Scalar(0))
+          {
+            if (OtherDerived::Flags & RowMajorBit) temp.fill(j,it.index()) = v;
+            else temp.fill(it.index(),j) = v;
+          }
+        }
+      }
+      temp.endFill();
+
+      derived() = temp.markAsRValue();
+    }
+
+
+    template<typename OtherDerived>
+    inline Derived& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+//       std::cout << typeid(OtherDerived).name() << "\n";
+//       std::cout << Flags << " " << OtherDerived::Flags << "\n";
+      const bool transpose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit);
+//       std::cout << "eval transpose = " << transpose << "\n";
+      const int outerSize = (int(OtherDerived::Flags) & RowMajorBit) ? other.rows() : other.cols();
+      if ((!transpose) && other.isRValue())
+      {
+        // eval without temporary
+        derived().resize(other.rows(), other.cols());
+        derived().startFill(std::max(this->rows(),this->cols())*2);
+        for (int j=0; j<outerSize; ++j)
+        {
+          for (typename OtherDerived::InnerIterator it(other.derived(), j); it; ++it)
+          {
+            Scalar v = it.value();
+            if (v!=Scalar(0))
+            {
+              if (IsRowMajor) derived().fill(j,it.index()) = v;
+              else derived().fill(it.index(),j) = v;
+            }
+          }
+        }
+        derived().endFill();
+      }
+      else
+      {
+        assignGeneric(other.derived());
+      }
+      return derived();
+    }
+
+    template<typename Lhs, typename Rhs>
+    inline Derived& operator=(const SparseProduct<Lhs,Rhs,SparseTimeSparseProduct>& product);
+
+    friend std::ostream & operator << (std::ostream & s, const SparseMatrixBase& m)
+    {
+      if (Flags&RowMajorBit)
+      {
+        for (int row=0; row<m.outerSize(); ++row)
+        {
+          int col = 0;
+          for (typename Derived::InnerIterator it(m.derived(), row); it; ++it)
+          {
+            for ( ; col<it.index(); ++col)
+              s << "0 ";
+            s << it.value() << " ";
+            ++col;
+          }
+          for ( ; col<m.cols(); ++col)
+            s << "0 ";
+          s << std::endl;
+        }
+      }
+      else
+      {
+        if (m.cols() == 1) {
+          int row = 0;
+          for (typename Derived::InnerIterator it(m.derived(), 0); it; ++it)
+          {
+            for ( ; row<it.index(); ++row)
+              s << "0" << std::endl;
+            s << it.value() << std::endl;
+            ++row;
+          }
+          for ( ; row<m.rows(); ++row)
+            s << "0" << std::endl;
+        }
+        else
+        {
+          SparseMatrix<Scalar, RowMajorBit> trans = m.derived();
+          s << trans;
+        }
+      }
+      return s;
+    }
+
+    const SparseCwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived> operator-() const;
+
+    template<typename OtherDerived>
+    const SparseCwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+    operator+(const SparseMatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    const SparseCwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
+    operator-(const SparseMatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    Derived& operator+=(const SparseMatrixBase<OtherDerived>& other);
+    template<typename OtherDerived>
+    Derived& operator-=(const SparseMatrixBase<OtherDerived>& other);
+
+//     template<typename Lhs,typename Rhs>
+//     Derived& operator+=(const Flagged<Product<Lhs,Rhs,CacheFriendlyProduct>, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other);
+
+    Derived& operator*=(const Scalar& other);
+    Derived& operator/=(const Scalar& other);
+
+    const SparseCwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<Derived>::Scalar>, Derived>
+    operator*(const Scalar& scalar) const;
+    const SparseCwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
+    operator/(const Scalar& scalar) const;
+
+    inline friend const SparseCwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<Derived>::Scalar>, Derived>
+    operator*(const Scalar& scalar, const SparseMatrixBase& matrix)
+    { return matrix*scalar; }
+
+
+    template<typename OtherDerived>
+    const typename SparseProductReturnType<Derived,OtherDerived>::Type
+    operator*(const SparseMatrixBase<OtherDerived> &other) const;
+    
+    // dense * sparse (return a dense object)
+    template<typename OtherDerived> friend 
+    const typename SparseProductReturnType<OtherDerived,Derived>::Type
+    operator*(const MatrixBase<OtherDerived>& lhs, const Derived& rhs)
+    { return typename SparseProductReturnType<OtherDerived,Derived>::Type(lhs.derived(),rhs); }
+    
+    template<typename OtherDerived>
+    const typename SparseProductReturnType<Derived,OtherDerived>::Type
+    operator*(const MatrixBase<OtherDerived> &other) const;
+
+    template<typename OtherDerived>
+    Derived& operator*=(const SparseMatrixBase<OtherDerived>& other);
+
+    template<typename OtherDerived>
+    typename ei_plain_matrix_type_column_major<OtherDerived>::type
+    solveTriangular(const MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived>
+    void solveTriangularInPlace(MatrixBase<OtherDerived>& other) const;
+
+    template<typename OtherDerived> Scalar dot(const MatrixBase<OtherDerived>& other) const;
+    template<typename OtherDerived> Scalar dot(const SparseMatrixBase<OtherDerived>& other) const;
+    RealScalar squaredNorm() const;
+    RealScalar norm()  const;
+//     const PlainMatrixType normalized() const;
+//     void normalize();
+
+    SparseTranspose<Derived> transpose() { return derived(); }
+    const SparseTranspose<Derived> transpose() const { return derived(); }
+    // void transposeInPlace();
+    const AdjointReturnType adjoint() const { return conjugate()/*.nestByValue()*/; }
+
+    // sub-vector
+    SparseInnerVectorSet<Derived,1> row(int i);
+    const SparseInnerVectorSet<Derived,1> row(int i) const;
+    SparseInnerVectorSet<Derived,1> col(int j);
+    const SparseInnerVectorSet<Derived,1> col(int j) const;
+    SparseInnerVectorSet<Derived,1> innerVector(int outer);
+    const SparseInnerVectorSet<Derived,1> innerVector(int outer) const;
+    
+    // set of sub-vectors
+    SparseInnerVectorSet<Derived,Dynamic> subrows(int start, int size);
+    const SparseInnerVectorSet<Derived,Dynamic> subrows(int start, int size) const;
+    SparseInnerVectorSet<Derived,Dynamic> subcols(int start, int size);
+    const SparseInnerVectorSet<Derived,Dynamic> subcols(int start, int size) const;
+    SparseInnerVectorSet<Derived,Dynamic> innerVectors(int outerStart, int outerSize);
+    const SparseInnerVectorSet<Derived,Dynamic> innerVectors(int outerStart, int outerSize) const;
+
+//     typename BlockReturnType<Derived>::Type block(int startRow, int startCol, int blockRows, int blockCols);
+//     const typename BlockReturnType<Derived>::Type
+//     block(int startRow, int startCol, int blockRows, int blockCols) const;
+//
+//     typename BlockReturnType<Derived>::SubVectorType segment(int start, int size);
+//     const typename BlockReturnType<Derived>::SubVectorType segment(int start, int size) const;
+//
+//     typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size);
+//     const typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size) const;
+//
+//     typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size);
+//     const typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size) const;
+//
+//     typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols);
+//     const typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols) const;
+//
+//     template<int BlockRows, int BlockCols>
+//     typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol);
+//     template<int BlockRows, int BlockCols>
+//     const typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol) const;
+
+//     template<int CRows, int CCols>
+//     typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type);
+//     template<int CRows, int CCols>
+//     const typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type) const;
+
+//     template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType start(void);
+//     template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType start() const;
+
+//     template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType end();
+//     template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType end() const;
+
+//     template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType segment(int start);
+//     template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType segment(int start) const;
+
+//     DiagonalCoeffs<Derived> diagonal();
+//     const DiagonalCoeffs<Derived> diagonal() const;
+
+//     template<unsigned int Mode> Part<Derived, Mode> part();
+//     template<unsigned int Mode> const Part<Derived, Mode> part() const;
+
+
+//     static const ConstantReturnType Constant(int rows, int cols, const Scalar& value);
+//     static const ConstantReturnType Constant(int size, const Scalar& value);
+//     static const ConstantReturnType Constant(const Scalar& value);
+
+//     template<typename CustomNullaryOp>
+//     static const CwiseNullaryOp<CustomNullaryOp, Derived> NullaryExpr(int rows, int cols, const CustomNullaryOp& func);
+//     template<typename CustomNullaryOp>
+//     static const CwiseNullaryOp<CustomNullaryOp, Derived> NullaryExpr(int size, const CustomNullaryOp& func);
+//     template<typename CustomNullaryOp>
+//     static const CwiseNullaryOp<CustomNullaryOp, Derived> NullaryExpr(const CustomNullaryOp& func);
+
+//     static const ConstantReturnType Zero(int rows, int cols);
+//     static const ConstantReturnType Zero(int size);
+//     static const ConstantReturnType Zero();
+//     static const ConstantReturnType Ones(int rows, int cols);
+//     static const ConstantReturnType Ones(int size);
+//     static const ConstantReturnType Ones();
+//     static const IdentityReturnType Identity();
+//     static const IdentityReturnType Identity(int rows, int cols);
+//     static const BasisReturnType Unit(int size, int i);
+//     static const BasisReturnType Unit(int i);
+//     static const BasisReturnType UnitX();
+//     static const BasisReturnType UnitY();
+//     static const BasisReturnType UnitZ();
+//     static const BasisReturnType UnitW();
+
+//     const DiagonalMatrix<Derived> asDiagonal() const;
+
+//     Derived& setConstant(const Scalar& value);
+//     Derived& setZero();
+//     Derived& setOnes();
+//     Derived& setRandom();
+//     Derived& setIdentity();
+
+      Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> toDense() const
+      {
+        Matrix<Scalar,RowsAtCompileTime,ColsAtCompileTime> res(rows(),cols());
+        res.setZero();
+        for (int j=0; j<outerSize(); ++j)
+        {
+          for (typename Derived::InnerIterator i(derived(),j); i; ++i)
+            if(IsRowMajor)
+              res.coeffRef(j,i.index()) = i.value();
+            else
+              res.coeffRef(i.index(),j) = i.value();
+        }
+        return res;
+      }
+
+    template<typename OtherDerived>
+    bool isApprox(const SparseMatrixBase<OtherDerived>& other,
+                  RealScalar prec = precision<Scalar>()) const
+    { return toDense().isApprox(other.toDense(),prec); }
+
+    template<typename OtherDerived>
+    bool isApprox(const MatrixBase<OtherDerived>& other,
+                  RealScalar prec = precision<Scalar>()) const
+    { return toDense().isApprox(other,prec); }
+//     bool isMuchSmallerThan(const RealScalar& other,
+//                            RealScalar prec = precision<Scalar>()) const;
+//     template<typename OtherDerived>
+//     bool isMuchSmallerThan(const MatrixBase<OtherDerived>& other,
+//                            RealScalar prec = precision<Scalar>()) const;
+
+//     bool isApproxToConstant(const Scalar& value, RealScalar prec = precision<Scalar>()) const;
+//     bool isZero(RealScalar prec = precision<Scalar>()) const;
+//     bool isOnes(RealScalar prec = precision<Scalar>()) const;
+//     bool isIdentity(RealScalar prec = precision<Scalar>()) const;
+//     bool isDiagonal(RealScalar prec = precision<Scalar>()) const;
+
+//     bool isUpperTriangular(RealScalar prec = precision<Scalar>()) const;
+//     bool isLowerTriangular(RealScalar prec = precision<Scalar>()) const;
+
+//     template<typename OtherDerived>
+//     bool isOrthogonal(const MatrixBase<OtherDerived>& other,
+//                       RealScalar prec = precision<Scalar>()) const;
+//     bool isUnitary(RealScalar prec = precision<Scalar>()) const;
+
+//     template<typename OtherDerived>
+//     inline bool operator==(const MatrixBase<OtherDerived>& other) const
+//     { return (cwise() == other).all(); }
+
+//     template<typename OtherDerived>
+//     inline bool operator!=(const MatrixBase<OtherDerived>& other) const
+//     { return (cwise() != other).any(); }
+
+
+    template<typename NewType>
+    const SparseCwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived> cast() const;
+
+    /** \returns the matrix or vector obtained by evaluating this expression.
+      *
+      * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
+      * a const reference, in order to avoid a useless copy.
+      */
+    EIGEN_STRONG_INLINE const typename ei_eval<Derived>::type eval() const
+    { return typename ei_eval<Derived>::type(derived()); }
+
+//     template<typename OtherDerived>
+//     void swap(const MatrixBase<OtherDerived>& other);
+
+    template<unsigned int Added>
+    const SparseFlagged<Derived, Added, 0> marked() const;
+//     const Flagged<Derived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit> lazy() const;
+
+    /** \returns number of elements to skip to pass from one row (resp. column) to another
+      * for a row-major (resp. column-major) matrix.
+      * Combined with coeffRef() and the \ref flags flags, it allows a direct access to the data
+      * of the underlying matrix.
+      */
+//     inline int stride(void) const { return derived().stride(); }
+
+//     inline const NestByValue<Derived> nestByValue() const;
+
+
+    ConjugateReturnType conjugate() const;
+    const RealReturnType real() const;
+    const ImagReturnType imag() const;
+
+    template<typename CustomUnaryOp>
+    const SparseCwiseUnaryOp<CustomUnaryOp, Derived> unaryExpr(const CustomUnaryOp& func = CustomUnaryOp()) const;
+
+//     template<typename CustomBinaryOp, typename OtherDerived>
+//     const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
+//     binaryExpr(const MatrixBase<OtherDerived> &other, const CustomBinaryOp& func = CustomBinaryOp()) const;
+
+
+    Scalar sum() const;
+//     Scalar trace() const;
+
+//     typename ei_traits<Derived>::Scalar minCoeff() const;
+//     typename ei_traits<Derived>::Scalar maxCoeff() const;
+
+//     typename ei_traits<Derived>::Scalar minCoeff(int* row, int* col = 0) const;
+//     typename ei_traits<Derived>::Scalar maxCoeff(int* row, int* col = 0) const;
+
+//     template<typename BinaryOp>
+//     typename ei_result_of<BinaryOp(typename ei_traits<Derived>::Scalar)>::type
+//     redux(const BinaryOp& func) const;
+
+//     template<typename Visitor>
+//     void visit(Visitor& func) const;
+
+
+    const SparseCwise<Derived> cwise() const;
+    SparseCwise<Derived> cwise();
+
+//     inline const WithFormat<Derived> format(const IOFormat& fmt) const;
+
+/////////// Array module ///////////
+    /*
+    bool all(void) const;
+    bool any(void) const;
+
+    const PartialRedux<Derived,Horizontal> rowwise() const;
+    const PartialRedux<Derived,Vertical> colwise() const;
+
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int rows, int cols);
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int size);
+    static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random();
+
+    template<typename ThenDerived,typename ElseDerived>
+    const Select<Derived,ThenDerived,ElseDerived>
+    select(const MatrixBase<ThenDerived>& thenMatrix,
+           const MatrixBase<ElseDerived>& elseMatrix) const;
+
+    template<typename ThenDerived>
+    inline const Select<Derived,ThenDerived, NestByValue<typename ThenDerived::ConstantReturnType> >
+    select(const MatrixBase<ThenDerived>& thenMatrix, typename ThenDerived::Scalar elseScalar) const;
+
+    template<typename ElseDerived>
+    inline const Select<Derived, NestByValue<typename ElseDerived::ConstantReturnType>, ElseDerived >
+    select(typename ElseDerived::Scalar thenScalar, const MatrixBase<ElseDerived>& elseMatrix) const;
+
+    template<int p> RealScalar lpNorm() const;
+    */
+
+
+//     template<typename OtherDerived>
+//     Scalar dot(const MatrixBase<OtherDerived>& other) const
+//     {
+//       EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+//       EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
+//       EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
+//         YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
+//
+//       ei_assert(derived().size() == other.size());
+//       // short version, but the assembly looks more complicated because
+//       // of the CwiseBinaryOp iterator complexity
+//       // return res = (derived().cwise() * other.derived().conjugate()).sum();
+//
+//       // optimized, generic version
+//       typename Derived::InnerIterator i(derived(),0);
+//       typename OtherDerived::InnerIterator j(other.derived(),0);
+//       Scalar res = 0;
+//       while (i && j)
+//       {
+//         if (i.index()==j.index())
+//         {
+// //           std::cerr << i.value() << " * " << j.value() << "\n";
+//           res += i.value() * ei_conj(j.value());
+//           ++i; ++j;
+//         }
+//         else if (i.index()<j.index())
+//           ++i;
+//         else
+//           ++j;
+//       }
+//       return res;
+//     }
+//
+//     Scalar sum() const
+//     {
+//       Scalar res = 0;
+//       for (typename Derived::InnerIterator iter(*this,0); iter; ++iter)
+//       {
+//         res += iter.value();
+//       }
+//       return res;
+//     }
+
+    #ifdef EIGEN_TAUCS_SUPPORT
+    taucs_ccs_matrix asTaucsMatrix();
+    #endif
+
+    #ifdef EIGEN_CHOLMOD_SUPPORT
+    cholmod_sparse asCholmodMatrix();
+    #endif
+
+    #ifdef EIGEN_SUPERLU_SUPPORT
+    SluMatrix asSluMatrix();
+    #endif
+
+  protected:
+
+    bool m_isRValue;
+};
+
+#endif // EIGEN_SPARSEMATRIXBASE_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseProduct.h b/extern/Eigen2/Eigen/src/Sparse/SparseProduct.h
new file mode 100644
index 00000000000..c98a71e993b
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseProduct.h
@@ -0,0 +1,415 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEPRODUCT_H
+#define EIGEN_SPARSEPRODUCT_H
+
+template<typename Lhs, typename Rhs> struct ei_sparse_product_mode
+{
+  enum {
+
+    value = ((Lhs::Flags&Diagonal)==Diagonal || (Rhs::Flags&Diagonal)==Diagonal)
+          ? DiagonalProduct
+          :  (Rhs::Flags&Lhs::Flags&SparseBit)==SparseBit
+          ? SparseTimeSparseProduct
+          : (Lhs::Flags&SparseBit)==SparseBit
+          ? SparseTimeDenseProduct
+          : DenseTimeSparseProduct };
+};
+
+template<typename Lhs, typename Rhs, int ProductMode>
+struct SparseProductReturnType
+{
+  typedef const typename ei_nested<Lhs,Rhs::RowsAtCompileTime>::type LhsNested;
+  typedef const typename ei_nested<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+
+  typedef SparseProduct<LhsNested, RhsNested, ProductMode> Type;
+};
+
+template<typename Lhs, typename Rhs>
+struct SparseProductReturnType<Lhs,Rhs,DiagonalProduct>
+{
+  typedef const typename ei_nested<Lhs,Rhs::RowsAtCompileTime>::type LhsNested;
+  typedef const typename ei_nested<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
+
+  typedef SparseDiagonalProduct<LhsNested, RhsNested> Type;
+};
+
+// sparse product return type specialization
+template<typename Lhs, typename Rhs>
+struct SparseProductReturnType<Lhs,Rhs,SparseTimeSparseProduct>
+{
+  typedef typename ei_traits<Lhs>::Scalar Scalar;
+  enum {
+    LhsRowMajor = ei_traits<Lhs>::Flags & RowMajorBit,
+    RhsRowMajor = ei_traits<Rhs>::Flags & RowMajorBit,
+    TransposeRhs = (!LhsRowMajor) && RhsRowMajor,
+    TransposeLhs = LhsRowMajor && (!RhsRowMajor)
+  };
+
+  // FIXME if we transpose let's evaluate to a LinkedVectorMatrix since it is the
+  // type of the temporary to perform the transpose op
+  typedef typename ei_meta_if<TransposeLhs,
+    SparseMatrix<Scalar,0>,
+    const typename ei_nested<Lhs,Rhs::RowsAtCompileTime>::type>::ret LhsNested;
+
+  typedef typename ei_meta_if<TransposeRhs,
+    SparseMatrix<Scalar,0>,
+    const typename ei_nested<Rhs,Lhs::RowsAtCompileTime>::type>::ret RhsNested;
+
+  typedef SparseProduct<LhsNested, RhsNested, SparseTimeSparseProduct> Type;
+};
+
+template<typename LhsNested, typename RhsNested, int ProductMode>
+struct ei_traits<SparseProduct<LhsNested, RhsNested, ProductMode> >
+{
+  // clean the nested types:
+  typedef typename ei_cleantype<LhsNested>::type _LhsNested;
+  typedef typename ei_cleantype<RhsNested>::type _RhsNested;
+  typedef typename _LhsNested::Scalar Scalar;
+
+  enum {
+    LhsCoeffReadCost = _LhsNested::CoeffReadCost,
+    RhsCoeffReadCost = _RhsNested::CoeffReadCost,
+    LhsFlags = _LhsNested::Flags,
+    RhsFlags = _RhsNested::Flags,
+
+    RowsAtCompileTime = _LhsNested::RowsAtCompileTime,
+    ColsAtCompileTime = _RhsNested::ColsAtCompileTime,
+    InnerSize = EIGEN_ENUM_MIN(_LhsNested::ColsAtCompileTime, _RhsNested::RowsAtCompileTime),
+
+    MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime,
+    MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime,
+
+//     LhsIsRowMajor = (LhsFlags & RowMajorBit)==RowMajorBit,
+//     RhsIsRowMajor = (RhsFlags & RowMajorBit)==RowMajorBit,
+
+    EvalToRowMajor = (RhsFlags & LhsFlags & RowMajorBit),
+    ResultIsSparse = ProductMode==SparseTimeSparseProduct || ProductMode==DiagonalProduct,
+
+    RemovedBits = ~( (EvalToRowMajor ? 0 : RowMajorBit) | (ResultIsSparse ? 0 : SparseBit) ),
+
+    Flags = (int(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits)
+          | EvalBeforeAssigningBit
+          | EvalBeforeNestingBit,
+
+    CoeffReadCost = Dynamic
+  };
+
+  typedef typename ei_meta_if<ResultIsSparse,
+    SparseMatrixBase<SparseProduct<LhsNested, RhsNested, ProductMode> >,
+    MatrixBase<SparseProduct<LhsNested, RhsNested, ProductMode> > >::ret Base;
+};
+
+template<typename LhsNested, typename RhsNested, int ProductMode>
+class SparseProduct : ei_no_assignment_operator,
+  public ei_traits<SparseProduct<LhsNested, RhsNested, ProductMode> >::Base
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(SparseProduct)
+
+  private:
+
+    typedef typename ei_traits<SparseProduct>::_LhsNested _LhsNested;
+    typedef typename ei_traits<SparseProduct>::_RhsNested _RhsNested;
+
+  public:
+
+    template<typename Lhs, typename Rhs>
+    EIGEN_STRONG_INLINE SparseProduct(const Lhs& lhs, const Rhs& rhs)
+      : m_lhs(lhs), m_rhs(rhs)
+    {
+      ei_assert(lhs.cols() == rhs.rows());
+
+      enum {
+        ProductIsValid = _LhsNested::ColsAtCompileTime==Dynamic
+                      || _RhsNested::RowsAtCompileTime==Dynamic
+                      || int(_LhsNested::ColsAtCompileTime)==int(_RhsNested::RowsAtCompileTime),
+        AreVectors = _LhsNested::IsVectorAtCompileTime && _RhsNested::IsVectorAtCompileTime,
+        SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(_LhsNested,_RhsNested)
+      };
+      // note to the lost user:
+      //    * for a dot product use: v1.dot(v2)
+      //    * for a coeff-wise product use: v1.cwise()*v2
+      EIGEN_STATIC_ASSERT(ProductIsValid || !(AreVectors && SameSizes),
+        INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
+      EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors),
+        INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
+      EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT)
+    }
+
+    EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
+    EIGEN_STRONG_INLINE int cols() const { return m_rhs.cols(); }
+
+    EIGEN_STRONG_INLINE const _LhsNested& lhs() const { return m_lhs; }
+    EIGEN_STRONG_INLINE const _RhsNested& rhs() const { return m_rhs; }
+
+  protected:
+    LhsNested m_lhs;
+    RhsNested m_rhs;
+};
+
+// perform a pseudo in-place sparse * sparse product assuming all matrices are col major
+template<typename Lhs, typename Rhs, typename ResultType>
+static void ei_sparse_product_impl(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+{
+  typedef typename ei_traits<typename ei_cleantype<Lhs>::type>::Scalar Scalar;
+
+  // make sure to call innerSize/outerSize since we fake the storage order.
+  int rows = lhs.innerSize();
+  int cols = rhs.outerSize();
+  //int size = lhs.outerSize();
+  ei_assert(lhs.outerSize() == rhs.innerSize());
+
+  // allocate a temporary buffer
+  AmbiVector<Scalar> tempVector(rows);
+
+  // estimate the number of non zero entries
+  float ratioLhs = float(lhs.nonZeros())/(float(lhs.rows())*float(lhs.cols()));
+  float avgNnzPerRhsColumn = float(rhs.nonZeros())/float(cols);
+  float ratioRes = std::min(ratioLhs * avgNnzPerRhsColumn, 1.f);
+
+  res.resize(rows, cols);
+  res.startFill(int(ratioRes*rows*cols));
+  for (int j=0; j<cols; ++j)
+  {
+    // let's do a more accurate determination of the nnz ratio for the current column j of res
+    //float ratioColRes = std::min(ratioLhs * rhs.innerNonZeros(j), 1.f);
+    // FIXME find a nice way to get the number of nonzeros of a sub matrix (here an inner vector)
+    float ratioColRes = ratioRes;
+    tempVector.init(ratioColRes);
+    tempVector.setZero();
+    for (typename Rhs::InnerIterator rhsIt(rhs, j); rhsIt; ++rhsIt)
+    {
+      // FIXME should be written like this: tmp += rhsIt.value() * lhs.col(rhsIt.index())
+      tempVector.restart();
+      Scalar x = rhsIt.value();
+      for (typename Lhs::InnerIterator lhsIt(lhs, rhsIt.index()); lhsIt; ++lhsIt)
+      {
+        tempVector.coeffRef(lhsIt.index()) += lhsIt.value() * x;
+      }
+    }
+    for (typename AmbiVector<Scalar>::Iterator it(tempVector); it; ++it)
+      if (ResultType::Flags&RowMajorBit)
+        res.fill(j,it.index()) = it.value();
+      else
+        res.fill(it.index(), j) = it.value();
+  }
+  res.endFill();
+}
+
+template<typename Lhs, typename Rhs, typename ResultType,
+  int LhsStorageOrder = ei_traits<Lhs>::Flags&RowMajorBit,
+  int RhsStorageOrder = ei_traits<Rhs>::Flags&RowMajorBit,
+  int ResStorageOrder = ei_traits<ResultType>::Flags&RowMajorBit>
+struct ei_sparse_product_selector;
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct ei_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,ColMajor>
+{
+  typedef typename ei_traits<typename ei_cleantype<Lhs>::type>::Scalar Scalar;
+
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    typename ei_cleantype<ResultType>::type _res(res.rows(), res.cols());
+    ei_sparse_product_impl<Lhs,Rhs,ResultType>(lhs, rhs, _res);
+    res.swap(_res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct ei_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,ColMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    // we need a col-major matrix to hold the result
+    typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
+    SparseTemporaryType _res(res.rows(), res.cols());
+    ei_sparse_product_impl<Lhs,Rhs,SparseTemporaryType>(lhs, rhs, _res);
+    res = _res;
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct ei_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,RowMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    // let's transpose the product to get a column x column product
+    typename ei_cleantype<ResultType>::type _res(res.rows(), res.cols());
+    ei_sparse_product_impl<Rhs,Lhs,ResultType>(rhs, lhs, _res);
+    res.swap(_res);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename ResultType>
+struct ei_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,RowMajor,ColMajor>
+{
+  static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+  {
+    // let's transpose the product to get a column x column product
+    typedef SparseMatrix<typename ResultType::Scalar> SparseTemporaryType;
+    SparseTemporaryType _res(res.cols(), res.rows());
+    ei_sparse_product_impl<Rhs,Lhs,SparseTemporaryType>(rhs, lhs, _res);
+    res = _res.transpose();
+  }
+};
+
+// NOTE eventually let's transpose one argument even in this case since it might be expensive if
+// the result is not dense.
+// template<typename Lhs, typename Rhs, typename ResultType, int ResStorageOrder>
+// struct ei_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,ColMajor,ResStorageOrder>
+// {
+//   static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
+//   {
+//     // trivial product as lhs.row/rhs.col dot products
+//     // loop over the preferred order of the result
+//   }
+// };
+
+// NOTE the 2 others cases (col row *) must never occurs since they are caught
+// by ProductReturnType which transform it to (col col *) by evaluating rhs.
+
+
+// template<typename Derived>
+// template<typename Lhs, typename Rhs>
+// inline Derived& SparseMatrixBase<Derived>::lazyAssign(const SparseProduct<Lhs,Rhs>& product)
+// {
+// //   std::cout << "sparse product to dense\n";
+//   ei_sparse_product_selector<
+//     typename ei_cleantype<Lhs>::type,
+//     typename ei_cleantype<Rhs>::type,
+//     typename ei_cleantype<Derived>::type>::run(product.lhs(),product.rhs(),derived());
+//   return derived();
+// }
+
+// sparse = sparse * sparse
+template<typename Derived>
+template<typename Lhs, typename Rhs>
+inline Derived& SparseMatrixBase<Derived>::operator=(const SparseProduct<Lhs,Rhs,SparseTimeSparseProduct>& product)
+{
+  ei_sparse_product_selector<
+    typename ei_cleantype<Lhs>::type,
+    typename ei_cleantype<Rhs>::type,
+    Derived>::run(product.lhs(),product.rhs(),derived());
+  return derived();
+}
+
+// dense = sparse * dense
+// template<typename Derived>
+// template<typename Lhs, typename Rhs>
+// Derived& MatrixBase<Derived>::lazyAssign(const SparseProduct<Lhs,Rhs,SparseTimeDenseProduct>& product)
+// {
+//   typedef typename ei_cleantype<Lhs>::type _Lhs;
+//   typedef typename _Lhs::InnerIterator LhsInnerIterator;
+//   enum { LhsIsRowMajor = (_Lhs::Flags&RowMajorBit)==RowMajorBit };
+//   derived().setZero();
+//   for (int j=0; j<product.lhs().outerSize(); ++j)
+//     for (LhsInnerIterator i(product.lhs(),j); i; ++i)
+//       derived().row(LhsIsRowMajor ? j : i.index()) += i.value() * product.rhs().row(LhsIsRowMajor ? i.index() : j);
+//   return derived();
+// }
+
+template<typename Derived>
+template<typename Lhs, typename Rhs>
+Derived& MatrixBase<Derived>::lazyAssign(const SparseProduct<Lhs,Rhs,SparseTimeDenseProduct>& product)
+{
+  typedef typename ei_cleantype<Lhs>::type _Lhs;
+  typedef typename ei_cleantype<Rhs>::type _Rhs;
+  typedef typename _Lhs::InnerIterator LhsInnerIterator;
+  enum {
+    LhsIsRowMajor = (_Lhs::Flags&RowMajorBit)==RowMajorBit,
+    LhsIsSelfAdjoint = (_Lhs::Flags&SelfAdjointBit)==SelfAdjointBit,
+    ProcessFirstHalf = LhsIsSelfAdjoint
+      && (   ((_Lhs::Flags&(UpperTriangularBit|LowerTriangularBit))==0)
+          || ( (_Lhs::Flags&UpperTriangularBit) && !LhsIsRowMajor)
+          || ( (_Lhs::Flags&LowerTriangularBit) && LhsIsRowMajor) ),
+    ProcessSecondHalf = LhsIsSelfAdjoint && (!ProcessFirstHalf)
+  };
+  derived().setZero();
+  for (int j=0; j<product.lhs().outerSize(); ++j)
+  {
+    LhsInnerIterator i(product.lhs(),j);
+    if (ProcessSecondHalf && i && (i.index()==j))
+    {
+      derived().row(j) += i.value() * product.rhs().row(j);
+      ++i;
+    }
+    Block<Derived,1,Derived::ColsAtCompileTime> res(derived().row(LhsIsRowMajor ? j : 0));
+    for (; (ProcessFirstHalf ? i && i.index() < j : i) ; ++i)
+    {
+      if (LhsIsSelfAdjoint)
+      {
+        int a = LhsIsRowMajor ? j : i.index();
+        int b = LhsIsRowMajor ? i.index() : j;
+        Scalar v = i.value();
+        derived().row(a) += (v) * product.rhs().row(b);
+        derived().row(b) += ei_conj(v) * product.rhs().row(a);
+      }
+      else if (LhsIsRowMajor)
+        res += i.value() * product.rhs().row(i.index());
+      else
+        derived().row(i.index()) += i.value() * product.rhs().row(j);
+    }
+    if (ProcessFirstHalf && i && (i.index()==j))
+      derived().row(j) += i.value() * product.rhs().row(j);
+  }
+  return derived();
+}
+
+// dense = dense * sparse
+template<typename Derived>
+template<typename Lhs, typename Rhs>
+Derived& MatrixBase<Derived>::lazyAssign(const SparseProduct<Lhs,Rhs,DenseTimeSparseProduct>& product)
+{
+  typedef typename ei_cleantype<Rhs>::type _Rhs;
+  typedef typename _Rhs::InnerIterator RhsInnerIterator;
+  enum { RhsIsRowMajor = (_Rhs::Flags&RowMajorBit)==RowMajorBit };
+  derived().setZero();
+  for (int j=0; j<product.rhs().outerSize(); ++j)
+    for (RhsInnerIterator i(product.rhs(),j); i; ++i)
+      derived().col(RhsIsRowMajor ? i.index() : j) += i.value() * product.lhs().col(RhsIsRowMajor ? j : i.index());
+  return derived();
+}
+
+// sparse * sparse
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const typename SparseProductReturnType<Derived,OtherDerived>::Type
+SparseMatrixBase<Derived>::operator*(const SparseMatrixBase<OtherDerived> &other) const
+{
+  return typename SparseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
+}
+
+// sparse * dense
+template<typename Derived>
+template<typename OtherDerived>
+EIGEN_STRONG_INLINE const typename SparseProductReturnType<Derived,OtherDerived>::Type
+SparseMatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
+{
+  return typename SparseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
+}
+
+#endif // EIGEN_SPARSEPRODUCT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseRedux.h b/extern/Eigen2/Eigen/src/Sparse/SparseRedux.h
new file mode 100644
index 00000000000..f0d3705488e
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseRedux.h
@@ -0,0 +1,40 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEREDUX_H
+#define EIGEN_SPARSEREDUX_H
+
+template<typename Derived>
+typename ei_traits<Derived>::Scalar
+SparseMatrixBase<Derived>::sum() const
+{
+  ei_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix");
+  Scalar res = 0;
+  for (int j=0; j<outerSize(); ++j)
+    for (typename Derived::InnerIterator iter(derived(),j); iter; ++iter)
+      res += iter.value();
+  return res;
+}
+
+#endif // EIGEN_SPARSEREDUX_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseTranspose.h b/extern/Eigen2/Eigen/src/Sparse/SparseTranspose.h
new file mode 100644
index 00000000000..89a14d70707
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseTranspose.h
@@ -0,0 +1,85 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSETRANSPOSE_H
+#define EIGEN_SPARSETRANSPOSE_H
+
+template<typename MatrixType>
+struct ei_traits<SparseTranspose<MatrixType> > : ei_traits<Transpose<MatrixType> >
+{};
+
+template<typename MatrixType> class SparseTranspose
+  : public SparseMatrixBase<SparseTranspose<MatrixType> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(SparseTranspose)
+
+    class InnerIterator;
+    class ReverseInnerIterator;
+
+    inline SparseTranspose(const MatrixType& matrix) : m_matrix(matrix) {}
+
+    //EIGEN_INHERIT_ASSIGNMENT_OPERATORS(SparseTranspose)
+
+    inline int rows() const { return m_matrix.cols(); }
+    inline int cols() const { return m_matrix.rows(); }
+    inline int nonZeros() const { return m_matrix.nonZeros(); }
+
+    // FIXME should be keep them ?
+    inline Scalar& coeffRef(int row, int col)
+    { return m_matrix.const_cast_derived().coeffRef(col, row); }
+
+    inline const Scalar coeff(int row, int col) const
+    { return m_matrix.coeff(col, row); }
+
+    inline const Scalar coeff(int index) const
+    { return m_matrix.coeff(index); }
+
+    inline Scalar& coeffRef(int index)
+    { return m_matrix.const_cast_derived().coeffRef(index); }
+
+  protected:
+    const typename MatrixType::Nested m_matrix;
+};
+
+template<typename MatrixType> class SparseTranspose<MatrixType>::InnerIterator : public MatrixType::InnerIterator
+{
+  public:
+
+    EIGEN_STRONG_INLINE InnerIterator(const SparseTranspose& trans, int outer)
+      : MatrixType::InnerIterator(trans.m_matrix, outer)
+    {}
+};
+
+template<typename MatrixType> class SparseTranspose<MatrixType>::ReverseInnerIterator : public MatrixType::ReverseInnerIterator
+{
+  public:
+
+    EIGEN_STRONG_INLINE ReverseInnerIterator(const SparseTranspose& xpr, int outer)
+      : MatrixType::ReverseInnerIterator(xpr.m_matrix, outer)
+    {}
+};
+
+#endif // EIGEN_SPARSETRANSPOSE_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseUtil.h b/extern/Eigen2/Eigen/src/Sparse/SparseUtil.h
new file mode 100644
index 00000000000..393cdda6ea2
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseUtil.h
@@ -0,0 +1,148 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEUTIL_H
+#define EIGEN_SPARSEUTIL_H
+
+#ifdef NDEBUG
+#define EIGEN_DBG_SPARSE(X)
+#else
+#define EIGEN_DBG_SPARSE(X) X
+#endif
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename OtherDerived> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Eigen::SparseMatrixBase<OtherDerived>& other) \
+{ \
+  return Base::operator Op(other.derived()); \
+} \
+EIGEN_STRONG_INLINE Derived& operator Op(const Derived& other) \
+{ \
+  return Base::operator Op(other); \
+}
+
+#define EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, Op) \
+template<typename Other> \
+EIGEN_STRONG_INLINE Derived& operator Op(const Other& scalar) \
+{ \
+  return Base::operator Op(scalar); \
+}
+
+#define EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, =) \
+EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, +=) \
+EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(Derived, -=) \
+EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, *=) \
+EIGEN_SPARSE_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Derived, /=)
+
+#define _EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(Derived, BaseClass) \
+typedef BaseClass Base; \
+typedef typename Eigen::ei_traits<Derived>::Scalar Scalar; \
+typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; \
+typedef typename Eigen::ei_nested<Derived>::type Nested; \
+enum { RowsAtCompileTime = Eigen::ei_traits<Derived>::RowsAtCompileTime, \
+       ColsAtCompileTime = Eigen::ei_traits<Derived>::ColsAtCompileTime, \
+       Flags = Eigen::ei_traits<Derived>::Flags, \
+       CoeffReadCost = Eigen::ei_traits<Derived>::CoeffReadCost, \
+       SizeAtCompileTime = Base::SizeAtCompileTime, \
+       IsVectorAtCompileTime = Base::IsVectorAtCompileTime };
+
+#define EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(Derived) \
+_EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(Derived, Eigen::SparseMatrixBase<Derived>)
+
+enum SparseBackend {
+  DefaultBackend,
+  Taucs,
+  Cholmod,
+  SuperLU,
+  UmfPack
+};
+
+// solver flags
+enum {
+  CompleteFactorization       = 0x0000,  // the default
+  IncompleteFactorization     = 0x0001,
+  MemoryEfficient             = 0x0002,
+
+  // For LLT Cholesky:
+  SupernodalMultifrontal      = 0x0010,
+  SupernodalLeftLooking       = 0x0020,
+
+  // Ordering methods:
+  NaturalOrdering             = 0x0100, // the default
+  MinimumDegree_AT_PLUS_A     = 0x0200,
+  MinimumDegree_ATA           = 0x0300,
+  ColApproxMinimumDegree      = 0x0400,
+  Metis                       = 0x0500,
+  Scotch                      = 0x0600,
+  Chaco                       = 0x0700,
+  OrderingMask                = 0x0f00
+};
+
+template<typename Derived> class SparseMatrixBase;
+template<typename _Scalar, int _Flags = 0> class SparseMatrix;
+template<typename _Scalar, int _Flags = 0> class DynamicSparseMatrix;
+template<typename _Scalar, int _Flags = 0> class SparseVector;
+template<typename _Scalar, int _Flags = 0> class MappedSparseMatrix;
+
+template<typename MatrixType>                            class SparseTranspose;
+template<typename MatrixType, int Size>                  class SparseInnerVectorSet;
+template<typename Derived>                               class SparseCwise;
+template<typename UnaryOp,   typename MatrixType>        class SparseCwiseUnaryOp;
+template<typename BinaryOp,  typename Lhs, typename Rhs> class SparseCwiseBinaryOp;
+template<typename ExpressionType,
+         unsigned int Added, unsigned int Removed>       class SparseFlagged;
+template<typename Lhs, typename Rhs>                     class SparseDiagonalProduct;
+
+template<typename Lhs, typename Rhs> struct ei_sparse_product_mode;
+template<typename Lhs, typename Rhs, int ProductMode = ei_sparse_product_mode<Lhs,Rhs>::value> struct SparseProductReturnType;
+
+const int CoherentAccessPattern  = 0x1;
+const int InnerRandomAccessPattern  = 0x2 | CoherentAccessPattern;
+const int OuterRandomAccessPattern  = 0x4 | CoherentAccessPattern;
+const int RandomAccessPattern       = 0x8 | OuterRandomAccessPattern | InnerRandomAccessPattern;
+
+// const int AccessPatternNotSupported = 0x0;
+// const int AccessPatternSupported    = 0x1;
+// 
+// template<typename MatrixType, int AccessPattern> struct ei_support_access_pattern
+// {
+//   enum { ret = (int(ei_traits<MatrixType>::SupportedAccessPatterns) & AccessPattern) == AccessPattern
+//              ? AccessPatternSupported
+//              : AccessPatternNotSupported
+//   };
+// };
+
+template<typename T> class ei_eval<T,IsSparse>
+{
+    typedef typename ei_traits<T>::Scalar _Scalar;
+    enum {
+          _Flags = ei_traits<T>::Flags
+    };
+
+  public:
+    typedef SparseMatrix<_Scalar, _Flags> type;
+};
+
+#endif // EIGEN_SPARSEUTIL_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SparseVector.h b/extern/Eigen2/Eigen/src/Sparse/SparseVector.h
new file mode 100644
index 00000000000..8e5a6efeda8
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SparseVector.h
@@ -0,0 +1,365 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSEVECTOR_H
+#define EIGEN_SPARSEVECTOR_H
+
+/** \class SparseVector
+  *
+  * \brief a sparse vector class
+  *
+  * \param _Scalar the scalar type, i.e. the type of the coefficients
+  *
+  * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme.
+  *
+  */
+template<typename _Scalar, int _Flags>
+struct ei_traits<SparseVector<_Scalar, _Flags> >
+{
+  typedef _Scalar Scalar;
+  enum {
+    IsColVector = _Flags & RowMajorBit ? 0 : 1,
+
+    RowsAtCompileTime = IsColVector ? Dynamic : 1,
+    ColsAtCompileTime = IsColVector ? 1 : Dynamic,
+    MaxRowsAtCompileTime = RowsAtCompileTime,
+    MaxColsAtCompileTime = ColsAtCompileTime,
+    Flags = SparseBit | _Flags,
+    CoeffReadCost = NumTraits<Scalar>::ReadCost,
+    SupportedAccessPatterns = InnerRandomAccessPattern
+  };
+};
+
+template<typename _Scalar, int _Flags>
+class SparseVector
+  : public SparseMatrixBase<SparseVector<_Scalar, _Flags> >
+{
+  public:
+    EIGEN_SPARSE_GENERIC_PUBLIC_INTERFACE(SparseVector)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, +=)
+    EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, -=)
+//     EIGEN_SPARSE_INHERIT_ASSIGNMENT_OPERATOR(SparseVector, =)
+
+  protected:
+  public:
+
+    typedef SparseMatrixBase<SparseVector> SparseBase;
+    enum { IsColVector = ei_traits<SparseVector>::IsColVector };
+
+    CompressedStorage<Scalar> m_data;
+    int m_size;
+    
+    CompressedStorage<Scalar>& _data() { return m_data; }
+    CompressedStorage<Scalar>& _data() const { return m_data; }
+
+  public:
+
+    EIGEN_STRONG_INLINE int rows() const { return IsColVector ? m_size : 1; }
+    EIGEN_STRONG_INLINE int cols() const { return IsColVector ? 1 : m_size; }
+    EIGEN_STRONG_INLINE int innerSize() const { return m_size; }
+    EIGEN_STRONG_INLINE int outerSize() const { return 1; }
+    EIGEN_STRONG_INLINE int innerNonZeros(int j) const { ei_assert(j==0); return m_size; }
+
+    EIGEN_STRONG_INLINE const Scalar* _valuePtr() const { return &m_data.value(0); }
+    EIGEN_STRONG_INLINE Scalar* _valuePtr() { return &m_data.value(0); }
+
+    EIGEN_STRONG_INLINE const int* _innerIndexPtr() const { return &m_data.index(0); }
+    EIGEN_STRONG_INLINE int* _innerIndexPtr() { return &m_data.index(0); }
+
+    inline Scalar coeff(int row, int col) const
+    {
+      ei_assert((IsColVector ? col : row)==0);
+      return coeff(IsColVector ? row : col);
+    }
+    inline Scalar coeff(int i) const { return m_data.at(i); }
+
+    inline Scalar& coeffRef(int row, int col)
+    {
+      ei_assert((IsColVector ? col : row)==0);
+      return coeff(IsColVector ? row : col);
+    }
+
+    /** \returns a reference to the coefficient value at given index \a i
+      * This operation involes a log(rho*size) binary search. If the coefficient does not
+      * exist yet, then a sorted insertion into a sequential buffer is performed.
+      * 
+      * This insertion might be very costly if the number of nonzeros above \a i is large.
+      */
+    inline Scalar& coeffRef(int i)
+    {
+      return m_data.atWithInsertion(i);
+    }
+
+  public:
+
+    class InnerIterator;
+
+    inline void setZero() { m_data.clear(); }
+
+    /** \returns the number of non zero coefficients */
+    inline int nonZeros() const  { return m_data.size(); }
+
+    /**
+      */
+    inline void reserve(int reserveSize) { m_data.reserve(reserveSize); }
+    
+    inline void startFill(int reserve)
+    {
+      setZero();
+      m_data.reserve(reserve);
+    }
+
+    /**
+      */
+    inline Scalar& fill(int r, int c)
+    {
+      ei_assert(r==0 || c==0);
+      return fill(IsColVector ? r : c);
+    }
+    
+    inline Scalar& fill(int i)
+    {
+      m_data.append(0, i);
+      return m_data.value(m_data.size()-1);
+    }
+    
+    inline Scalar& fillrand(int r, int c)
+    {
+      ei_assert(r==0 || c==0);
+      return fillrand(IsColVector ? r : c);
+    }
+
+    /** Like fill() but with random coordinates.
+      */
+    inline Scalar& fillrand(int i)
+    {
+      int startId = 0;
+      int id = m_data.size() - 1;
+      m_data.resize(id+2,1);
+
+      while ( (id >= startId) && (m_data.index(id) > i) )
+      {
+        m_data.index(id+1) = m_data.index(id);
+        m_data.value(id+1) = m_data.value(id);
+        --id;
+      }
+      m_data.index(id+1) = i;
+      m_data.value(id+1) = 0;
+      return m_data.value(id+1);
+    }
+    
+    inline void endFill() {}
+    
+    void prune(Scalar reference, RealScalar epsilon = precision<RealScalar>())
+    {
+      m_data.prune(reference,epsilon);
+    }
+    
+    void resize(int rows, int cols)
+    {
+      ei_assert(rows==1 || cols==1);
+      resize(IsColVector ? rows : cols);
+    }
+
+    void resize(int newSize)
+    {
+      m_size = newSize;
+      m_data.clear();
+    }
+
+    void resizeNonZeros(int size) { m_data.resize(size); }
+
+    inline SparseVector() : m_size(0) { resize(0); }
+
+    inline SparseVector(int size) : m_size(0) { resize(size); }
+    
+    inline SparseVector(int rows, int cols) : m_size(0) { resize(rows,cols); }
+
+    template<typename OtherDerived>
+    inline SparseVector(const MatrixBase<OtherDerived>& other)
+      : m_size(0)
+    {
+      *this = other.derived();
+    }
+    
+    template<typename OtherDerived>
+    inline SparseVector(const SparseMatrixBase<OtherDerived>& other)
+      : m_size(0)
+    {
+      *this = other.derived();
+    }
+
+    inline SparseVector(const SparseVector& other)
+      : m_size(0)
+    {
+      *this = other.derived();
+    }
+
+    inline void swap(SparseVector& other)
+    {
+      std::swap(m_size, other.m_size);
+      m_data.swap(other.m_data);
+    }
+
+    inline SparseVector& operator=(const SparseVector& other)
+    {
+      if (other.isRValue())
+      {
+        swap(other.const_cast_derived());
+      }
+      else
+      {
+        resize(other.size());
+        m_data = other.m_data;
+      }
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    inline SparseVector& operator=(const SparseMatrixBase<OtherDerived>& other)
+    {
+      return Base::operator=(other);
+    }
+    
+//       const bool needToTranspose = (Flags & RowMajorBit) != (OtherDerived::Flags & RowMajorBit);
+//       if (needToTranspose)
+//       {
+//         // two passes algorithm:
+//         //  1 - compute the number of coeffs per dest inner vector
+//         //  2 - do the actual copy/eval
+//         // Since each coeff of the rhs has to be evaluated twice, let's evauluate it if needed
+//         typedef typename ei_nested<OtherDerived,2>::type OtherCopy;
+//         OtherCopy otherCopy(other.derived());
+//         typedef typename ei_cleantype<OtherCopy>::type _OtherCopy;
+//
+//         resize(other.rows(), other.cols());
+//         Eigen::Map<VectorXi>(m_outerIndex,outerSize()).setZero();
+//         // pass 1
+//         // FIXME the above copy could be merged with that pass
+//         for (int j=0; j<otherCopy.outerSize(); ++j)
+//           for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
+//             ++m_outerIndex[it.index()];
+//
+//         // prefix sum
+//         int count = 0;
+//         VectorXi positions(outerSize());
+//         for (int j=0; j<outerSize(); ++j)
+//         {
+//           int tmp = m_outerIndex[j];
+//           m_outerIndex[j] = count;
+//           positions[j] = count;
+//           count += tmp;
+//         }
+//         m_outerIndex[outerSize()] = count;
+//         // alloc
+//         m_data.resize(count);
+//         // pass 2
+//         for (int j=0; j<otherCopy.outerSize(); ++j)
+//           for (typename _OtherCopy::InnerIterator it(otherCopy, j); it; ++it)
+//           {
+//             int pos = positions[it.index()]++;
+//             m_data.index(pos) = j;
+//             m_data.value(pos) = it.value();
+//           }
+//
+//         return *this;
+//       }
+//       else
+//       {
+//         // there is no special optimization
+//         return SparseMatrixBase<SparseMatrix>::operator=(other.derived());
+//       }
+//     }
+
+    friend std::ostream & operator << (std::ostream & s, const SparseVector& m)
+    {
+      for (unsigned int i=0; i<m.nonZeros(); ++i)
+        s << "(" << m.m_data.value(i) << "," << m.m_data.index(i) << ") ";
+      s << std::endl;
+      return s;
+    }
+
+    // this specialized version does not seems to be faster
+//     Scalar dot(const SparseVector& other) const
+//     {
+//       int i=0, j=0;
+//       Scalar res = 0;
+//       asm("#begindot");
+//       while (i<nonZeros() && j<other.nonZeros())
+//       {
+//         if (m_data.index(i)==other.m_data.index(j))
+//         {
+//           res += m_data.value(i) * ei_conj(other.m_data.value(j));
+//           ++i; ++j;
+//         }
+//         else if (m_data.index(i)<other.m_data.index(j))
+//           ++i;
+//         else
+//           ++j;
+//       }
+//       asm("#enddot");
+//       return res;
+//     }
+
+    /** Destructor */
+    inline ~SparseVector() {}
+};
+
+template<typename Scalar, int _Flags>
+class SparseVector<Scalar,_Flags>::InnerIterator
+{
+  public:
+    InnerIterator(const SparseVector& vec, int outer=0)
+      : m_data(vec.m_data), m_id(0), m_end(m_data.size())
+    {
+      ei_assert(outer==0);
+    }
+    
+    InnerIterator(const CompressedStorage<Scalar>& data)
+      : m_data(data), m_id(0), m_end(m_data.size())
+    {}
+
+    template<unsigned int Added, unsigned int Removed>
+    InnerIterator(const Flagged<SparseVector,Added,Removed>& vec, int outer)
+      : m_data(vec._expression().m_data), m_id(0), m_end(m_data.size())
+    {}
+
+    inline InnerIterator& operator++() { m_id++; return *this; }
+
+    inline Scalar value() const { return m_data.value(m_id); }
+    inline Scalar& valueRef() { return const_cast<Scalar&>(m_data.value(m_id)); }
+
+    inline int index() const { return m_data.index(m_id); }
+    inline int row() const { return IsColVector ? index() : 0; }
+    inline int col() const { return IsColVector ? 0 : index(); }
+
+    inline operator bool() const { return (m_id < m_end); }
+
+  protected:
+    const CompressedStorage<Scalar>& m_data;
+    int m_id;
+    const int m_end;
+};
+
+#endif // EIGEN_SPARSEVECTOR_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/SuperLUSupport.h b/extern/Eigen2/Eigen/src/Sparse/SuperLUSupport.h
new file mode 100644
index 00000000000..3c9a4fcced6
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/SuperLUSupport.h
@@ -0,0 +1,565 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SUPERLUSUPPORT_H
+#define EIGEN_SUPERLUSUPPORT_H
+
+// declaration of gssvx taken from GMM++
+#define DECL_GSSVX(NAMESPACE,FNAME,FLOATTYPE,KEYTYPE) \
+    inline float SuperLU_gssvx(superlu_options_t *options, SuperMatrix *A,  \
+         int *perm_c, int *perm_r, int *etree, char *equed,  \
+         FLOATTYPE *R, FLOATTYPE *C, SuperMatrix *L,         \
+         SuperMatrix *U, void *work, int lwork,              \
+         SuperMatrix *B, SuperMatrix *X,                     \
+         FLOATTYPE *recip_pivot_growth,                      \
+         FLOATTYPE *rcond, FLOATTYPE *ferr, FLOATTYPE *berr, \
+         SuperLUStat_t *stats, int *info, KEYTYPE) {         \
+    using namespace NAMESPACE; \
+    mem_usage_t mem_usage;                                    \
+    NAMESPACE::FNAME(options, A, perm_c, perm_r, etree, equed, R, C, L,  \
+         U, work, lwork, B, X, recip_pivot_growth, rcond,    \
+         ferr, berr, &mem_usage, stats, info);               \
+    return mem_usage.for_lu; /* bytes used by the factor storage */     \
+  }
+
+DECL_GSSVX(SuperLU_S,sgssvx,float,float)
+DECL_GSSVX(SuperLU_C,cgssvx,float,std::complex<float>)
+DECL_GSSVX(SuperLU_D,dgssvx,double,double)
+DECL_GSSVX(SuperLU_Z,zgssvx,double,std::complex<double>)
+
+template<typename MatrixType>
+struct SluMatrixMapHelper;
+
+/** \internal
+  *
+  * A wrapper class for SuperLU matrices. It supports only compressed sparse matrices
+  * and dense matrices. Supernodal and other fancy format are not supported by this wrapper.
+  *
+  * This wrapper class mainly aims to avoids the need of dynamic allocation of the storage structure.
+  */
+struct SluMatrix : SuperMatrix
+{
+  SluMatrix()
+  {
+    Store = &storage;
+  }
+
+  SluMatrix(const SluMatrix& other)
+    : SuperMatrix(other)
+  {
+    Store = &storage;
+    storage = other.storage;
+  }
+  
+  SluMatrix& operator=(const SluMatrix& other)
+  {
+    SuperMatrix::operator=(static_cast<const SuperMatrix&>(other));
+    Store = &storage;
+    storage = other.storage;
+    return *this;
+  }
+
+  struct
+  {
+    union {int nnz;int lda;};
+    void *values;
+    int *innerInd;
+    int *outerInd;
+  } storage;
+
+  void setStorageType(Stype_t t)
+  {
+    Stype = t;
+    if (t==SLU_NC || t==SLU_NR || t==SLU_DN)
+      Store = &storage;
+    else
+    {
+      ei_assert(false && "storage type not supported");
+      Store = 0;
+    }
+  }
+
+  template<typename Scalar>
+  void setScalarType()
+  {
+    if (ei_is_same_type<Scalar,float>::ret)
+      Dtype = SLU_S;
+    else if (ei_is_same_type<Scalar,double>::ret)
+      Dtype = SLU_D;
+    else if (ei_is_same_type<Scalar,std::complex<float> >::ret)
+      Dtype = SLU_C;
+    else if (ei_is_same_type<Scalar,std::complex<double> >::ret)
+      Dtype = SLU_Z;
+    else
+    {
+      ei_assert(false && "Scalar type not supported by SuperLU");
+    }
+  }
+
+  template<typename Scalar, int Rows, int Cols, int Options, int MRows, int MCols>
+  static SluMatrix Map(Matrix<Scalar,Rows,Cols,Options,MRows,MCols>& mat)
+  {
+    typedef Matrix<Scalar,Rows,Cols,Options,MRows,MCols> MatrixType;
+    ei_assert( ((Options&RowMajor)!=RowMajor) && "row-major dense matrices is not supported by SuperLU");
+    SluMatrix res;
+    res.setStorageType(SLU_DN);
+    res.setScalarType<Scalar>();
+    res.Mtype     = SLU_GE;
+
+    res.nrow      = mat.rows();
+    res.ncol      = mat.cols();
+
+    res.storage.lda       = mat.stride();
+    res.storage.values    = mat.data();
+    return res;
+  }
+
+  template<typename MatrixType>
+  static SluMatrix Map(SparseMatrixBase<MatrixType>& mat)
+  {
+    SluMatrix res;
+    if ((MatrixType::Flags&RowMajorBit)==RowMajorBit)
+    {
+      res.setStorageType(SLU_NR);
+      res.nrow      = mat.cols();
+      res.ncol      = mat.rows();
+    }
+    else
+    {
+      res.setStorageType(SLU_NC);
+      res.nrow      = mat.rows();
+      res.ncol      = mat.cols();
+    }
+
+    res.Mtype     = SLU_GE;
+
+    res.storage.nnz       = mat.nonZeros();
+    res.storage.values    = mat.derived()._valuePtr();
+    res.storage.innerInd  = mat.derived()._innerIndexPtr();
+    res.storage.outerInd  = mat.derived()._outerIndexPtr();
+
+    res.setScalarType<typename MatrixType::Scalar>();
+
+    // FIXME the following is not very accurate
+    if (MatrixType::Flags & UpperTriangular)
+      res.Mtype = SLU_TRU;
+    if (MatrixType::Flags & LowerTriangular)
+      res.Mtype = SLU_TRL;
+    if (MatrixType::Flags & SelfAdjoint)
+      ei_assert(false && "SelfAdjoint matrix shape not supported by SuperLU");
+    return res;
+  }
+};
+
+template<typename Scalar, int Rows, int Cols, int Options, int MRows, int MCols>
+struct SluMatrixMapHelper<Matrix<Scalar,Rows,Cols,Options,MRows,MCols> >
+{
+  typedef Matrix<Scalar,Rows,Cols,Options,MRows,MCols> MatrixType;
+  static void run(MatrixType& mat, SluMatrix& res)
+  {
+    ei_assert( ((Options&RowMajor)!=RowMajor) && "row-major dense matrices is not supported by SuperLU");
+    res.setStorageType(SLU_DN);
+    res.setScalarType<Scalar>();
+    res.Mtype     = SLU_GE;
+
+    res.nrow      = mat.rows();
+    res.ncol      = mat.cols();
+
+    res.storage.lda       = mat.stride();
+    res.storage.values    = mat.data();
+  }
+};
+
+template<typename Derived>
+struct SluMatrixMapHelper<SparseMatrixBase<Derived> >
+{
+  typedef Derived MatrixType;
+  static void run(MatrixType& mat, SluMatrix& res)
+  {
+    if ((MatrixType::Flags&RowMajorBit)==RowMajorBit)
+    {
+      res.setStorageType(SLU_NR);
+      res.nrow      = mat.cols();
+      res.ncol      = mat.rows();
+    }
+    else
+    {
+      res.setStorageType(SLU_NC);
+      res.nrow      = mat.rows();
+      res.ncol      = mat.cols();
+    }
+
+    res.Mtype     = SLU_GE;
+
+    res.storage.nnz       = mat.nonZeros();
+    res.storage.values    = mat._valuePtr();
+    res.storage.innerInd  = mat._innerIndexPtr();
+    res.storage.outerInd  = mat._outerIndexPtr();
+
+    res.setScalarType<typename MatrixType::Scalar>();
+
+    // FIXME the following is not very accurate
+    if (MatrixType::Flags & UpperTriangular)
+      res.Mtype = SLU_TRU;
+    if (MatrixType::Flags & LowerTriangular)
+      res.Mtype = SLU_TRL;
+    if (MatrixType::Flags & SelfAdjoint)
+      ei_assert(false && "SelfAdjoint matrix shape not supported by SuperLU");
+  }
+};
+
+template<typename Derived>
+SluMatrix SparseMatrixBase<Derived>::asSluMatrix()
+{
+  return SluMatrix::Map(derived());
+}
+
+/** View a Super LU matrix as an Eigen expression */
+template<typename Scalar, int Flags>
+MappedSparseMatrix<Scalar,Flags>::MappedSparseMatrix(SluMatrix& sluMat)
+{
+  if ((Flags&RowMajorBit)==RowMajorBit)
+  {
+    assert(sluMat.Stype == SLU_NR);
+    m_innerSize   = sluMat.ncol;
+    m_outerSize   = sluMat.nrow;
+  }
+  else
+  {
+    assert(sluMat.Stype == SLU_NC);
+    m_innerSize   = sluMat.nrow;
+    m_outerSize   = sluMat.ncol;
+  }
+  m_outerIndex = sluMat.storage.outerInd;
+  m_innerIndices = sluMat.storage.innerInd;
+  m_values = reinterpret_cast<Scalar*>(sluMat.storage.values);
+  m_nnz = sluMat.storage.outerInd[m_outerSize];
+}
+
+template<typename MatrixType>
+class SparseLU<MatrixType,SuperLU> : public SparseLU<MatrixType>
+{
+  protected:
+    typedef SparseLU<MatrixType> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::RealScalar RealScalar;
+    typedef Matrix<Scalar,Dynamic,1> Vector;
+    typedef Matrix<int, 1, MatrixType::ColsAtCompileTime> IntRowVectorType;
+    typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> IntColVectorType;
+    typedef SparseMatrix<Scalar,LowerTriangular|UnitDiagBit> LMatrixType;
+    typedef SparseMatrix<Scalar,UpperTriangular> UMatrixType;
+    using Base::m_flags;
+    using Base::m_status;
+
+  public:
+
+    SparseLU(int flags = NaturalOrdering)
+      : Base(flags)
+    {
+    }
+
+    SparseLU(const MatrixType& matrix, int flags = NaturalOrdering)
+      : Base(flags)
+    {
+      compute(matrix);
+    }
+
+    ~SparseLU()
+    {
+    }
+
+    inline const LMatrixType& matrixL() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_l;
+    }
+
+    inline const UMatrixType& matrixU() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_u;
+    }
+
+    inline const IntColVectorType& permutationP() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_p;
+    }
+
+    inline const IntRowVectorType& permutationQ() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_q;
+    }
+
+    Scalar determinant() const;
+
+    template<typename BDerived, typename XDerived>
+    bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+
+    void extractData() const;
+
+  protected:
+    // cached data to reduce reallocation, etc.
+    mutable LMatrixType m_l;
+    mutable UMatrixType m_u;
+    mutable IntColVectorType m_p;
+    mutable IntRowVectorType m_q;
+
+    mutable SparseMatrix<Scalar> m_matrix;
+    mutable SluMatrix m_sluA;
+    mutable SuperMatrix m_sluL, m_sluU;
+    mutable SluMatrix m_sluB, m_sluX;
+    mutable SuperLUStat_t m_sluStat;
+    mutable superlu_options_t m_sluOptions;
+    mutable std::vector<int> m_sluEtree;
+    mutable std::vector<RealScalar> m_sluRscale, m_sluCscale;
+    mutable std::vector<RealScalar> m_sluFerr, m_sluBerr;
+    mutable char m_sluEqued;
+    mutable bool m_extractedDataAreDirty;
+};
+
+template<typename MatrixType>
+void SparseLU<MatrixType,SuperLU>::compute(const MatrixType& a)
+{
+  const int size = a.rows();
+  m_matrix = a;
+
+  set_default_options(&m_sluOptions);
+  m_sluOptions.ColPerm = NATURAL;
+  m_sluOptions.PrintStat = NO;
+  m_sluOptions.ConditionNumber = NO;
+  m_sluOptions.Trans = NOTRANS;
+  // m_sluOptions.Equil = NO;
+
+  switch (Base::orderingMethod())
+  {
+      case NaturalOrdering          : m_sluOptions.ColPerm = NATURAL; break;
+      case MinimumDegree_AT_PLUS_A  : m_sluOptions.ColPerm = MMD_AT_PLUS_A; break;
+      case MinimumDegree_ATA        : m_sluOptions.ColPerm = MMD_ATA; break;
+      case ColApproxMinimumDegree   : m_sluOptions.ColPerm = COLAMD; break;
+      default:
+        std::cerr << "Eigen: ordering method \"" << Base::orderingMethod() << "\" not supported by the SuperLU backend\n";
+        m_sluOptions.ColPerm = NATURAL;
+  };
+
+  m_sluA = m_matrix.asSluMatrix();
+  memset(&m_sluL,0,sizeof m_sluL);
+  memset(&m_sluU,0,sizeof m_sluU);
+  m_sluEqued = 'B';
+  int info = 0;
+
+  m_p.resize(size);
+  m_q.resize(size);
+  m_sluRscale.resize(size);
+  m_sluCscale.resize(size);
+  m_sluEtree.resize(size);
+
+  RealScalar recip_pivot_gross, rcond;
+  RealScalar ferr, berr;
+
+  // set empty B and X
+  m_sluB.setStorageType(SLU_DN);
+  m_sluB.setScalarType<Scalar>();
+  m_sluB.Mtype = SLU_GE;
+  m_sluB.storage.values = 0;
+  m_sluB.nrow = m_sluB.ncol = 0;
+  m_sluB.storage.lda = size;
+  m_sluX = m_sluB;
+
+  StatInit(&m_sluStat);
+  SuperLU_gssvx(&m_sluOptions, &m_sluA, m_q.data(), m_p.data(), &m_sluEtree[0],
+          &m_sluEqued, &m_sluRscale[0], &m_sluCscale[0],
+          &m_sluL, &m_sluU,
+          NULL, 0,
+          &m_sluB, &m_sluX,
+          &recip_pivot_gross, &rcond,
+          &ferr, &berr,
+          &m_sluStat, &info, Scalar());
+  StatFree(&m_sluStat);
+
+  m_extractedDataAreDirty = true;
+
+  // FIXME how to better check for errors ???
+  Base::m_succeeded = (info == 0);
+}
+
+template<typename MatrixType>
+template<typename BDerived,typename XDerived>
+bool SparseLU<MatrixType,SuperLU>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived> *x) const
+{
+  const int size = m_matrix.rows();
+  const int rhsCols = b.cols();
+  ei_assert(size==b.rows());
+
+  m_sluOptions.Fact = FACTORED;
+  m_sluOptions.IterRefine = NOREFINE;
+
+  m_sluFerr.resize(rhsCols);
+  m_sluBerr.resize(rhsCols);
+  m_sluB = SluMatrix::Map(b.const_cast_derived());
+  m_sluX = SluMatrix::Map(x->derived());
+
+  StatInit(&m_sluStat);
+  int info = 0;
+  RealScalar recip_pivot_gross, rcond;
+  SuperLU_gssvx(
+    &m_sluOptions, &m_sluA,
+    m_q.data(), m_p.data(),
+    &m_sluEtree[0], &m_sluEqued,
+    &m_sluRscale[0], &m_sluCscale[0],
+    &m_sluL, &m_sluU,
+    NULL, 0,
+    &m_sluB, &m_sluX,
+    &recip_pivot_gross, &rcond,
+    &m_sluFerr[0], &m_sluBerr[0],
+    &m_sluStat, &info, Scalar());
+  StatFree(&m_sluStat);
+
+  return info==0;
+}
+
+//
+// the code of this extractData() function has been adapted from the SuperLU's Matlab support code,
+//
+//  Copyright (c) 1994 by Xerox Corporation.  All rights reserved.
+//
+//  THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
+//  EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
+//
+template<typename MatrixType>
+void SparseLU<MatrixType,SuperLU>::extractData() const
+{
+  if (m_extractedDataAreDirty)
+  {
+    int         upper;
+    int         fsupc, istart, nsupr;
+    int         lastl = 0, lastu = 0;
+    SCformat    *Lstore = static_cast<SCformat*>(m_sluL.Store);
+    NCformat    *Ustore = static_cast<NCformat*>(m_sluU.Store);
+    Scalar      *SNptr;
+
+    const int size = m_matrix.rows();
+    m_l.resize(size,size);
+    m_l.resizeNonZeros(Lstore->nnz);
+    m_u.resize(size,size);
+    m_u.resizeNonZeros(Ustore->nnz);
+
+    int* Lcol = m_l._outerIndexPtr();
+    int* Lrow = m_l._innerIndexPtr();
+    Scalar* Lval = m_l._valuePtr();
+
+    int* Ucol = m_u._outerIndexPtr();
+    int* Urow = m_u._innerIndexPtr();
+    Scalar* Uval = m_u._valuePtr();
+
+    Ucol[0] = 0;
+    Ucol[0] = 0;
+
+    /* for each supernode */
+    for (int k = 0; k <= Lstore->nsuper; ++k)
+    {
+      fsupc   = L_FST_SUPC(k);
+      istart  = L_SUB_START(fsupc);
+      nsupr   = L_SUB_START(fsupc+1) - istart;
+      upper   = 1;
+
+      /* for each column in the supernode */
+      for (int j = fsupc; j < L_FST_SUPC(k+1); ++j)
+      {
+        SNptr = &((Scalar*)Lstore->nzval)[L_NZ_START(j)];
+
+        /* Extract U */
+        for (int i = U_NZ_START(j); i < U_NZ_START(j+1); ++i)
+        {
+          Uval[lastu] = ((Scalar*)Ustore->nzval)[i];
+          /* Matlab doesn't like explicit zero. */
+          if (Uval[lastu] != 0.0)
+            Urow[lastu++] = U_SUB(i);
+        }
+        for (int i = 0; i < upper; ++i)
+        {
+          /* upper triangle in the supernode */
+          Uval[lastu] = SNptr[i];
+          /* Matlab doesn't like explicit zero. */
+          if (Uval[lastu] != 0.0)
+            Urow[lastu++] = L_SUB(istart+i);
+        }
+        Ucol[j+1] = lastu;
+
+        /* Extract L */
+        Lval[lastl] = 1.0; /* unit diagonal */
+        Lrow[lastl++] = L_SUB(istart + upper - 1);
+        for (int i = upper; i < nsupr; ++i)
+        {
+          Lval[lastl] = SNptr[i];
+          /* Matlab doesn't like explicit zero. */
+          if (Lval[lastl] != 0.0)
+            Lrow[lastl++] = L_SUB(istart+i);
+        }
+        Lcol[j+1] = lastl;
+
+        ++upper;
+      } /* for j ... */
+
+    } /* for k ... */
+
+    // squeeze the matrices :
+    m_l.resizeNonZeros(lastl);
+    m_u.resizeNonZeros(lastu);
+
+    m_extractedDataAreDirty = false;
+  }
+}
+
+template<typename MatrixType>
+typename SparseLU<MatrixType,SuperLU>::Scalar SparseLU<MatrixType,SuperLU>::determinant() const
+{
+  if (m_extractedDataAreDirty)
+    extractData();
+
+  // TODO this code coule be moved to the default/base backend
+  // FIXME perhaps we have to take into account the scale factors m_sluRscale and m_sluCscale ???
+  Scalar det = Scalar(1);
+  for (int j=0; j<m_u.cols(); ++j)
+  {
+    if (m_u._outerIndexPtr()[j+1]-m_u._outerIndexPtr()[j] > 0)
+    {
+      int lastId = m_u._outerIndexPtr()[j+1]-1;
+      ei_assert(m_u._innerIndexPtr()[lastId]<=j);
+      if (m_u._innerIndexPtr()[lastId]==j)
+      {
+        det *= m_u._valuePtr()[lastId];
+      }
+    }
+    // std::cout << m_sluRscale[j] << " " << m_sluCscale[j] << "   ";
+  }
+  return det;
+}
+
+#endif // EIGEN_SUPERLUSUPPORT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/TaucsSupport.h b/extern/Eigen2/Eigen/src/Sparse/TaucsSupport.h
new file mode 100644
index 00000000000..4dddca7b622
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/TaucsSupport.h
@@ -0,0 +1,210 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_TAUCSSUPPORT_H
+#define EIGEN_TAUCSSUPPORT_H
+
+template<typename Derived>
+taucs_ccs_matrix SparseMatrixBase<Derived>::asTaucsMatrix()
+{
+  taucs_ccs_matrix res;
+  res.n         = cols();
+  res.m         = rows();
+  res.flags     = 0;
+  res.colptr    = derived()._outerIndexPtr();
+  res.rowind    = derived()._innerIndexPtr();
+  res.values.v  = derived()._valuePtr();
+  if (ei_is_same_type<Scalar,int>::ret)
+    res.flags |= TAUCS_INT;
+  else if (ei_is_same_type<Scalar,float>::ret)
+    res.flags |= TAUCS_SINGLE;
+  else if (ei_is_same_type<Scalar,double>::ret)
+    res.flags |= TAUCS_DOUBLE;
+  else if (ei_is_same_type<Scalar,std::complex<float> >::ret)
+    res.flags |= TAUCS_SCOMPLEX;
+  else if (ei_is_same_type<Scalar,std::complex<double> >::ret)
+    res.flags |= TAUCS_DCOMPLEX;
+  else
+  {
+    ei_assert(false && "Scalar type not supported by TAUCS");
+  }
+
+  if (Flags & UpperTriangular)
+    res.flags |= TAUCS_UPPER;
+  if (Flags & LowerTriangular)
+    res.flags |= TAUCS_LOWER;
+  if (Flags & SelfAdjoint)
+    res.flags |= (NumTraits<Scalar>::IsComplex ? TAUCS_HERMITIAN : TAUCS_SYMMETRIC);
+  else if ((Flags & UpperTriangular) || (Flags & LowerTriangular))
+    res.flags |= TAUCS_TRIANGULAR;
+
+  return res;
+}
+
+template<typename Scalar, int Flags>
+MappedSparseMatrix<Scalar,Flags>::MappedSparseMatrix(taucs_ccs_matrix& taucsMat)
+{
+  m_innerSize = taucsMat.m;
+  m_outerSize = taucsMat.n;
+  m_outerIndex = taucsMat.colptr;
+  m_innerIndices = taucsMat.rowind;
+  m_values = reinterpret_cast<Scalar*>(taucsMat.values.v);
+  m_nnz = taucsMat.colptr[taucsMat.n];
+}
+
+template<typename MatrixType>
+class SparseLLT<MatrixType,Taucs> : public SparseLLT<MatrixType>
+{
+  protected:
+    typedef SparseLLT<MatrixType> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::RealScalar RealScalar;
+    using Base::MatrixLIsDirty;
+    using Base::SupernodalFactorIsDirty;
+    using Base::m_flags;
+    using Base::m_matrix;
+    using Base::m_status;
+
+  public:
+
+    SparseLLT(int flags = 0)
+      : Base(flags), m_taucsSupernodalFactor(0)
+    {
+    }
+
+    SparseLLT(const MatrixType& matrix, int flags = 0)
+      : Base(flags), m_taucsSupernodalFactor(0)
+    {
+      compute(matrix);
+    }
+
+    ~SparseLLT()
+    {
+      if (m_taucsSupernodalFactor)
+        taucs_supernodal_factor_free(m_taucsSupernodalFactor);
+    }
+
+    inline const typename Base::CholMatrixType& matrixL(void) const;
+
+    template<typename Derived>
+    void solveInPlace(MatrixBase<Derived> &b) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+    void* m_taucsSupernodalFactor;
+};
+
+template<typename MatrixType>
+void SparseLLT<MatrixType,Taucs>::compute(const MatrixType& a)
+{
+  if (m_taucsSupernodalFactor)
+  {
+    taucs_supernodal_factor_free(m_taucsSupernodalFactor);
+    m_taucsSupernodalFactor = 0;
+  }
+
+  if (m_flags & IncompleteFactorization)
+  {
+    taucs_ccs_matrix taucsMatA = const_cast<MatrixType&>(a).asTaucsMatrix();
+    taucs_ccs_matrix* taucsRes = taucs_ccs_factor_llt(&taucsMatA, Base::m_precision, 0);
+    // the matrix returned by Taucs is not necessarily sorted,
+    // so let's copy it in two steps
+    DynamicSparseMatrix<Scalar,RowMajor> tmp = MappedSparseMatrix<Scalar>(*taucsRes);
+    m_matrix = tmp;
+    free(taucsRes);
+    m_status = (m_status & ~(CompleteFactorization|MatrixLIsDirty))
+             | IncompleteFactorization
+             | SupernodalFactorIsDirty;
+  }
+  else
+  {
+    taucs_ccs_matrix taucsMatA = const_cast<MatrixType&>(a).asTaucsMatrix();
+    if ( (m_flags & SupernodalLeftLooking)
+      || ((!(m_flags & SupernodalMultifrontal)) && (m_flags & MemoryEfficient)) )
+    {
+      m_taucsSupernodalFactor = taucs_ccs_factor_llt_ll(&taucsMatA);
+    }
+    else
+    {
+      // use the faster Multifrontal routine
+      m_taucsSupernodalFactor = taucs_ccs_factor_llt_mf(&taucsMatA);
+    }
+    m_status = (m_status & ~IncompleteFactorization) | CompleteFactorization | MatrixLIsDirty;
+  }
+}
+
+template<typename MatrixType>
+inline const typename SparseLLT<MatrixType>::CholMatrixType&
+SparseLLT<MatrixType,Taucs>::matrixL() const
+{
+  if (m_status & MatrixLIsDirty)
+  {
+    ei_assert(!(m_status & SupernodalFactorIsDirty));
+
+    taucs_ccs_matrix* taucsL = taucs_supernodal_factor_to_ccs(m_taucsSupernodalFactor);
+
+    // the matrix returned by Taucs is not necessarily sorted,
+    // so let's copy it in two steps
+    DynamicSparseMatrix<Scalar,RowMajor> tmp = MappedSparseMatrix<Scalar>(*taucsL);
+    const_cast<typename Base::CholMatrixType&>(m_matrix) = tmp;
+    free(taucsL);
+    m_status = (m_status & ~MatrixLIsDirty);
+  }
+  return m_matrix;
+}
+
+template<typename MatrixType>
+template<typename Derived>
+void SparseLLT<MatrixType,Taucs>::solveInPlace(MatrixBase<Derived> &b) const
+{
+  bool inputIsCompatibleWithTaucs = (Derived::Flags&RowMajorBit)==0;
+
+  if (!inputIsCompatibleWithTaucs)
+  {
+    matrixL();
+    Base::solveInPlace(b);
+  }
+  else if (m_flags & IncompleteFactorization)
+  {
+    taucs_ccs_matrix taucsLLT = const_cast<typename Base::CholMatrixType&>(m_matrix).asTaucsMatrix();
+    typename ei_plain_matrix_type<Derived>::type x(b.rows());
+    for (int j=0; j<b.cols(); ++j)
+    {
+      taucs_ccs_solve_llt(&taucsLLT,x.data(),&b.col(j).coeffRef(0));
+      b.col(j) = x;
+    }
+  }
+  else
+  {
+    typename ei_plain_matrix_type<Derived>::type x(b.rows());
+    for (int j=0; j<b.cols(); ++j)
+    {
+      taucs_supernodal_solve_llt(m_taucsSupernodalFactor,x.data(),&b.col(j).coeffRef(0));
+      b.col(j) = x;
+    }
+  }
+}
+
+#endif // EIGEN_TAUCSSUPPORT_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/TriangularSolver.h b/extern/Eigen2/Eigen/src/Sparse/TriangularSolver.h
new file mode 100644
index 00000000000..8948ae45e1d
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/TriangularSolver.h
@@ -0,0 +1,178 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_SPARSETRIANGULARSOLVER_H
+#define EIGEN_SPARSETRIANGULARSOLVER_H
+
+// forward substitution, row-major
+template<typename Lhs, typename Rhs>
+struct ei_solve_triangular_selector<Lhs,Rhs,LowerTriangular,RowMajor|IsSparse>
+{
+  typedef typename Rhs::Scalar Scalar;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    for(int col=0 ; col<other.cols() ; ++col)
+    {
+      for(int i=0; i<lhs.rows(); ++i)
+      {
+        Scalar tmp = other.coeff(i,col);
+        Scalar lastVal = 0;
+        int lastIndex = 0;
+        for(typename Lhs::InnerIterator it(lhs, i); it; ++it)
+        {
+          lastVal = it.value();
+          lastIndex = it.index();
+          tmp -= lastVal * other.coeff(lastIndex,col);
+        }
+        if (Lhs::Flags & UnitDiagBit)
+          other.coeffRef(i,col) = tmp;
+        else
+        {
+          ei_assert(lastIndex==i);
+          other.coeffRef(i,col) = tmp/lastVal;
+        }
+      }
+    }
+  }
+};
+
+// backward substitution, row-major
+template<typename Lhs, typename Rhs>
+struct ei_solve_triangular_selector<Lhs,Rhs,UpperTriangular,RowMajor|IsSparse>
+{
+  typedef typename Rhs::Scalar Scalar;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    for(int col=0 ; col<other.cols() ; ++col)
+    {
+      for(int i=lhs.rows()-1 ; i>=0 ; --i)
+      {
+        Scalar tmp = other.coeff(i,col);
+        typename Lhs::InnerIterator it(lhs, i);
+        if (it.index() == i)
+          ++it;
+        for(; it; ++it)
+        {
+          tmp -= it.value() * other.coeff(it.index(),col);
+        }
+
+        if (Lhs::Flags & UnitDiagBit)
+          other.coeffRef(i,col) = tmp;
+        else
+        {
+          typename Lhs::InnerIterator it(lhs, i);
+          ei_assert(it.index() == i);
+          other.coeffRef(i,col) = tmp/it.value();
+        }
+      }
+    }
+  }
+};
+
+// forward substitution, col-major
+template<typename Lhs, typename Rhs>
+struct ei_solve_triangular_selector<Lhs,Rhs,LowerTriangular,ColMajor|IsSparse>
+{
+  typedef typename Rhs::Scalar Scalar;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    for(int col=0 ; col<other.cols() ; ++col)
+    {
+      for(int i=0; i<lhs.cols(); ++i)
+      {
+        typename Lhs::InnerIterator it(lhs, i);
+        if(!(Lhs::Flags & UnitDiagBit))
+        {
+          // std::cerr << it.value() << " ; " << it.index() << " == " << i << "\n";
+          ei_assert(it.index()==i);
+          other.coeffRef(i,col) /= it.value();
+        }
+        Scalar tmp = other.coeffRef(i,col);
+        if (it.index()==i)
+          ++it;
+        for(; it; ++it)
+          other.coeffRef(it.index(), col) -= tmp * it.value();
+      }
+    }
+  }
+};
+
+// backward substitution, col-major
+template<typename Lhs, typename Rhs>
+struct ei_solve_triangular_selector<Lhs,Rhs,UpperTriangular,ColMajor|IsSparse>
+{
+  typedef typename Rhs::Scalar Scalar;
+  static void run(const Lhs& lhs, Rhs& other)
+  {
+    for(int col=0 ; col<other.cols() ; ++col)
+    {
+      for(int i=lhs.cols()-1; i>=0; --i)
+      {
+        if(!(Lhs::Flags & UnitDiagBit))
+        {
+          // FIXME lhs.coeff(i,i) might not be always efficient while it must simply be the
+          // last element of the column !
+          other.coeffRef(i,col) /= lhs.coeff(i,i);
+        }
+        Scalar tmp = other.coeffRef(i,col);
+        typename Lhs::InnerIterator it(lhs, i);
+        for(; it && it.index()<i; ++it)
+          other.coeffRef(it.index(), col) -= tmp * it.value();
+      }
+    }
+  }
+};
+
+template<typename Derived>
+template<typename OtherDerived>
+void SparseMatrixBase<Derived>::solveTriangularInPlace(MatrixBase<OtherDerived>& other) const
+{
+  ei_assert(derived().cols() == derived().rows());
+  ei_assert(derived().cols() == other.rows());
+  ei_assert(!(Flags & ZeroDiagBit));
+  ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit));
+
+  enum { copy = ei_traits<OtherDerived>::Flags & RowMajorBit };
+
+  typedef typename ei_meta_if<copy,
+    typename ei_plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::ret OtherCopy;
+  OtherCopy otherCopy(other.derived());
+
+  ei_solve_triangular_selector<Derived, typename ei_unref<OtherCopy>::type>::run(derived(), otherCopy);
+
+  if (copy)
+    other = otherCopy;
+}
+
+template<typename Derived>
+template<typename OtherDerived>
+typename ei_plain_matrix_type_column_major<OtherDerived>::type
+SparseMatrixBase<Derived>::solveTriangular(const MatrixBase<OtherDerived>& other) const
+{
+  typename ei_plain_matrix_type_column_major<OtherDerived>::type res(other);
+  solveTriangularInPlace(res);
+  return res;
+}
+
+#endif // EIGEN_SPARSETRIANGULARSOLVER_H
diff --git a/extern/Eigen2/Eigen/src/Sparse/UmfPackSupport.h b/extern/Eigen2/Eigen/src/Sparse/UmfPackSupport.h
new file mode 100644
index 00000000000..b76ffb25248
--- /dev/null
+++ b/extern/Eigen2/Eigen/src/Sparse/UmfPackSupport.h
@@ -0,0 +1,289 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen 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 3 of the License, or (at your option) any later version.
+//
+// Alternatively, 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.
+//
+// Eigen 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 or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_UMFPACKSUPPORT_H
+#define EIGEN_UMFPACKSUPPORT_H
+
+/* TODO extract L, extract U, compute det, etc... */
+
+// generic double/complex<double> wrapper functions:
+
+inline void umfpack_free_numeric(void **Numeric, double)
+{ umfpack_di_free_numeric(Numeric); }
+
+inline void umfpack_free_numeric(void **Numeric, std::complex<double>)
+{ umfpack_zi_free_numeric(Numeric); }
+
+inline void umfpack_free_symbolic(void **Symbolic, double)
+{ umfpack_di_free_symbolic(Symbolic); }
+
+inline void umfpack_free_symbolic(void **Symbolic, std::complex<double>)
+{ umfpack_zi_free_symbolic(Symbolic); }
+
+inline int umfpack_symbolic(int n_row,int n_col,
+                            const int Ap[], const int Ai[], const double Ax[], void **Symbolic,
+                            const double Control [UMFPACK_CONTROL], double Info [UMFPACK_INFO])
+{
+  return umfpack_di_symbolic(n_row,n_col,Ap,Ai,Ax,Symbolic,Control,Info);
+}
+
+inline int umfpack_symbolic(int n_row,int n_col,
+                            const int Ap[], const int Ai[], const std::complex<double> Ax[], void **Symbolic,
+                            const double Control [UMFPACK_CONTROL], double Info [UMFPACK_INFO])
+{
+  return umfpack_zi_symbolic(n_row,n_col,Ap,Ai,&Ax[0].real(),0,Symbolic,Control,Info);
+}
+
+inline int umfpack_numeric( const int Ap[], const int Ai[], const double Ax[],
+                            void *Symbolic, void **Numeric,
+                            const double Control[UMFPACK_CONTROL],double Info [UMFPACK_INFO])
+{
+  return umfpack_di_numeric(Ap,Ai,Ax,Symbolic,Numeric,Control,Info);
+}
+
+inline int umfpack_numeric( const int Ap[], const int Ai[], const std::complex<double> Ax[],
+                            void *Symbolic, void **Numeric,
+                            const double Control[UMFPACK_CONTROL],double Info [UMFPACK_INFO])
+{
+  return umfpack_zi_numeric(Ap,Ai,&Ax[0].real(),0,Symbolic,Numeric,Control,Info);
+}
+
+inline int umfpack_solve( int sys, const int Ap[], const int Ai[], const double Ax[],
+                          double X[], const double B[], void *Numeric,
+                          const double Control[UMFPACK_CONTROL], double Info[UMFPACK_INFO])
+{
+  return umfpack_di_solve(sys,Ap,Ai,Ax,X,B,Numeric,Control,Info);
+}
+
+inline int umfpack_solve( int sys, const int Ap[], const int Ai[], const std::complex<double> Ax[],
+                          std::complex<double> X[], const std::complex<double> B[], void *Numeric,
+                          const double Control[UMFPACK_CONTROL], double Info[UMFPACK_INFO])
+{
+  return umfpack_zi_solve(sys,Ap,Ai,&Ax[0].real(),0,&X[0].real(),0,&B[0].real(),0,Numeric,Control,Info);
+}
+
+inline int umfpack_get_lunz(int *lnz, int *unz, int *n_row, int *n_col, int *nz_udiag, void *Numeric, double)
+{
+  return umfpack_di_get_lunz(lnz,unz,n_row,n_col,nz_udiag,Numeric);
+}
+
+inline int umfpack_get_lunz(int *lnz, int *unz, int *n_row, int *n_col, int *nz_udiag, void *Numeric, std::complex<double>)
+{
+  return umfpack_zi_get_lunz(lnz,unz,n_row,n_col,nz_udiag,Numeric);
+}
+
+inline int umfpack_get_numeric(int Lp[], int Lj[], double Lx[], int Up[], int Ui[], double Ux[],
+                               int P[], int Q[], double Dx[], int *do_recip, double Rs[], void *Numeric)
+{
+  return umfpack_di_get_numeric(Lp,Lj,Lx,Up,Ui,Ux,P,Q,Dx,do_recip,Rs,Numeric);
+}
+
+inline int umfpack_get_numeric(int Lp[], int Lj[], std::complex<double> Lx[], int Up[], int Ui[], std::complex<double> Ux[],
+                               int P[], int Q[], std::complex<double> Dx[], int *do_recip, double Rs[], void *Numeric)
+{
+  return umfpack_zi_get_numeric(Lp,Lj,Lx?&Lx[0].real():0,0,Up,Ui,Ux?&Ux[0].real():0,0,P,Q,
+                               Dx?&Dx[0].real():0,0,do_recip,Rs,Numeric);
+}
+
+inline int umfpack_get_determinant(double *Mx, double *Ex, void *NumericHandle, double User_Info [UMFPACK_INFO])
+{
+  return umfpack_di_get_determinant(Mx,Ex,NumericHandle,User_Info);
+}
+
+inline int umfpack_get_determinant(std::complex<double> *Mx, double *Ex, void *NumericHandle, double User_Info [UMFPACK_INFO])
+{
+  return umfpack_zi_get_determinant(&Mx->real(),0,Ex,NumericHandle,User_Info);
+}
+
+
+template<typename MatrixType>
+class SparseLU<MatrixType,UmfPack> : public SparseLU<MatrixType>
+{
+  protected:
+    typedef SparseLU<MatrixType> Base;
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::RealScalar RealScalar;
+    typedef Matrix<Scalar,Dynamic,1> Vector;
+    typedef Matrix<int, 1, MatrixType::ColsAtCompileTime> IntRowVectorType;
+    typedef Matrix<int, MatrixType::RowsAtCompileTime, 1> IntColVectorType;
+    typedef SparseMatrix<Scalar,LowerTriangular|UnitDiagBit> LMatrixType;
+    typedef SparseMatrix<Scalar,UpperTriangular> UMatrixType;
+    using Base::m_flags;
+    using Base::m_status;
+
+  public:
+
+    SparseLU(int flags = NaturalOrdering)
+      : Base(flags), m_numeric(0)
+    {
+    }
+
+    SparseLU(const MatrixType& matrix, int flags = NaturalOrdering)
+      : Base(flags), m_numeric(0)
+    {
+      compute(matrix);
+    }
+
+    ~SparseLU()
+    {
+      if (m_numeric)
+        umfpack_free_numeric(&m_numeric,Scalar());
+    }
+
+    inline const LMatrixType& matrixL() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_l;
+    }
+
+    inline const UMatrixType& matrixU() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_u;
+    }
+
+    inline const IntColVectorType& permutationP() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_p;
+    }
+
+    inline const IntRowVectorType& permutationQ() const
+    {
+      if (m_extractedDataAreDirty) extractData();
+      return m_q;
+    }
+
+    Scalar determinant() const;
+
+    template<typename BDerived, typename XDerived>
+    bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x) const;
+
+    void compute(const MatrixType& matrix);
+
+  protected:
+
+    void extractData() const;
+  
+  protected:
+    // cached data:
+    void* m_numeric;
+    const MatrixType* m_matrixRef;
+    mutable LMatrixType m_l;
+    mutable UMatrixType m_u;
+    mutable IntColVectorType m_p;
+    mutable IntRowVectorType m_q;
+    mutable bool m_extractedDataAreDirty;
+};
+
+template<typename MatrixType>
+void SparseLU<MatrixType,UmfPack>::compute(const MatrixType& a)
+{
+  const int rows = a.rows();
+  const int cols = a.cols();
+  ei_assert((MatrixType::Flags&RowMajorBit)==0 && "Row major matrices are not supported yet");
+
+  m_matrixRef = &a;
+
+  if (m_numeric)
+    umfpack_free_numeric(&m_numeric,Scalar());
+
+  void* symbolic;
+  int errorCode = 0;
+  errorCode = umfpack_symbolic(rows, cols, a._outerIndexPtr(), a._innerIndexPtr(), a._valuePtr(),
+                                  &symbolic, 0, 0);
+  if (errorCode==0)
+    errorCode = umfpack_numeric(a._outerIndexPtr(), a._innerIndexPtr(), a._valuePtr(),
+                                   symbolic, &m_numeric, 0, 0);
+
+  umfpack_free_symbolic(&symbolic,Scalar());
+
+  m_extractedDataAreDirty = true;
+
+  Base::m_succeeded = (errorCode==0);
+}
+
+template<typename MatrixType>
+void SparseLU<MatrixType,UmfPack>::extractData() const
+{
+  if (m_extractedDataAreDirty)
+  {
+    // get size of the data
+    int lnz, unz, rows, cols, nz_udiag;
+    umfpack_get_lunz(&lnz, &unz, &rows, &cols, &nz_udiag, m_numeric, Scalar());
+
+    // allocate data
+    m_l.resize(rows,std::min(rows,cols));
+    m_l.resizeNonZeros(lnz);
+    
+    m_u.resize(std::min(rows,cols),cols);
+    m_u.resizeNonZeros(unz);
+
+    m_p.resize(rows);
+    m_q.resize(cols);
+
+    // extract
+    umfpack_get_numeric(m_l._outerIndexPtr(), m_l._innerIndexPtr(), m_l._valuePtr(),
+                        m_u._outerIndexPtr(), m_u._innerIndexPtr(), m_u._valuePtr(),
+                        m_p.data(), m_q.data(), 0, 0, 0, m_numeric);
+    
+    m_extractedDataAreDirty = false;
+  }
+}
+
+template<typename MatrixType>
+typename SparseLU<MatrixType,UmfPack>::Scalar SparseLU<MatrixType,UmfPack>::determinant() const
+{
+  Scalar det;
+  umfpack_get_determinant(&det, 0, m_numeric, 0);
+  return det;
+}
+
+template<typename MatrixType>
+template<typename BDerived,typename XDerived>
+bool SparseLU<MatrixType,UmfPack>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived> *x) const
+{
+  //const int size = m_matrix.rows();
+  const int rhsCols = b.cols();
+//   ei_assert(size==b.rows());
+  ei_assert((BDerived::Flags&RowMajorBit)==0 && "UmfPack backend does not support non col-major rhs yet");
+  ei_assert((XDerived::Flags&RowMajorBit)==0 && "UmfPack backend does not support non col-major result yet");
+
+  int errorCode;
+  for (int j=0; j<rhsCols; ++j)
+  {
+    errorCode = umfpack_solve(UMFPACK_A,
+        m_matrixRef->_outerIndexPtr(), m_matrixRef->_innerIndexPtr(), m_matrixRef->_valuePtr(),
+        &x->col(j).coeffRef(0), &b.const_cast_derived().col(j).coeffRef(0), m_numeric, 0, 0);
+    if (errorCode!=0)
+      return false;
+  }
+//   errorCode = umfpack_di_solve(UMFPACK_A,
+//       m_matrixRef._outerIndexPtr(), m_matrixRef._innerIndexPtr(), m_matrixRef._valuePtr(),
+//       x->derived().data(), b.derived().data(), m_numeric, 0, 0);
+
+  return true;
+}
+
+#endif // EIGEN_UMFPACKSUPPORT_H
diff --git a/extern/Eigen2/eigen-update.sh b/extern/Eigen2/eigen-update.sh
new file mode 100644
index 00000000000..26155ed428b
--- /dev/null
+++ b/extern/Eigen2/eigen-update.sh
@@ -0,0 +1,28 @@
+#!/bin/sh
+
+echo "*** EIGEN2-SVN Update utility"
+echo "*** This gets a new eigen2-svn tree and adapts it to blenders build structure"
+echo "*** Warning! This script will wipe all the header file"
+echo "*** Please run again with --i-really-know-what-im-doing ..."
+
+if [ "x$1" = "x--i-really-know-what-im-doing" ] ; then
+   echo proceeding...
+else
+   exit -1
+fi
+
+# get the latest revision from repository.
+hg clone http://bitbucket.org/eigen/eigen2
+if [ -d eigen2 ]
+then
+	cd eigen2
+	# put here the version you want to use
+	hg up 2.0.6
+	rm -f `find Eigen/ -type f -name "CMakeLists.txt"`
+	cp -r Eigen ..
+	cd ..
+	rm -rf eigen2
+else
+	echo "Did you install Mercurial?"
+fi
+
diff --git a/extern/glew/make/msvc_9_0/glew.vcproj b/extern/glew/make/msvc_9_0/glew.vcproj
index f9d8df478ca..a7186f61cbc 100644
--- a/extern/glew/make/msvc_9_0/glew.vcproj
+++ b/extern/glew/make/msvc_9_0/glew.vcproj
@@ -112,6 +112,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB"

diff --git a/intern/CMakeLists.txt b/intern/CMakeLists.txt
index 9efd1a6ee7c..ac08b780ab8 100644
--- a/intern/CMakeLists.txt
+++ b/intern/CMakeLists.txt
@@ -33,6 +33,7 @@ ADD_SUBDIRECTORY(container)
 ADD_SUBDIRECTORY(memutil)
 ADD_SUBDIRECTORY(decimation)
 ADD_SUBDIRECTORY(iksolver)
+ADD_SUBDIRECTORY(itasc)
 ADD_SUBDIRECTORY(boolop)
 ADD_SUBDIRECTORY(opennl)
 ADD_SUBDIRECTORY(smoke)
diff --git a/intern/Makefile b/intern/Makefile
index 4bf18f987a4..32375f16b7a 100644
--- a/intern/Makefile
+++ b/intern/Makefile
@@ -32,7 +32,7 @@ SOURCEDIR = intern
 # include nan_subdirs.mk
 
 ALLDIRS = string ghost guardedalloc moto container memutil
-ALLDIRS += decimation iksolver bsp opennl elbeem boolop smoke audaspace
+ALLDIRS += decimation iksolver itasc bsp SoundSystem opennl elbeem boolop smoke audaspace
 
 all::
 	@for i in $(ALLDIRS); do \
diff --git a/intern/SConscript b/intern/SConscript
index af5d0671c27..241662b7088 100644
--- a/intern/SConscript
+++ b/intern/SConscript
@@ -10,6 +10,7 @@ SConscript(['audaspace/SConscript',
             'memutil/SConscript/',
             'decimation/SConscript',
             'iksolver/SConscript',
+            'itasc/SConscript',
             'boolop/SConscript',
             'opennl/SConscript',
             'smoke/SConscript'])
diff --git a/intern/audaspace/make/msvc_9_0/audaspace.vcproj b/intern/audaspace/make/msvc_9_0/audaspace.vcproj
index 0d8ade43e07..93dcdd66628 100644
--- a/intern/audaspace/make/msvc_9_0/audaspace.vcproj
+++ b/intern/audaspace/make/msvc_9_0/audaspace.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..;..\..\ffmpeg;..\..\FX;..\..\intern;..\..\OpenAL;..\..\SDL;..\..\SRC;..\..\sndfile;..\..\..\..\..\lib\windows\pthreads\include;..\..\..\..\..\lib\windows\samplerate\include;..\..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\..\lib\windows\sdl\include;..\..\..\..\..\lib\windows\openal\include;..\..\..\..\..\lib\windows\jack\include;..\..\..\..\..\lib\windows\sndfile\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB,WITH_FFMPEG,WITH_SDL,WITH_OPENAL"

diff --git a/intern/boolop/make/msvc_9_0/boolop.vcproj b/intern/boolop/make/msvc_9_0/boolop.vcproj
index 7fe83962695..357d189376a 100644
--- a/intern/boolop/make/msvc_9_0/boolop.vcproj
+++ b/intern/boolop/make/msvc_9_0/boolop.vcproj
@@ -119,6 +119,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\extern;..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\..\build\msvc_9\intern\container\include;..\..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\source\blender\blenlib;..\..\..\..\source\blender\makesdna;$(NOINHERIT)"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/bsp/make/msvc_9_0/bsplib.vcproj b/intern/bsp/make/msvc_9_0/bsplib.vcproj
index a1b16d5b93f..ed6978b8229 100644
--- a/intern/bsp/make/msvc_9_0/bsplib.vcproj
+++ b/intern/bsp/make/msvc_9_0/bsplib.vcproj
@@ -119,6 +119,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..;..\..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\container\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/container/make/msvc_9_0/container.vcproj b/intern/container/make/msvc_9_0/container.vcproj
index 2b40571672d..76bc56f413f 100644
--- a/intern/container/make/msvc_9_0/container.vcproj
+++ b/intern/container/make/msvc_9_0/container.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

 				StringPooling="true"

diff --git a/intern/decimation/make/msvc_9_0/decimation.vcproj b/intern/decimation/make/msvc_9_0/decimation.vcproj
index 7d58bf1f4c6..a75332857ad 100644
--- a/intern/decimation/make/msvc_9_0/decimation.vcproj
+++ b/intern/decimation/make/msvc_9_0/decimation.vcproj
@@ -119,6 +119,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\..\build\msvc_9\intern\container\include;..\..\..\..\..\build\msvc_9\intern\moto\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/elbeem/make/msvc_9_0/elbeem.vcproj b/intern/elbeem/make/msvc_9_0/elbeem.vcproj
index 4108e09799d..2369a76fff0 100644
--- a/intern/elbeem/make/msvc_9_0/elbeem.vcproj
+++ b/intern/elbeem/make/msvc_9_0/elbeem.vcproj
@@ -114,6 +114,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				AdditionalIncludeDirectories="..\..\intern;..\..\extern;..\..\..\..\..\lib\windows\png\include;..\..\..\..\..\lib\windows\zlib\include;..\..\..\..\source\kernel\gen_messaging;..\..\..\..\..\lib\windows\sdl\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB;NOGUI;ELBEEM_BLENDER=1;LBM_INCLUDE_CONTROL=1"

 				StringPooling="true"

diff --git a/intern/ghost/intern/GHOST_System.cpp b/intern/ghost/intern/GHOST_System.cpp
index 229744e2000..84298d3e3ff 100644
--- a/intern/ghost/intern/GHOST_System.cpp
+++ b/intern/ghost/intern/GHOST_System.cpp
@@ -291,7 +291,7 @@ GHOST_TSuccess GHOST_System::init()
 #ifdef GHOST_DEBUG
 	if (m_eventManager) {
 		m_eventPrinter = new GHOST_EventPrinter();
-		//m_eventManager->addConsumer(m_eventPrinter);
+		m_eventManager->addConsumer(m_eventPrinter);
 	}
 #endif // GHOST_DEBUG
 
diff --git a/intern/ghost/make/msvc_9_0/ghost.vcproj b/intern/ghost/make/msvc_9_0/ghost.vcproj
index fa128786a90..6b3a49cfc9c 100644
--- a/intern/ghost/make/msvc_9_0/ghost.vcproj
+++ b/intern/ghost/make/msvc_9_0/ghost.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\lib\windows\wintab\INCLUDE"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/guardedalloc/make/msvc_9_0/guardedalloc.vcproj b/intern/guardedalloc/make/msvc_9_0/guardedalloc.vcproj
index d59b80f7b62..16deb7b71fa 100644
--- a/intern/guardedalloc/make/msvc_9_0/guardedalloc.vcproj
+++ b/intern/guardedalloc/make/msvc_9_0/guardedalloc.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\.."

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/iksolver/make/msvc_9_0/iksolver.vcproj b/intern/iksolver/make/msvc_9_0/iksolver.vcproj
index 0e87556380b..296a23e57cc 100644
--- a/intern/iksolver/make/msvc_9_0/iksolver.vcproj
+++ b/intern/iksolver/make/msvc_9_0/iksolver.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\..\build\msvc_9\intern\moto\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/itasc/Armature.cpp b/intern/itasc/Armature.cpp
new file mode 100644
index 00000000000..cd059505b4a
--- /dev/null
+++ b/intern/itasc/Armature.cpp
@@ -0,0 +1,769 @@
+/* $Id: Armature.cpp 21152 2009-06-25 11:57:19Z ben2610 $
+ * Armature.cpp
+ *
+ *  Created on: Feb 3, 2009
+ *      Author: benoitbolsee
+ */
+
+#include "Armature.hpp"
+#include <algorithm>
+#include <malloc.h>
+#include <string.h>
+
+namespace iTaSC {
+
+// a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot
+static const unsigned int constraintCacheSize = 5;
+std::string Armature::m_root = "root";
+
+Armature::Armature():
+	ControlledObject(),
+	m_tree(),
+	m_njoint(0),
+	m_nconstraint(0),
+	m_noutput(0),
+	m_neffector(0),
+	m_finalized(false),
+	m_cache(NULL),
+	m_buf(NULL),
+	m_qCCh(-1),
+	m_qCTs(0),
+	m_yCCh(-1),
+	m_yCTs(0),
+	m_qKdl(),
+	m_oldqKdl(),
+	m_newqKdl(),
+	m_qdotKdl(),
+	m_jac(NULL),
+	m_jacsolver(NULL),
+	m_fksolver(NULL),
+	m_armlength(0.0)
+{
+}
+
+Armature::~Armature()
+{
+	if (m_jac)
+		delete m_jac;
+	if (m_jacsolver)
+		delete m_jacsolver;
+	if (m_fksolver)
+		delete m_fksolver;
+	for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
+		if (*it != NULL)
+			delete (*it);
+	}
+	if (m_buf)
+		delete [] m_buf;
+	m_constraints.clear();
+}
+
+Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep):
+	segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep)
+{
+	memset(values, 0, sizeof(values));
+	memset(value, 0, sizeof(value));
+	values[0].feedback = 20.0;
+	values[1].feedback = 20.0;
+	values[2].feedback = 20.0;
+	values[0].tolerance = 1.0;
+	values[1].tolerance = 1.0;
+	values[2].tolerance = 1.0;
+	values[0].values = &value[0];
+	values[1].values = &value[1];
+	values[2].values = &value[2];
+	values[0].number = 1;
+	values[1].number = 1;
+	values[2].number = 1;
+	switch (segment->second.segment.getJoint().getType()) {
+	case Joint::RotX:
+		value[0].id = ID_JOINT_RX;		
+		values[0].id = ID_JOINT_RX;		
+		v_nr = 1;
+		break;
+	case Joint::RotY:
+		value[0].id = ID_JOINT_RY;		
+		values[0].id = ID_JOINT_RY;		
+		v_nr = 1;
+		break;
+	case Joint::RotZ:
+		value[0].id = ID_JOINT_RZ;		
+		values[0].id = ID_JOINT_RZ;		
+		v_nr = 1;
+		break;
+	case Joint::TransX:
+		value[0].id = ID_JOINT_TX;		
+		values[0].id = ID_JOINT_TX;		
+		v_nr = 1;
+		break;
+	case Joint::TransY:
+		value[0].id = ID_JOINT_TY;		
+		values[0].id = ID_JOINT_TY;		
+		v_nr = 1;
+		break;
+	case Joint::TransZ:
+		value[0].id = ID_JOINT_TZ;		
+		values[0].id = ID_JOINT_TZ;		
+		v_nr = 1;
+		break;
+	case Joint::Sphere:
+		values[0].id = value[0].id = ID_JOINT_RX;		
+		values[1].id = value[1].id = ID_JOINT_RY;
+		values[2].id = value[2].id = ID_JOINT_RZ;		
+		v_nr = 3;
+		break;
+	case Joint::Swing:
+		values[0].id = value[0].id = ID_JOINT_RX;		
+		values[1].id = value[1].id = ID_JOINT_RZ;		
+		v_nr = 2;
+		break;
+	}
+}
+
+Armature::JointConstraint_struct::~JointConstraint_struct()
+{
+	if (freeParam && param)
+		free(param);
+}
+
+void Armature::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_qCCh = -1;
+	m_yCCh = -1;
+	m_buf = NULL;
+	if (m_cache) {
+		// add a special channel for the joint
+		m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
+#if 0
+		// for the constraints, instead of creating many different channels, we will
+		// create a single channel for all the constraints
+		if (m_nconstraint) {
+			m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double));
+			m_buf = new double[m_nconstraint*constraintCacheSize];
+		}
+		// store the initial cache position at timestamp 0
+		pushConstraints(0);
+#endif
+		pushQ(0);
+	}
+}
+
+void Armature::pushQ(CacheTS timestamp)
+{
+	if (m_qCCh >= 0) {
+		// try to keep the cache if the joints are the same
+		m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
+		m_qCTs = timestamp;
+	}
+}
+
+/* return true if a m_cache position was loaded */
+bool Armature::popQ(CacheTS timestamp)
+{
+	if (m_qCCh >= 0) {
+		double* item;
+		item = (double*)m_cache->getPreviousCacheItem(this, m_qCCh, &timestamp);
+		if (item && m_qCTs != timestamp) {
+			double& q = m_qKdl(0);
+			memcpy(&q, item, m_qKdl.rows()*sizeof(q));
+			m_qCTs = timestamp;
+			// changing the joint => recompute the jacobian
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+#if 0
+void Armature::pushConstraints(CacheTS timestamp)
+{
+	if (m_yCCh >= 0) {
+		double *buf = NULL;
+		if (m_nconstraint) {
+			double *item = m_buf;
+			for (unsigned int i=0; i<m_nconstraint; i++) {
+				JointConstraint_struct* pConstraint = m_constraints[i];
+				*item++ = pConstraint->values.feedback;
+				*item++ = pConstraint->values.tolerance;
+				*item++ = pConstraint->value.yd;
+				*item++ = pConstraint->value.yddot;
+				*item++ = pConstraint->values.alpha;
+			}
+		}
+		m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon);
+		m_yCTs = timestamp;
+	}
+}
+
+/* return true if a cache position was loaded */
+bool Armature::popConstraints(CacheTS timestamp)
+{
+	if (m_yCCh >= 0) {
+		double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, &timestamp);
+		if (item && m_yCTs != timestamp) {
+			for (unsigned int i=0; i<m_nconstraint; i++) {
+				JointConstraint_struct* pConstraint = m_constraints[i];
+				if (pConstraint->function != Joint1DOFLimitCallback) {
+					pConstraint->values.feedback = *item++;
+					pConstraint->values.tolerance = *item++;
+					pConstraint->value.yd = *item++;
+					pConstraint->value.yddot = *item++;
+					pConstraint->values.alpha = *item++;
+				} else {
+					item += constraintCacheSize;
+				}
+			}
+			m_yCTs = timestamp;
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+#endif
+
+bool Armature::addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const Inertia& M)
+{
+	if (m_finalized)
+		return false;
+
+	Segment segment(joint, f_tip, M);
+	if (!m_tree.addSegment(segment, segment_name, hook_name))
+		return false;
+	int ndof = joint.getNDof();
+	for (int dof=0; dof<ndof; dof++) {
+		Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
+		m_joints.push_back(js);
+	}
+	m_njoint+=ndof;
+	return true;
+}
+
+bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
+{
+	SegmentMap::const_iterator sit = m_tree.getSegment(name);
+	if (sit == m_tree.getSegments().end())
+		return false;
+	p_joint = &sit->second.segment.getJoint();
+	if (q_size < p_joint->getNDof())
+		return false;
+	p_tip = &sit->second.segment.getFrameToTip();
+	for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
+		(&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
+		(&q)[dof] = m_qKdl(sit->second.q_nr+dof);
+	}
+	return true;
+}
+
+double Armature::getMaxJointChange()
+{
+	if (!m_finalized)
+		return 0.0;
+	double maxJoint = 0.0;
+	for (unsigned int i=0; i<m_njoint; i++) {
+		// this is a very rough calculation, it doesn't work well for spherical joint
+		double joint = fabs(m_oldqKdl(i)-m_qKdl(i));
+		if (maxJoint < joint)
+			maxJoint = joint;
+	}
+	return maxJoint;
+}
+
+double Armature::getMaxEndEffectorChange()
+{
+	if (!m_finalized)
+		return 0.0;
+	double maxDelta = 0.0;
+	double delta;
+	Twist twist;
+	for (unsigned int i = 0; i<m_neffector; i++) {
+		twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
+		delta = twist.rot.Norm();
+		if (delta > maxDelta)
+			maxDelta = delta;
+		delta = twist.vel.Norm();
+		if (delta > maxDelta)
+			maxDelta = delta;
+	}
+	return maxDelta;
+}
+
+int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep)
+{
+	SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+	// not suitable for NDof joints
+	if (segment_it == m_tree.getSegments().end()) {
+		if (_freeParam && _param)
+			free(_param);
+		return -1;
+	}
+	JointConstraintList::iterator constraint_it;
+	JointConstraint_struct* pConstraint;
+	int iConstraint;
+	for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
+		pConstraint = *constraint_it;
+		if (pConstraint->segment == segment_it) {
+			// redefining a constraint
+			if (pConstraint->freeParam && pConstraint->param) {
+				free(pConstraint->param);
+			}
+			pConstraint->function = _function;
+			pConstraint->param = _param;
+			pConstraint->freeParam = _freeParam;
+			pConstraint->substep = _substep;
+			return iConstraint;
+		}
+	}
+	if (m_finalized)  {
+		if (_freeParam && _param)
+			free(_param);
+		return -1;
+	}
+	// new constraint, append
+	pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep);
+	m_constraints.push_back(pConstraint);
+	m_noutput += pConstraint->v_nr;
+	return m_nconstraint++;
+}
+
+int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max)
+{
+	SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
+	if (segment_it == m_tree.getSegments().end())
+		return -1;
+	const Joint& joint = segment_it->second.segment.getJoint();
+	if (joint.getNDof() != 1 && joint.getType() != Joint::Swing) {
+		// not suitable for Sphere joints
+		return -1;
+	}
+	if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1))
+		return -1;
+	if (joint.getType() < Joint::TransX || joint.getType() == Joint::Swing) {
+		// for rotation joint, the limit is given in degree, convert to radian
+		_min *= KDL::deg2rad;
+		_max *= KDL::deg2rad;
+	}
+	Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
+	p_joint.min = _min;
+	p_joint.max = _max;
+	p_joint.useLimit = true;
+	return 0;
+}
+
+int Armature::addEndEffector(const std::string& name)
+{
+	const SegmentMap& segments = m_tree.getSegments();
+	if (segments.find(name) == segments.end())
+		return -1;
+
+	EffectorList::const_iterator it;
+	int ee;
+	for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
+		if (it->name == name)
+			return ee;
+	}
+	if (m_finalized)
+		return -1;
+	Effector_struct effector(name);
+	m_effectors.push_back(effector);
+	return m_neffector++;
+}
+
+void Armature::finalize()
+{
+	unsigned int i, j, c;
+	if (m_finalized)
+		return;
+	initialize(m_njoint, m_noutput, m_neffector);
+	for (i=c=0; i<m_nconstraint; i++) {
+		JointConstraint_struct* pConstraint = m_constraints[i];
+		for (j=0; j<pConstraint->v_nr; j++, c++) {
+			m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0;
+			m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+		}
+	}
+	m_jacsolver= new KDL::TreeJntToJacSolver(m_tree);
+	m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
+	m_jac = new Jacobian(m_njoint);
+	m_qKdl.resize(m_njoint);
+	m_oldqKdl.resize(m_njoint);
+	m_newqKdl.resize(m_njoint);
+	m_qdotKdl.resize(m_njoint);
+	for (i=0; i<m_njoint; i++) {
+		m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
+	}
+	updateJacobian();
+	// estimate the maximum size of the robot arms
+	double length;
+	m_armlength = 0.0;
+	for (i=0; i<m_neffector; i++) {
+		length = 0.0;
+		KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
+		while (sit->first != "root") {
+			Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
+			length += tip.p.Norm();
+			sit = sit->second.parent;
+		}
+		if (length > m_armlength)
+			m_armlength = length;
+	}
+	if (m_armlength < KDL::epsilon)
+		m_armlength = KDL::epsilon;
+	m_finalized = true;
+}
+
+void Armature::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache) {
+		pushQ(timestamp.cacheTimestamp);
+		//pushConstraints(timestamp.cacheTimestamp);
+	}
+}
+
+bool Armature::setJointArray(const KDL::JntArray& joints)
+{
+	if (!m_finalized)
+		return false;
+	if (joints.rows() != m_qKdl.rows())
+		return false;
+	m_qKdl = joints;
+	updateJacobian();
+	return true;
+}
+
+const KDL::JntArray& Armature::getJointArray()
+{
+	return m_qKdl;
+}
+
+bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
+{
+	if (!m_finalized)
+		return false;
+	
+	// integration and joint limit
+	// for spherical joint we must use a more sophisticated method
+	unsigned int q_nr;
+	double* qdot=&m_qdotKdl(0);
+	double* q=&m_qKdl(0);
+	double* newq=&m_newqKdl(0);
+	double norm, qx, qz, CX, CZ, sx, sz;
+	bool locked = false;
+	int unlocked = 0;
+
+	for (q_nr=0; q_nr<m_nq; ++q_nr)
+		m_qdotKdl(q_nr)=m_qdot(q_nr);
+
+	for (q_nr=0; q_nr<m_nq; ) {
+		Joint_struct* joint = &m_joints[q_nr];
+		if (!joint->locked) {
+			switch (joint->type) {
+			case KDL::Joint::Swing:
+			{
+				KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
+				(base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
+				if (joint[0].useLimit) {
+					if (joint[1].useLimit) {
+						// elliptical limit
+						sx = sz = 1.0;
+						qx = newq[0];
+						qz = newq[1];
+						// determine in which quadrant we are
+						if (qx > 0.0 && qz > 0.0) {
+							CX = joint[0].max;
+							CZ = joint[1].max;
+						} else if (qx <= 0.0 && qz > 0.0) {
+							CX = -joint[0].min;
+							CZ = joint[1].max;
+							qx = -qx;
+							sx = -1.0;
+						} else if (qx <= 0.0 && qz <= 0.0) {
+							CX = -joint[0].min;
+							CZ = -joint[1].min;
+							qx = -qx;
+							qz = -qz;
+							sx = sz = -1.0;
+						} else {
+							CX = joint[0].max;
+							CZ = -joint[0].min;
+							qz = -qz;
+							sz = -1.0;
+						}
+						if (CX < KDL::epsilon || CZ < KDL::epsilon) {
+							// quadrant is degenerated
+							if (qx > CX) {
+								newq[0] = CX*sx;
+								joint[0].locked = true;
+							}
+							if (qz > CZ) {
+								newq[1] = CZ*sz;
+								joint[0].locked = true;
+							}
+						} else {
+							// general case
+							qx /= CX;
+							qz /= CZ;
+							norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
+							if (norm > 1.0) {
+								norm = 1.0/norm;
+								newq[0] = qx*norm*CX*sx;
+								newq[1] = qz*norm*CZ*sz;
+								joint[0].locked = true;
+							}
+						}
+					} else {
+						// limit on X only
+						qx = newq[0];
+						if (qx > joint[0].max) {
+							newq[0] = joint[0].max;
+							joint[0].locked = true;
+						} else if (qx < joint[0].min) {
+							newq[0] = joint[0].min;
+							joint[0].locked = true;
+						}
+					}
+				} else if (joint[1].useLimit) {
+					// limit on Z only
+					qz = newq[1];
+					if (qz > joint[1].max) {
+						newq[1] = joint[1].max;
+						joint[0].locked = true;
+					} else if (qz < joint[1].min) {
+						newq[1] = joint[1].min;
+						joint[0].locked = true;
+					}
+				}
+				if (joint[0].locked) {
+					// check the difference from previous position
+					locked = true;
+					norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]);
+					if (norm < KDL::epsilon2) {
+						// joint didn't move, no need to update the jacobian
+						callback.lockJoint(q_nr, 2);
+					} else {
+						// joint moved, compute the corresponding velocity
+						double deltaq[2];
+						(base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq);
+						deltaq[0] /= timestamp.realTimestep;
+						deltaq[1] /= timestamp.realTimestep;
+						callback.lockJoint(q_nr, 2, deltaq);
+						// no need to update the other joints, it will be done after next rerun
+						goto end_loop;
+					}
+				} else
+					unlocked++;
+				break;
+			}
+			case KDL::Joint::Sphere:
+			{
+				(KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
+				// no limit on this joint
+				unlocked++;
+				break;
+			}
+			default:
+				for (unsigned int i=0; i<joint->ndof; i++) {
+					newq[i] = q[i]+qdot[i]*timestamp.realTimestep;
+					if (joint[i].useLimit) {
+						if (newq[i] > joint[i].max) {
+							newq[i] = joint[i].max;
+							joint[0].locked = true;
+						} else if (newq[i] < joint[i].min) {
+							newq[i] = joint[i].min;
+							joint[0].locked = true;
+						}
+					}
+				}
+				if (joint[0].locked) {
+					locked = true;
+					norm = 0.0;
+					// compute delta to locked position
+					for (unsigned int i=0; i<joint->ndof; i++) {
+						qdot[i] = newq[i] - q[i];
+						norm += qdot[i]*qdot[i];
+					}
+					if (norm < KDL::epsilon2) {
+						// joint didn't move, no need to update the jacobian
+						callback.lockJoint(q_nr, joint->ndof);
+					} else {
+						// solver needs velocity, compute equivalent velocity
+						for (unsigned int i=0; i<joint->ndof; i++) {
+							qdot[i] /= timestamp.realTimestep;
+						}
+						callback.lockJoint(q_nr, joint->ndof, qdot);
+						goto end_loop;
+					}
+				} else 
+					unlocked++;
+			}
+		}
+		qdot += joint->ndof;
+		q    += joint->ndof;
+		newq += joint->ndof;
+		q_nr += joint->ndof;
+	}
+end_loop:
+	// check if there any other unlocked joint
+	for ( ; q_nr<m_nq; ) {
+		Joint_struct* joint = &m_joints[q_nr];
+		if (!joint->locked)
+			unlocked++;
+		q_nr += joint->ndof;
+	}
+	// if all joints have been locked no need to run the solver again
+	return (unlocked) ? locked : false;
+}
+
+void Armature::updateKinematics(const Timestamp& timestamp){
+
+    //Integrate m_qdot
+	if (!m_finalized)
+		return;
+
+	// the new joint value have been computed already, just copy
+	memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows());
+	pushCache(timestamp);
+	updateJacobian();
+	// here update the desired output.
+	// We assume constant desired output for the joint limit constraint, no need to update it.
+}
+
+void Armature::updateJacobian()
+{
+    //calculate pose and jacobian
+	for (unsigned int ee=0; ee<m_nee; ee++) {
+		m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
+		m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
+		// get the jacobian for the base point, to prepare transformation to world reference
+		changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac);
+		//copy to Jq:
+		e_matrix& Jq = m_JqArray[ee];
+		for(unsigned int i=0;i<6;i++) {
+			for(unsigned int j=0;j<m_nq;j++)
+				Jq(i,j)=(*m_jac)(i,j);
+		}
+	}
+	// remember that this object has moved 
+	m_updated = true;
+}
+
+const Frame& Armature::getPose(const unsigned int ee)
+{
+	if (!m_finalized)
+		return F_identity;
+	return (ee >= m_nee) ? F_identity : m_effectors[ee].pose;
+}
+
+bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name)
+{
+	if (!m_finalized)
+		return false;
+	return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true;
+}
+
+void Armature::updateControlOutput(const Timestamp& timestamp)
+{
+	if (!m_finalized)
+		return;
+
+
+	if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
+		popQ(timestamp.cacheTimestamp);
+		//popConstraints(timestamp.cacheTimestamp);
+	}
+
+	if (!timestamp.substep) {
+		// save previous joint state for getMaxJointChange()
+		memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
+		for (unsigned int i=0; i<m_neffector; i++) {
+			m_effectors[i].oldpose = m_effectors[i].pose;
+		}
+	}
+
+	// remove all joint lock
+	for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) {
+		(*jit).locked = false;
+	}
+
+	JointConstraintList::iterator it;
+	unsigned int iConstraint;
+
+	// scan through the constraints and call the callback functions
+	for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
+		JointConstraint_struct* pConstraint = *it;
+		unsigned int nr, i;
+		for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
+			*(double*)&pConstraint->value[i].y = m_qKdl(nr);
+			*(double*)&pConstraint->value[i].ydot = m_qdotKdl(nr);
+		}
+		if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
+			(*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
+		}
+		// recompute the weight in any case, that's the most likely modification
+		for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) {
+			m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+			m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y);
+		}
+	}
+}
+
+bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep)
+{
+	unsigned int lastid, i;
+	if (constraintId == CONSTRAINT_ID_ALL) {
+		constraintId = 0;
+		lastid = m_nconstraint;
+	} else if (constraintId < m_nconstraint) {
+		lastid = constraintId+1;
+	} else {
+		return false;
+	}
+	for ( ; constraintId<lastid; ++constraintId) {
+		JointConstraint_struct* pConstraint = m_constraints[constraintId];
+		if (valueId == ID_JOINT) {
+			for (i=0; i<pConstraint->v_nr; i++) {
+				switch (action) {
+				case ACT_TOLERANCE:
+					pConstraint->values[i].tolerance = value;
+					break;
+				case ACT_FEEDBACK:
+					pConstraint->values[i].feedback = value;
+					break;
+				case ACT_ALPHA:
+					pConstraint->values[i].alpha = value;
+					break;
+				}
+			}
+		} else {
+			for (i=0; i<pConstraint->v_nr; i++) {
+				if (valueId == pConstraint->value[i].id) {
+					switch (action) {
+					case ACT_VALUE:
+						pConstraint->value[i].yd = value;
+						break;
+					case ACT_VELOCITY:
+						pConstraint->value[i].yddot = value;
+						break;
+					case ACT_TOLERANCE:
+						pConstraint->values[i].tolerance = value;
+						break;
+					case ACT_FEEDBACK:
+						pConstraint->values[i].feedback = value;
+						break;
+					case ACT_ALPHA:
+						pConstraint->values[i].alpha = value;
+						break;
+					}
+				}
+			}
+		}
+		if (m_finalized) {
+			for (i=0; i<pConstraint->v_nr; i++) 
+				m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
+		}
+	}
+	return true;
+}
+
+}
+
diff --git a/intern/itasc/Armature.hpp b/intern/itasc/Armature.hpp
new file mode 100644
index 00000000000..312ca1b28c3
--- /dev/null
+++ b/intern/itasc/Armature.hpp
@@ -0,0 +1,137 @@
+/* $Id: Armature.hpp 20853 2009-06-13 12:29:46Z ben2610 $
+ * Armature.hpp
+ *
+ *  Created on: Feb 3, 2009
+ *      Author: benoitbolsee
+ */
+
+#ifndef ARMATURE_HPP_
+#define ARMATURE_HPP_
+
+#include "ControlledObject.hpp"
+#include "ConstraintSet.hpp"
+#include "kdl/treejnttojacsolver.hpp"
+#include "kdl/treefksolverpos_recursive.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+class Armature: public iTaSC::ControlledObject {
+public:
+    Armature();
+    virtual ~Armature();
+
+	bool addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip=F_identity, const Inertia& M = Inertia::Zero());
+	// general purpose constraint on joint
+	int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false);
+	// specific limit constraint on joint
+	int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max);
+	double getMaxJointChange();
+	double getMaxEndEffectorChange();
+	bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
+	bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
+
+	virtual void finalize();
+
+	virtual int addEndEffector(const std::string& name);
+	virtual const Frame& getPose(const unsigned int end_effector);
+	virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback);
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
+	virtual void initCache(Cache *_cache);
+	virtual bool setJointArray(const KDL::JntArray& joints);
+	virtual const KDL::JntArray& getJointArray();
+
+	virtual double getArmLength()
+	{
+		return m_armlength;
+	}
+
+	struct Effector_struct {
+		std::string name;
+		Frame oldpose;
+		Frame pose;
+		Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
+	};
+	typedef std::vector<Effector_struct> EffectorList;
+
+	enum ID  {
+		ID_JOINT=1,
+		ID_JOINT_RX=2,
+		ID_JOINT_RY=3,
+		ID_JOINT_RZ=4,
+		ID_JOINT_TX=2,
+		ID_JOINT_TY=3,
+		ID_JOINT_TZ=4,
+	};
+	struct JointConstraint_struct {
+		SegmentMap::const_iterator segment;
+		ConstraintSingleValue value[3];
+		ConstraintValues values[3];
+		ConstraintCallback function;
+		unsigned int v_nr;
+		unsigned int y_nr;	// first coordinate of constraint in Y vector
+		void* param;
+		bool freeParam;
+		bool substep;
+		JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep);
+		~JointConstraint_struct();
+	};
+	typedef std::vector<JointConstraint_struct*> JointConstraintList;	
+
+	struct Joint_struct {
+		KDL::Joint::JointType	type;
+		unsigned short			ndof;
+		bool					useLimit;
+		bool					locked;
+		double					rest;
+		double					min;
+		double					max;
+
+		Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) :
+			type(_type), ndof(_ndof), rest(_rest)  { useLimit=locked=false; min=0.0; max=0.0; }
+	};
+	typedef std::vector<Joint_struct> JointList;
+	
+protected:
+    virtual void updateJacobian();
+
+private:
+	static std::string m_root;
+    Tree m_tree;
+	unsigned int m_njoint;
+	unsigned int m_nconstraint;
+	unsigned int m_noutput;
+	unsigned int m_neffector;
+	bool m_finalized;
+	Cache* m_cache;
+	double *m_buf;
+	int m_qCCh;
+	CacheTS m_qCTs;
+	int m_yCCh;
+	CacheTS m_yCTs;
+    JntArray m_qKdl;
+    JntArray m_oldqKdl;
+    JntArray m_newqKdl;
+    JntArray m_qdotKdl;
+    Jacobian* m_jac;
+	double m_armlength;
+
+	KDL::TreeJntToJacSolver* m_jacsolver;
+	KDL::TreeFkSolverPos_recursive* m_fksolver;
+	EffectorList m_effectors;
+	JointConstraintList m_constraints;
+	JointList m_joints;
+
+	void pushQ(CacheTS timestamp);
+	bool popQ(CacheTS timestamp);
+	//void pushConstraints(CacheTS timestamp);
+	//bool popConstraints(CacheTS timestamp);
+
+};
+
+}
+
+#endif /* ARMATURE_HPP_ */
diff --git a/intern/itasc/CMakeLists.txt b/intern/itasc/CMakeLists.txt
new file mode 100644
index 00000000000..405d74d17ac
--- /dev/null
+++ b/intern/itasc/CMakeLists.txt
@@ -0,0 +1,32 @@
+# $Id: CMakeLists.txt 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) 2006, Blender Foundation
+# All rights reserved.
+#
+# The Original Code is: all of this file.
+#
+# Contributor(s): Jacques Beaurain.
+#
+# ***** END GPL LICENSE BLOCK *****
+
+SET(INC ../../extern/Eigen2)
+
+FILE(GLOB SRC *.cpp kdl/*.cpp kdl/utilities/*.cpp)
+
+BLENDERLIB(bf_ITASC "${SRC}" "${INC}")
+#, libtype=['blender'], priority = [10] )
diff --git a/intern/itasc/Cache.cpp b/intern/itasc/Cache.cpp
new file mode 100644
index 00000000000..68c281910e3
--- /dev/null
+++ b/intern/itasc/Cache.cpp
@@ -0,0 +1,621 @@
+/* $Id: Cache.cpp 21152 2009-06-25 11:57:19Z ben2610 $
+ * Cache.cpp
+ *
+ *  Created on: Feb 24, 2009
+ *      Author: benoit bolsee
+ */
+#include <string.h>
+#include <assert.h>
+#include <malloc.h>
+#include "Cache.hpp"
+
+#include <math.h>
+
+namespace iTaSC {
+
+CacheEntry::~CacheEntry()
+{
+   for (unsigned int id=0; id < m_count; id++)
+		m_channelArray[id].clear();
+   if (m_channelArray)
+	   free(m_channelArray);
+}
+
+CacheItem *CacheChannel::_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *retBlock)
+{
+	// the timestamp is necessarily in this buffer
+	unsigned int lowBlock, highBlock, midBlock;
+	if (timeOffset <= buffer->lookup[0].m_timeOffset) {
+		// special case: the item is in the first block, search from start
+		*retBlock = 0;
+		return &buffer->m_firstItem;
+	}
+	// general case, the item is in the middle of the buffer
+	// before doing a dycotomic search, we will assume that timestamp
+	// are regularly spaced so that we can try to locate the block directly
+	highBlock = buffer->m_lastItemPositionW>>m_positionToBlockShiftW;
+	lowBlock = midBlock = (timeOffset*highBlock)/(buffer->m_lastTimestamp-buffer->m_firstTimestamp);
+	// give some space for security
+	if (lowBlock > 0)
+		lowBlock--;
+	if (timeOffset <= buffer->lookup[lowBlock].m_timeOffset) {
+		// bad guess, but we know this block is a good high block, just use it
+		highBlock = lowBlock;
+		lowBlock = 0;
+	} else {
+		// ok, good guess, now check the high block, give some space
+		if (midBlock < highBlock)
+			midBlock++;
+		if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) {
+			// good guess, keep that block as the high block
+			highBlock = midBlock;
+		}
+	}
+	// the item is in a different block, do a dycotomic search
+	// the timestamp is alway > lowBlock and <= highBlock
+	while (1) {
+		midBlock = (lowBlock+highBlock)/2;
+		if (midBlock == lowBlock) {
+			// low block and high block are contigous, we can start search from the low block
+			break;
+		} else if (timeOffset <= buffer->lookup[midBlock].m_timeOffset) {
+			highBlock = midBlock;
+		} else {
+			lowBlock = midBlock;
+		}
+	}
+	assert (lowBlock != highBlock);
+	*retBlock = highBlock;
+	return CACHE_BLOCK_ITEM_ADDR(this,buffer,lowBlock);
+}
+
+void CacheChannel::clear()
+{
+	CacheBuffer *buffer, *next;
+	for (buffer=m_firstBuffer; buffer != 0; buffer = next) {
+		next = buffer->m_next;
+		free(buffer);
+	}
+	m_firstBuffer = NULL;
+	m_lastBuffer = NULL;
+	if (initItem) {
+		free(initItem);
+		initItem = NULL;
+	}
+}
+
+CacheBuffer* CacheChannel::allocBuffer()
+{
+	CacheBuffer* buffer;
+	if (!m_busy)
+		return NULL;
+	buffer = (CacheBuffer*)malloc(CACHE_BUFFER_HEADER_SIZE+(m_bufferSizeW<<2));
+	if (buffer) {
+		memset(buffer, 0, CACHE_BUFFER_HEADER_SIZE);
+	}
+	return buffer;
+}
+
+CacheItem* CacheChannel::findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer)
+{
+	CacheBuffer* buffer;
+	CacheItem *item, *limit;
+	if (!m_busy)
+		return NULL;
+	if (timestamp == 0 && initItem) {
+		*rBuffer = NULL;
+		return initItem;
+	}
+	for (buffer=m_firstBuffer; buffer; buffer = buffer->m_next) {
+		if (buffer->m_firstFreePositionW == 0)
+			// buffer is empty, this must be the last and we didn't find the timestamp
+			return NULL;
+		if (timestamp < buffer->m_firstTimestamp) {
+			*rBuffer = buffer;
+			return &buffer->m_firstItem;
+		}
+		if (timestamp <= buffer->m_lastTimestamp) {
+			// the timestamp is necessarily in this buffer
+			unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+			unsigned int highBlock;
+			item = _findBlock(buffer, timeOffset, &highBlock);
+			// now we do a linear search until we find a timestamp that is equal or higher
+			// we should normally always find an item but let's put a limit just in case
+			limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock);
+			while (item<=limit && item->m_timeOffset < timeOffset )
+				item = CACHE_NEXT_ITEM(item);
+			assert(item<=limit);
+			*rBuffer = buffer;
+			return item;
+		}
+		// search in next buffer
+	}
+	return NULL;
+}
+
+CacheItem* CacheChannel::findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer)
+{
+	CacheBuffer *buffer, *prevBuffer;
+	CacheItem *item, *limit, *prevItem;
+	if (!m_busy)
+		return NULL;
+	if (timestamp == 0)
+		return NULL;
+	for (prevBuffer=NULL, buffer=m_firstBuffer; buffer; prevBuffer = buffer, buffer = buffer->m_next) {
+		if (buffer->m_firstFreePositionW == 0)
+			// buffer is empty, this must be the last and we didn't find the timestamp
+			return NULL;
+		if (timestamp <= buffer->m_firstTimestamp) {
+			if (prevBuffer == NULL) {
+				// no item before, except the initial item
+				*rBuffer = NULL;
+				return initItem;
+			}
+			// the item is necessarily the last one of previous buffer
+			*rBuffer = prevBuffer;
+			return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW);
+		}
+		if (timestamp <= buffer->m_lastTimestamp) {
+			// the timestamp is necessarily in this buffer
+			unsigned short timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+			unsigned int highBlock;
+			item = _findBlock(buffer, timeOffset, &highBlock);
+			// now we do a linear search until we find a timestamp that is equal or higher
+			// we should normally always find an item but let's put a limit just in case
+			limit = CACHE_BLOCK_ITEM_ADDR(this,buffer,highBlock);
+			prevItem = NULL;
+			while (item<=limit && item->m_timeOffset < timeOffset) {
+				prevItem = item;
+				item = CACHE_NEXT_ITEM(item);
+			}
+			assert(item<=limit && prevItem!=NULL);
+			*rBuffer = buffer;
+			return prevItem;
+		}
+		// search in next buffer
+	}
+	// pass all buffer, the last item is the last item of the last buffer
+	if (prevBuffer == NULL) {
+		// no item before, except the initial item
+		*rBuffer = NULL;
+		return initItem;
+	}
+	// the item is necessarily the last one of previous buffer
+	*rBuffer = prevBuffer;
+	return CACHE_ITEM_ADDR(prevBuffer,prevBuffer->m_lastItemPositionW);
+}
+
+
+Cache::Cache()
+{
+}
+
+Cache::~Cache()
+{
+	CacheMap::iterator it;
+	for (it=m_cache.begin(); it!=m_cache.end(); it=m_cache.begin()) {
+		deleteDevice(it->first);
+	}
+}
+
+int Cache::addChannel(const void *device, const char *name, unsigned int maxItemSize)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	unsigned int id;
+
+	if (maxItemSize > 0x3FFF0)
+		return -1;
+
+	if (it == m_cache.end()) {
+		// device does not exist yet, create a new entry
+		entry = new CacheEntry();
+		if (entry == NULL)
+			return -1;
+		if (!m_cache.insert(CacheMap::value_type(device,entry)).second)
+			return -1;
+	} else {
+		entry = it->second;
+	}
+	// locate a channel with the same name and reuse
+	for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) {
+		if (channel->m_busy && !strcmp(name, channel->m_name)) {
+			// make this channel free again
+			deleteChannel(device, id);
+			// there can only be one channel with the same name
+			break;
+		}
+	}
+	for (channel=entry->m_channelArray, id=0; id<entry->m_count; id++, channel++) {
+		// locate a free channel
+		if (!channel->m_busy)
+			break;
+	}
+	if (id == entry->m_count) {
+		// no channel free, create new channels
+		int newcount = entry->m_count + CACHE_CHANNEL_EXTEND_SIZE;
+		channel = (CacheChannel*)realloc(entry->m_channelArray, newcount*sizeof(CacheChannel));
+		if (channel == NULL)
+			return -1;
+		entry->m_channelArray = channel;
+		memset(&entry->m_channelArray[entry->m_count], 0, CACHE_CHANNEL_EXTEND_SIZE*sizeof(CacheChannel));
+		entry->m_count = newcount;
+		channel = &entry->m_channelArray[id];
+	}
+	// compute the optimal buffer size
+	// The buffer size must be selected so that
+	// - it does not contain more than 1630 items (=1s of cache assuming 25 items per second)
+	// - it contains at least one item
+	// - it's not bigger than 256kb and preferably around 32kb
+	// - it a multiple of 4
+	unsigned int bufSize = 1630*(maxItemSize+4);
+	if (bufSize >= CACHE_DEFAULT_BUFFER_SIZE)
+		bufSize = CACHE_DEFAULT_BUFFER_SIZE;
+	if (bufSize < maxItemSize+16)
+		bufSize = maxItemSize+16;
+	bufSize = (bufSize + 3) & ~0x3;
+	// compute block size and offset bit mask
+	// the block size is computed so that
+	// - it is a power of 2
+	// - there is at least one item per block
+	// - there is no more than CACHE_LOOKUP_TABLE_SIZE blocks per buffer
+	unsigned int blockSize = bufSize/CACHE_LOOKUP_TABLE_SIZE;
+	if (blockSize < maxItemSize+12)
+		blockSize = maxItemSize+12;
+	// find the power of 2 that is immediately larger than blockSize
+	unsigned int m;
+	unsigned int pwr2Size = blockSize;
+	while ((m = (pwr2Size & (pwr2Size-1))) != 0)
+		pwr2Size = m;
+	blockSize = (pwr2Size < blockSize) ? pwr2Size<<1 : pwr2Size;
+	// convert byte size to word size because all positions and size are expressed in 32 bit words
+	blockSize >>= 2;
+	channel->m_blockSizeW = blockSize;
+	channel->m_bufferSizeW = bufSize>>2;
+	channel->m_firstBuffer = NULL;
+	channel->m_lastBuffer = NULL;
+	channel->m_busy = 1;
+	channel->initItem = NULL;
+	channel->m_maxItemSizeB = maxItemSize;
+	strncpy(channel->m_name, name, sizeof(channel->m_name));
+	channel->m_name[sizeof(channel->m_name)-1] = 0;
+	channel->m_positionToOffsetMaskW = (blockSize-1);
+	for (m=0; blockSize!=1; m++, blockSize>>=1);
+	channel->m_positionToBlockShiftW = m;
+	return (int)id;
+}
+
+int Cache::deleteChannel(const void *device, int id)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return -1;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int)entry->m_count || !entry->m_channelArray[id].m_busy)
+		return -1;
+	entry->m_channelArray[id].clear();
+	entry->m_channelArray[id].m_busy = 0;
+	return 0;
+}
+
+int Cache::deleteDevice(const void *device)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return -1;
+	}
+	entry = it->second;
+	delete entry;
+	m_cache.erase(it);
+	return 0;
+}
+
+void Cache::clearCacheFrom(const void *device, CacheTS timestamp)
+{
+	CacheMap::iterator it = (device) ? m_cache.find(device) : m_cache.begin();
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer, *nextBuffer, *prevBuffer;
+	CacheItem *item, *prevItem, *nextItem;
+	unsigned int positionW, block;
+
+	while (it != m_cache.end()) {
+		entry = it->second;
+		for (unsigned int ch=0; ch<entry->m_count; ch++) {
+			channel = &entry->m_channelArray[ch];
+			if (channel->m_busy) {
+				item = channel->findItemOrLater(timestamp, &buffer);
+				if (item ) {
+					if (!buffer) {
+						// this is possible if we return the special timestamp=0 item, delete all buffers
+						channel->clear();
+					} else {
+						// this item and all later items will be removed, clear any later buffer
+						while ((nextBuffer = buffer->m_next) != NULL) {
+							buffer->m_next = nextBuffer->m_next;
+							free(nextBuffer);
+						}
+						positionW = CACHE_ITEM_POSITIONW(buffer,item);
+						if (positionW == 0) {
+							// this item is the first one of the buffer, remove the buffer completely
+							// first find the buffer just before it
+							nextBuffer = channel->m_firstBuffer;
+							prevBuffer = NULL;
+							while (nextBuffer != buffer) {
+								prevBuffer = nextBuffer;
+								nextBuffer = nextBuffer->m_next;
+								// we must quit this loop before reaching the end of the list
+								assert(nextBuffer);
+							}
+							free(buffer);
+							buffer = prevBuffer;
+							if (buffer == NULL)
+								// this was also the first buffer
+								channel->m_firstBuffer = NULL;
+						} else {
+							// removing this item means finding the previous item to make it the last one
+							block = positionW>>channel->m_positionToBlockShiftW;
+							if (block == 0) {
+								// start from first item, we know it is not our item because positionW > 0
+								prevItem = &buffer->m_firstItem;
+							} else {
+								// no need to check the current block, it will point to our item or a later one
+								// but the previous block will be a good start for sure.
+								block--;
+								prevItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+							}
+							while ((nextItem = CACHE_NEXT_ITEM(prevItem)) < item)
+								prevItem = nextItem;
+							// we must have found our item
+							assert(nextItem==item);
+							// now set the buffer
+							buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,prevItem);
+							buffer->m_firstFreePositionW = positionW;
+							buffer->m_lastTimestamp = buffer->m_firstTimestamp + prevItem->m_timeOffset;
+							block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+							buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+							buffer->lookup[block].m_timeOffset = prevItem->m_timeOffset;
+						}
+						// set the channel
+						channel->m_lastBuffer = buffer;
+						if (buffer) {
+							channel->m_lastTimestamp = buffer->m_lastTimestamp;
+							channel->m_lastItemPositionW = buffer->m_lastItemPositionW;
+						}
+					}
+				}
+			}
+		}
+		if (device)
+			break;
+		++it;
+	}
+}
+
+void *Cache::addCacheItem(const void *device, int id, unsigned int timestamp, void *data, unsigned int length)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer, *next;
+	CacheItem *item;
+	unsigned int positionW, sizeW, block;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if (length > channel->m_maxItemSizeB)
+		return NULL;
+	if (timestamp == 0) {
+		// initial item, delete all buffers
+		channel->clear();
+		// and create initial item
+		item = NULL;
+		// we will allocate the memory, which is always pointer aligned => compute size
+		// with NULL will give same result.
+		sizeW = CACHE_ITEM_SIZEW(item,length);
+		item = (CacheItem*)calloc(sizeW, 4);
+		item->m_sizeW = sizeW;
+		channel->initItem = item;
+	} else {
+		if (!channel->m_lastBuffer) {
+			// no item in buffer, insert item at first position of first buffer
+			positionW = 0;
+			if ((buffer = channel->m_firstBuffer) == NULL) {
+				buffer = channel->allocBuffer();
+				channel->m_firstBuffer = buffer;
+			}
+		} else if (timestamp > channel->m_lastTimestamp) {
+			// this is the normal case: we are writing past lastest timestamp
+			buffer = channel->m_lastBuffer;
+			positionW = buffer->m_firstFreePositionW;
+		} else if (timestamp == channel->m_lastTimestamp) {
+			// common case, rewriting the last timestamp, just reuse the last position
+			buffer = channel->m_lastBuffer;
+			positionW = channel->m_lastItemPositionW;
+		} else {
+			// general case, write in the middle of the buffer, locate the timestamp
+			// (or the timestamp just after), clear this item and all future items,
+			// and write at that position
+			item = channel->findItemOrLater(timestamp, &buffer);
+			if (item == NULL) {
+				// this should not happen
+				return NULL;
+			}
+			// this item will become the last one of this channel, clear any later buffer
+			while ((next = buffer->m_next) != NULL) {
+				buffer->m_next = next->m_next;
+				free(next);
+			}
+			// no need to update the buffer, this will be done when the item is written
+			positionW = CACHE_ITEM_POSITIONW(buffer,item);
+		}
+		item = CACHE_ITEM_ADDR(buffer,positionW);
+		sizeW = CACHE_ITEM_SIZEW(item,length);
+		// we have positionW pointing where we can put the item
+		// before we do that we have to check if we can:
+		// - enough room
+		// - timestamp not too late
+		if ((positionW+sizeW > channel->m_bufferSizeW) ||
+			(positionW > 0 && timestamp >= buffer->m_firstTimestamp+0x10000)) {
+			// we must allocate a new buffer to store this item
+			// but before we must make sure that the current buffer is consistent
+			if (positionW != buffer->m_firstFreePositionW) {
+				// This means that we were trying to write in the middle of the buffer.
+				// We must set the buffer right with positionW being the last position
+				// and find the item before positionW to make it the last.
+				block = positionW>>channel->m_positionToBlockShiftW;
+				CacheItem *previousItem, *nextItem;
+				if (block == 0) {
+					// start from first item, we know it is not our item because positionW > 0
+					previousItem = &buffer->m_firstItem;
+				} else {
+					// no need to check the current block, it will point to our item or a later one
+					// but the previous block will be a good start for sure.
+					block--;
+					previousItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+				}
+				while ((nextItem = CACHE_NEXT_ITEM(previousItem)) < item)
+					previousItem = nextItem;
+				// we must have found our item
+				assert(nextItem==item);
+				// now set the buffer
+				buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,previousItem);
+				buffer->m_firstFreePositionW = positionW;
+				buffer->m_lastTimestamp = buffer->m_firstTimestamp + previousItem->m_timeOffset;
+				block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+				buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+				buffer->lookup[block].m_timeOffset = previousItem->m_timeOffset;
+				// and also the channel, just in case
+				channel->m_lastBuffer = buffer;
+				channel->m_lastTimestamp = buffer->m_lastTimestamp;
+				channel->m_lastItemPositionW = buffer->m_lastItemPositionW;
+			}
+			// now allocate a new buffer
+			buffer->m_next = channel->allocBuffer();
+			if (buffer->m_next == NULL)
+				return NULL;
+			buffer = buffer->m_next;
+			positionW = 0;
+			item = &buffer->m_firstItem;
+			sizeW = CACHE_ITEM_SIZEW(item,length);
+		}
+		// all check passed, ready to write the item
+		item->m_sizeW = sizeW;
+		if (positionW == 0) {
+			item->m_timeOffset = 0;
+			buffer->m_firstTimestamp = timestamp;
+		} else {
+			item->m_timeOffset = (unsigned short)(timestamp-buffer->m_firstTimestamp);
+		}
+		buffer->m_lastItemPositionW = positionW;
+		buffer->m_firstFreePositionW = positionW+sizeW;
+		buffer->m_lastTimestamp = timestamp;
+		block = positionW>>channel->m_positionToBlockShiftW;
+		buffer->lookup[block].m_offsetW = positionW&channel->m_positionToOffsetMaskW;
+		buffer->lookup[block].m_timeOffset = item->m_timeOffset;
+		buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,item);
+		buffer->m_firstFreePositionW = buffer->m_lastItemPositionW+item->m_sizeW;
+		channel->m_lastBuffer = buffer;
+		channel->m_lastItemPositionW = positionW;
+		channel->m_lastTimestamp = timestamp;
+	}
+	// now copy the item
+	void *itemData = CACHE_ITEM_DATA_POINTER(item);
+	if (data)
+		memcpy(itemData, data, length);
+	return itemData;
+}
+
+const void *Cache::getPreviousCacheItem(const void *device, int id, unsigned int *timestamp)
+{
+	CacheMap::iterator it;
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer;
+	CacheItem *item;
+
+	if (device) {
+		it = m_cache.find(device);	
+	} else {
+		it = m_cache.begin();
+	}
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if ((item = channel->findItemEarlier(*timestamp,&buffer)) == NULL)
+		return NULL;
+	*timestamp = (buffer) ? buffer->m_firstTimestamp+item->m_timeOffset : 0;
+	return CACHE_ITEM_DATA_POINTER(item);
+}
+
+const CacheItem *Cache::getCurrentCacheItemInternal(const void *device, int id, CacheTS timestamp)
+{
+	CacheMap::iterator it = m_cache.find(device);
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer;
+	CacheItem *item;
+
+	if (it == m_cache.end()) {
+		// device does not exist
+		return NULL;
+	}
+	entry = it->second;
+	if (id < 0 || id >= (int) entry->m_count || !entry->m_channelArray[id].m_busy)
+		return NULL;
+	channel = &entry->m_channelArray[id];
+	if ((item = channel->findItemOrLater(timestamp,&buffer)) == NULL)
+		return NULL;
+	if (buffer && buffer->m_firstTimestamp+item->m_timeOffset != timestamp)
+		return NULL;
+	return item;
+}
+
+const void *Cache::getCurrentCacheItem(const void *device, int channel, unsigned int timestamp)
+{
+	const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+	return (item) ? CACHE_ITEM_DATA_POINTER(item) : NULL;
+}
+
+double *Cache::addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *newdata, unsigned int length, double threshold)
+{
+	const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+	unsigned int sizeW = CACHE_ITEM_SIZEW(item,length*sizeof(double));
+	if (!item || item->m_sizeW != sizeW)
+		return (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+	double *olddata = (double*)CACHE_ITEM_DATA_POINTER(item);
+	if (!length)
+		return olddata;
+	double *ref = olddata;
+	double *v = newdata;
+	unsigned int i;
+	for (i=length; i>0; --i) {
+		if (fabs(*v-*ref) > threshold)
+			break;
+		*ref++ = *v++;
+	}
+	if (i) 
+		olddata = (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+	return olddata;
+}
+
+}
diff --git a/intern/itasc/Cache.hpp b/intern/itasc/Cache.hpp
new file mode 100644
index 00000000000..64707782e6f
--- /dev/null
+++ b/intern/itasc/Cache.hpp
@@ -0,0 +1,227 @@
+/* $Id: Cache.hpp 21152 2009-06-25 11:57:19Z ben2610 $
+ * Cache.hpp
+ *
+ *  Created on: Feb 24, 2009
+ *      Author: benoit tbolsee
+ */
+
+#ifndef CACHE_HPP_
+#define CACHE_HPP_
+
+#include <map>
+
+namespace iTaSC {
+
+#define CACHE_LOOKUP_TABLE_SIZE				128
+#define CACHE_DEFAULT_BUFFER_SIZE			32768
+#define CACHE_CHANNEL_EXTEND_SIZE			10
+#define CACHE_MAX_ITEM_SIZE					0x3FFF0
+
+/* macro to get the alignement gap after an item header */
+#define CACHE_ITEM_GAPB(item)				(unsigned int)(((size_t)item+sizeof(CacheItem))&(sizeof(void*)-1))
+/* macro to get item data position, item=CacheItem pointer */
+#define CACHE_ITEM_DATA_POINTER(item)		(void*)((unsigned char*)item+sizeof(CacheItem)+CACHE_ITEM_GAPB(item))
+/* macro to get item size in 32bit words from item address and length, item=CacheItem pointer */
+#define CACHE_ITEM_SIZEW(item,length)		(unsigned int)((sizeof(CacheItem)+CACHE_ITEM_GAPB(item)+(((length)+3)&~0x3))>>2)
+/* macto to move from one item to the next, item=CacheItem pointer, updated by the macro */
+#define CACHE_NEXT_ITEM(item)				((item)+(item)->m_sizeW)
+#define CACHE_BLOCK_ITEM_ADDR(chan,buf,block)	(&(buf)->m_firstItem+(((unsigned int)(block)<<chan->m_positionToBlockShiftW)+(buf)->lookup[block].m_offsetW))
+#define CACHE_ITEM_ADDR(buf,pos)			(&(buf)->m_firstItem+(pos))
+#define CACHE_ITEM_POSITIONW(buf,item)		(unsigned int)(item-&buf->m_firstItem)
+
+typedef unsigned int CacheTS;
+
+struct Timestamp 
+{
+	double realTimestamp;
+	double realTimestep;
+	CacheTS cacheTimestamp;
+	unsigned int numstep:8;
+	unsigned int substep:1;
+	unsigned int reiterate:1;
+	unsigned int cache:1;
+	unsigned int update:1;
+	unsigned int interpolate:1;
+	unsigned int dummy:19;
+
+	Timestamp() { memset(this, 0, sizeof(Timestamp)); }
+};
+
+/* utility function to return second timestamp to millisecond */
+inline void setCacheTimestamp(Timestamp& timestamp)
+{
+	if (timestamp.realTimestamp < 0.0 || timestamp.realTimestamp > 4294967.295)
+		timestamp.cacheTimestamp = 0;
+	else
+		timestamp.cacheTimestamp = (CacheTS)(timestamp.realTimestamp*1000.0+0.5);
+}
+
+
+/*
+class Cache:
+Top level class, only one instance of this class should exists.
+A device (=constraint, object) uses this class to create a cache entry for its data.
+A cache entry is divided into cache channels, each providing a separate buffer for cache items.
+The cache channels must be declared by the devices before they can be used.
+The device must specify the largest cache item (limited to 256Kb) so that the cache
+buffer can be organized optimally.
+Cache channels are identified by small number (starting from 0) allocated by the cache system.
+Cache items are inserted into cache channels ordered by timestamp. Writing is always done
+at the end of the cache buffer: writing an item automatically clears all items with 
+higher timestamp.
+A cache item is an array of bytes provided by the device; the format of the cache item is left 
+to the device. 
+The device can retrieve a cache item associated with a certain timestamp. The cache system
+returns a pointer that points directly in the cache buffer to avoid unnecessary copy. 
+The pointer is guaranteed to be pointer aligned so that direct mapping to C structure is possible 
+(=32 bit aligned on 32 systems and 64 bit aligned on 64 bits system).
+
+Timestamp = rounded time in millisecond.
+*/
+
+struct CacheEntry;
+struct CacheBuffer;
+struct CacheItem;
+struct CacheChannel;
+
+class Cache 
+{
+private:
+	/* map between device and cache entry.
+	   Dynamically updated when more devices create cache channels */
+	typedef std::map<const void *, struct CacheEntry*> CacheMap;
+	CacheMap  m_cache;
+	const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp);
+   
+public:
+	Cache();
+	~Cache();
+	/* add a cache channel, maxItemSize must be < 256k.
+	   name : channel name, truncated at 31 characters
+	   msxItemSize : maximum size of item in bytes, items of larger size will be rejected
+	   return value >= 0: channel id, -1: error */
+	int addChannel(const void *device, const char *name, unsigned int maxItemSize);
+
+	/* delete a cache channel (and all associated buffers and items) */
+	int deleteChannel(const void *device, int channel);
+	/* delete all channels of a device and remove the device from the map */
+	int deleteDevice(const void *device);
+	/* removes all cache items, leaving the special item at timestamp=0. 
+	   if device=NULL, apply to all devices. */
+	void clearCacheFrom(const void *device, CacheTS timestamp);
+
+	/* add a new cache item
+	   channel: the cache channel (as returned by AddChannel
+	   data, length: the cache item and length in bytes
+	                 If data is NULL, the memory is allocated in the cache but not writen
+	   return: error: NULL, success: pointer to item in cache */
+	void *addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length);
+
+	/* specialized function to add a vector of double in the cache
+	   It will first check if a vector exist already in the cache for the same timestamp
+	   and compared the cached vector with the new values. 
+	   If all values are within threshold, the vector is updated but the cache is not deleted
+	   for the future timestamps. */
+	double *addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold);
+
+	/* returns the cache item with timestamp that is just before the given timestamp.
+	   returns the data pointer or NULL if there is no cache item before timestamp.
+	   On return, timestamp is updated with the actual timestamp of the item being returned.
+	   Note that the length of the item is not returned, it is up to the device to organize
+	   the data so that length can be retrieved from the data if needed.
+	   Device can NULL, it will then just look the first channel available, useful to 
+	   test the status of the cache. */
+	const void *getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp);
+
+	/* returns the cache item with the timestamp that is exactly equal to the given timestamp
+	   If there is no cache item for this timestamp, returns NULL.*/
+	const void *getCurrentCacheItem(const void *device, int channel, CacheTS timestamp);
+
+};
+
+/* the following structures are not internal use only, they should not be used directly */
+
+struct CacheEntry 
+{
+	CacheChannel *m_channelArray;		// array of channels, automatically resized if more channels are created
+	unsigned int m_count;				// number of channel in channelArray
+	CacheEntry() : m_channelArray(NULL), m_count(0) {}
+	~CacheEntry();
+};
+
+struct CacheChannel
+{
+	CacheItem* initItem;				// item corresponding to timestamp=0
+	struct CacheBuffer *m_firstBuffer;	// first buffer of list
+	struct CacheBuffer *m_lastBuffer;		// last buffer of list to which an item was written
+	char m_name[32];						// channel name
+	unsigned char m_busy;					// =0 if channel is free, !=0 when channel is in use
+	unsigned char m_positionToBlockShiftW;	// number of bits to shift a position in word to get the block number
+	unsigned short m_positionToOffsetMaskW;   // bit mask to apply on a position in word to get offset in a block
+	unsigned int m_maxItemSizeB;			// maximum item size in bytes
+	unsigned int m_bufferSizeW;			// size of item buffer in word to allocate when a new buffer must be created
+	unsigned int m_blockSizeW;			// block size in words of the lookup table
+	unsigned int m_lastTimestamp;			// timestamp of the last item that was written
+	unsigned int m_lastItemPositionW;		// position in words in lastBuffer of the last item written
+	void clear();
+	CacheBuffer* allocBuffer();
+	CacheItem* findItemOrLater(unsigned int timestamp, CacheBuffer **rBuffer);
+	CacheItem* findItemEarlier(unsigned int timestamp, CacheBuffer **rBuffer);
+	// Internal function: finds an item in a buffer that is < timeOffset
+	// timeOffset must be a valid offset for the buffer and the buffer must not be empty
+	// on return highBlock contains the block with items above or equal to timeOffset
+	CacheItem *_findBlock(CacheBuffer *buffer, unsigned short timeOffset, unsigned int *highBlock);
+};
+
+struct CacheBlock {
+	unsigned short m_timeOffset;		// timestamp relative to m_firstTimestamp
+	unsigned short m_offsetW;			// position in words of item relative to start of block
+};
+
+/* CacheItem is the header of each item in the buffer, must be 32bit
+   Items are always 32 bits aligned and size is the number of 32 bit words until the
+   next item header, including an eventual pre and post padding gap for pointer alignment */
+struct CacheItem
+{
+	unsigned short m_timeOffset;		// timestamp relative to m_firstTimestamp
+	unsigned short m_sizeW;			// size of item in 32 bit words
+	// item data follows header immediately or after a gap if position is not pointer aligned
+};
+
+// Buffer header
+// Defined in a macro to avoid sizeof() potential problem.
+//	next				for linked list. = NULL for last buffer
+//  m_firstTimestamp		timestamp of first item in this buffer
+//  m_lastTimestamp		timestamp of last item in this buffer
+//						m_lastTimestamp must be < m_firstTimestamp+65536
+//  m_lastItemPositionW	position in word of last item written
+//  m_firstFreePositionW	position in word where a new item can be written, 0 if buffer is empty
+//  lookup				lookup table for fast access to item by timestamp
+//						The buffer is divided in blocks of 2**n bytes with n chosen so that 
+//						there are no more than CACHE_LOOKUP_TABLE_SIZE blocks and that each 
+//						block will contain at least one item. 
+//						Each element of the lookup table gives the timestamp and offset 
+//						of the last cache item occupying (=starting in) the corresponding block.
+#define CACHE_HEADER \
+	struct CacheBuffer *m_next;		\
+	unsigned int m_firstTimestamp;	\
+	unsigned int m_lastTimestamp;		\
+									\
+	unsigned int m_lastItemPositionW;	\
+	unsigned int m_firstFreePositionW;\
+	struct CacheBlock lookup[CACHE_LOOKUP_TABLE_SIZE]
+
+struct CacheBufferHeader {
+	CACHE_HEADER;
+};
+#define CACHE_BUFFER_HEADER_SIZE	(sizeof(struct CacheBufferHeader))
+struct CacheBuffer
+{
+	CACHE_HEADER;
+	struct CacheItem m_firstItem;			// the address of this field marks the start of the buffer
+};
+
+
+}
+
+#endif /* CACHE_HPP_ */
diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp
new file mode 100644
index 00000000000..f47af4246e4
--- /dev/null
+++ b/intern/itasc/ConstraintSet.cpp
@@ -0,0 +1,170 @@
+/* $Id: ConstraintSet.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * ConstraintSet.cpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#include "ConstraintSet.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+
+namespace iTaSC {
+
+ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
+    m_nc(_nc),
+    m_Jf(e_identity_matrix(6,6)),
+    m_Cf(e_zero_matrix(m_nc,6)),
+    m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
+    m_Jf_inv(e_zero_matrix(6,6)),
+    m_Wy(e_scalar_vector(m_nc,1.0)),
+    m_chi(e_zero_vector(6)),m_y(m_nc),m_ydot(e_zero_vector(m_nc)),
+    m_S(6),m_temp(6),m_tdelta(6),
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+    m_threshold(accuracy),m_maxIter(maximum_iterations)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+ConstraintSet::ConstraintSet():
+    m_nc(0), 
+	m_internalPose(F_identity), m_externalPose(F_identity),
+	m_constraintCallback(NULL), m_constraintParam(NULL), 
+	m_toggle(false),m_substep(false),
+	m_threshold(0.0),m_maxIter(0)
+{
+	m_maxDeltaChi = e_scalar(0.52);
+}
+
+void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
+{
+    m_nc = _nc;
+    m_Jf = e_identity_matrix(6,6);
+    m_Cf = e_zero_matrix(m_nc,6);
+    m_U = e_identity_matrix(6,6);
+	m_V = e_identity_matrix(6,6);
+	m_B = e_zero_matrix(6,6);
+    m_Jf_inv = e_zero_matrix(6,6),
+    m_Wy = e_scalar_vector(m_nc,1.0),
+    m_chi = e_zero_vector(6);
+    m_chidot = e_zero_vector(6);
+	m_y = e_zero_vector(m_nc);
+	m_ydot = e_zero_vector(m_nc);
+	m_S = e_zero_vector(6);
+	m_temp = e_zero_vector(6);
+	m_tdelta = e_zero_vector(6);
+    m_threshold = accuracy;
+	m_maxIter = maximum_iterations;
+}
+
+ConstraintSet::~ConstraintSet() {
+
+}
+
+void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
+{
+    m_chi+=m_chidot*timestamp.realTimestep;
+	m_externalPose = _external_pose;
+
+    //update the internal pose and Jf
+    updateJacobian();
+	//check if loop is already closed, if not update the pose and Jf
+    unsigned int iter=0;
+    while(iter<5&&!closeLoop())
+        iter++;
+}
+
+double ConstraintSet::getMaxTimestep(double& timestep)
+{
+	e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
+	if (timestep*maxChidot > m_maxDeltaChi) {
+		timestep = m_maxDeltaChi/maxChidot;
+	}
+	return timestep;
+}
+
+bool ConstraintSet::initialise(Frame& init_pose){
+    m_externalPose=init_pose;
+	// get current Jf
+    updateJacobian();
+
+    unsigned int iter=0;
+    while(iter<m_maxIter&&!closeLoop()){
+        iter++;
+    }
+    if (iter<m_maxIter)
+        return true;
+    else
+        return false;
+}
+
+bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
+{
+	ConstraintValues values;
+	ConstraintSingleValue value;
+	values.values = &value;
+	values.number = 0;
+	values.action = action;
+	values.id = id;
+	value.action = action;
+	value.id = id;
+	switch (action) {
+	case ACT_NONE:
+		return true;
+	case ACT_VALUE:
+		value.yd = data;
+		values.number = 1;
+		break;
+	case ACT_VELOCITY:
+		value.yddot = data;
+		values.number = 1;
+		break;
+	case ACT_TOLERANCE:
+		values.tolerance = data;
+		break;
+	case ACT_FEEDBACK:
+		values.feedback = data;
+		break;
+	case ACT_ALPHA:
+		values.alpha = data;
+		break;
+	default:
+		assert(action==ACT_NONE);
+	}
+	return setControlParameters(&values, 1, timestep);
+}
+
+bool ConstraintSet::closeLoop(){
+    //Invert Jf
+    //TODO: svd_boost_Macie has problems if Jf contains zero-rows
+    //toggle=!toggle;
+    //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
+	int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
+    if(ret<0)
+        return false;
+
+	// the reference point and frame of the jacobian is the base frame
+	// m_externalPose-m_internalPose is the twist to extend the end effector
+	// to get the required pose => change the reference point to the base frame
+	Twist twist_delta(diff(m_internalPose,m_externalPose));
+	twist_delta=twist_delta.RefPoint(-m_internalPose.p);
+    for(unsigned int i=0;i<6;i++)
+        m_tdelta(i)=twist_delta(i);
+    //TODO: use damping in constraintset inversion?
+    for(unsigned int i=0;i<6;i++)
+        if(m_S(i)<m_threshold){
+			m_B.row(i).setConstant(0.0);
+        }else
+            m_B.row(i) = m_U.col(i)/m_S(i);
+
+    m_Jf_inv=(m_V*m_B).lazy();
+
+    m_chi+=(m_Jf_inv*m_tdelta).lazy();
+    updateJacobian();
+	// m_externalPose-m_internalPose in end effector frame
+	// this is just to compare the pose, a different formula would work too
+	return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
+
+}
+}
diff --git a/intern/itasc/ConstraintSet.hpp b/intern/itasc/ConstraintSet.hpp
new file mode 100644
index 00000000000..12bb085c911
--- /dev/null
+++ b/intern/itasc/ConstraintSet.hpp
@@ -0,0 +1,115 @@
+/* $Id: ConstraintSet.hpp 20307 2009-05-20 20:39:18Z ben2610 $
+ * ConstraintSet.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef CONSTRAINTSET_HPP_
+#define CONSTRAINTSET_HPP_
+
+#include "kdl/frames.hpp"
+#include "eigen_types.hpp"
+#include "Cache.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+enum ConstraintAction {
+	ACT_NONE=		0,
+	ACT_VALUE=		1,
+	ACT_VELOCITY=	2,
+	ACT_TOLERANCE=	4,
+	ACT_FEEDBACK=	8,
+	ACT_ALPHA=		16
+};
+
+struct ConstraintSingleValue {
+	unsigned int id;	// identifier of constraint value, depends on constraint
+	unsigned int action;// action performed, compbination of ACT_..., set on return
+	const double y;		// actual constraint value
+	const double ydot;	// actual constraint velocity
+	double yd;			// current desired constraint value, changed on return
+	double yddot;		// current desired constraint velocity, changed on return
+	ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
+};
+
+struct ConstraintValues {
+	unsigned int id;	// identifier of group of constraint values, depend on constraint
+	unsigned short number;		// number of constraints in list
+	unsigned short action;		// action performed, ACT_..., set on return
+	double alpha;		// constraint activation coefficient, should be [0..1]
+	double tolerance;	// current desired tolerance on constraint, same unit than yd, changed on return
+	double feedback;	// current desired feedback on error, in 1/sec, changed on return
+	struct ConstraintSingleValue* values;
+	ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
+};
+
+class ConstraintSet;
+typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
+
+class ConstraintSet {
+protected:
+    unsigned int m_nc;
+	e_scalar m_maxDeltaChi;
+	e_matrix m_Cf;
+    e_vector m_Wy,m_y,m_ydot;
+	e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
+    e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
+	KDL::Frame m_internalPose,m_externalPose;
+    ConstraintCallback m_constraintCallback;
+    void* m_constraintParam;
+	void* m_poseParam;
+    bool m_toggle;
+	bool m_substep;
+    double m_threshold;
+    unsigned int m_maxIter;
+
+	friend class Scene;
+	virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
+    virtual void updateKinematics(const Timestamp& timestamp)=0;
+    virtual void pushCache(const Timestamp& timestamp)=0;
+    virtual void updateJacobian()=0;
+    virtual void updateControlOutput(const Timestamp& timestamp)=0;
+	virtual void initCache(Cache *_cache) = 0;
+	virtual bool initialise(KDL::Frame& init_pose);
+	virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    virtual bool closeLoop();
+	virtual double getMaxTimestep(double& timestep);
+
+
+public:
+    ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    ConstraintSet();
+    virtual ~ConstraintSet();
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	virtual bool registerCallback(ConstraintCallback _function, void* _param)
+	{
+		m_constraintCallback = _function;
+		m_constraintParam = _param;
+		return true;
+	}
+
+    virtual const e_vector& getControlOutput()const{return m_ydot;};
+	virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
+	virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
+	bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
+
+    virtual const e_matrix6& getJf() const{return m_Jf;};
+	virtual const KDL::Frame& getPose() const{return m_internalPose;};
+    virtual const e_matrix& getCf() const{return m_Cf;};
+
+    virtual const e_vector& getWy() const {return m_Wy;};
+    virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
+    virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
+
+    virtual unsigned int getNrOfConstraints(){return m_nc;};
+	void substep(bool _substep) {m_substep=_substep;}
+	bool substep() {return m_substep;}
+};
+
+}
+
+#endif /* CONSTRAINTSET_HPP_ */
diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp
new file mode 100644
index 00000000000..f9e819d2950
--- /dev/null
+++ b/intern/itasc/ControlledObject.cpp
@@ -0,0 +1,61 @@
+/* $Id: ControlledObject.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * ControlledObject.cpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#include "ControlledObject.hpp"
+
+
+namespace iTaSC {
+ControlledObject::ControlledObject():
+    Object(Controlled),m_nq(0),m_nc(0),m_nee(0)
+{
+	// max joint variable = 0.52 radian or 0.52 meter in one timestep
+	m_maxDeltaQ = e_scalar(0.52);
+}
+
+void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee)
+{
+	assert(_nee >= 1);
+	m_nq = _nq;
+	m_nc = _nc;
+	m_nee = _nee;
+	if (m_nq > 0) {
+		m_Wq =  e_identity_matrix(m_nq,m_nq);
+		m_qdot = e_zero_vector(m_nq);
+	}
+	if (m_nc > 0) {
+		m_Wy = e_scalar_vector(m_nc,1.0);
+		m_ydot = e_zero_vector(m_nc);
+	}
+	if (m_nc > 0 && m_nq > 0)
+		m_Cq = e_zero_matrix(m_nc,m_nq);
+	// clear all Jacobian if any
+	m_JqArray.clear();
+	// reserve one more to have a zero matrix handy
+	if (m_nq > 0)
+		m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq));
+}
+
+ControlledObject::~ControlledObject() {}
+
+
+
+const e_matrix& ControlledObject::getJq(unsigned int ee) const
+{
+	assert(m_nq > 0);
+	return m_JqArray[(ee>m_nee)?m_nee:ee];
+}
+
+double ControlledObject::getMaxTimestep(double& timestep)
+{
+	e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff();
+	if (timestep*maxQdot > m_maxDeltaQ) {
+		timestep = m_maxDeltaQ/maxQdot;
+	}
+	return timestep;
+}
+
+}
diff --git a/intern/itasc/ControlledObject.hpp b/intern/itasc/ControlledObject.hpp
new file mode 100644
index 00000000000..2370f6594ed
--- /dev/null
+++ b/intern/itasc/ControlledObject.hpp
@@ -0,0 +1,70 @@
+/* $Id: ControlledObject.hpp 20853 2009-06-13 12:29:46Z ben2610 $
+ * ControlledObject.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef CONTROLLEDOBJECT_HPP_
+#define CONTROLLEDOBJECT_HPP_
+
+#include "kdl/frames.hpp"
+#include "eigen_types.hpp"
+
+#include "Object.hpp"
+#include "ConstraintSet.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+#define CONSTRAINT_ID_ALL	((unsigned int)-1)
+
+class ControlledObject : public Object {
+protected:
+	e_scalar m_maxDeltaQ;
+    unsigned int m_nq,m_nc,m_nee;
+    e_matrix m_Wq,m_Cq;
+    e_vector m_Wy,m_ydot,m_qdot;
+	std::vector<e_matrix> m_JqArray;
+public:
+    ControlledObject();
+    virtual ~ControlledObject();
+
+	class JointLockCallback {
+	public:
+		JointLockCallback() {}
+		virtual ~JointLockCallback() {}
+
+		// lock a joint, no need to update output
+		virtual void lockJoint(unsigned int q_nr, unsigned int ndof) = 0;
+		// lock a joint and update output in view of reiteration
+		virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot) = 0;
+	};
+
+	virtual void initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee);
+
+	// returns true when a joint has been locked via the callback and the solver must run again
+	virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback) = 0;
+	virtual void updateControlOutput(const Timestamp& timestamp)=0;
+    virtual void setJointVelocity(const e_vector qdot_in){m_qdot = qdot_in;};
+	virtual double getMaxTimestep(double& timestep);
+	virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, e_scalar value, double timestep=0.0)=0;
+
+    virtual const e_vector& getControlOutput() const{return m_ydot;}
+
+	virtual const e_matrix& getJq(unsigned int ee) const;
+
+    virtual const e_matrix& getCq() const{return m_Cq;};
+
+    virtual e_matrix& getWq() {return m_Wq;};
+    virtual void setWq(const e_matrix& Wq_in){m_Wq = Wq_in;};
+
+    virtual const e_vector& getWy() const {return m_Wy;};
+
+    virtual const unsigned int getNrOfCoordinates(){return m_nq;};
+    virtual const unsigned int getNrOfConstraints(){return m_nc;};
+};
+
+}
+
+#endif /* CONTROLLEDOBJECT_HPP_ */
diff --git a/intern/itasc/CopyPose.cpp b/intern/itasc/CopyPose.cpp
new file mode 100644
index 00000000000..df0bc38b704
--- /dev/null
+++ b/intern/itasc/CopyPose.cpp
@@ -0,0 +1,481 @@
+/* $Id: CopyPose.cpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * CopyPose.cpp
+ *
+ *  Created on: Mar 17, 2009
+ *      Author: benoit bolsee
+ */
+
+#include "CopyPose.hpp"
+#include "kdl/kinfam_io.hpp"
+#include <math.h>
+#include <malloc.h>
+#include <string.h>
+
+namespace iTaSC
+{
+
+const unsigned int maxPoseCacheSize = (2*(3+3*2));
+CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
+    ConstraintSet(),
+	m_cache(NULL),
+    m_poseCCh(-1),m_poseCTs(0)
+{
+	m_maxerror = armlength/2.0;
+	m_outputControl = (control_output & CTL_ALL);
+	int _nc = nBitsOn(m_outputControl);
+	if (!_nc) 
+		return;
+	// reset the constraint set
+	reset(_nc, accuracy, maximum_iterations);
+	_nc = 0;
+	m_nvalues = 0;
+	int nrot = 0, npos = 0;
+	int nposCache = 0, nrotCache = 0;
+	m_outputDynamic = (dynamic_output & m_outputControl);
+	memset(m_values, 0, sizeof(m_values));
+	memset(m_posData, 0, sizeof(m_posData));
+	memset(m_rotData, 0, sizeof(m_rotData));
+	memset(&m_rot, 0, sizeof(m_rot));
+	memset(&m_pos, 0, sizeof(m_pos));
+	if (m_outputControl & CTL_POSITION) {
+		m_pos.alpha = 1.0;		
+		m_pos.K = 20.0;		
+		m_pos.tolerance = 0.05;	
+		m_values[m_nvalues].alpha = m_pos.alpha;
+		m_values[m_nvalues].feedback = m_pos.K;
+		m_values[m_nvalues].tolerance = m_pos.tolerance;
+		m_values[m_nvalues].id = ID_POSITION;
+		if (m_outputControl & CTL_POSITIONX) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,0)=1.0;
+			m_posData[npos++].id = ID_POSITIONX;
+			if (m_outputDynamic & CTL_POSITIONX)
+				nposCache++;
+		} 
+		if (m_outputControl & CTL_POSITIONY) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,1)=1.0;
+			m_posData[npos++].id = ID_POSITIONY;
+			if (m_outputDynamic & CTL_POSITIONY)
+				nposCache++;
+		}
+		if (m_outputControl & CTL_POSITIONZ) {
+		    m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
+			m_Cf(_nc++,2)=1.0;
+			m_posData[npos++].id = ID_POSITIONZ;
+			if (m_outputDynamic & CTL_POSITIONZ)
+				nposCache++;
+		}
+		m_values[m_nvalues].number = npos;
+		m_values[m_nvalues++].values = m_posData;
+		m_pos.firsty = 0;
+		m_pos.ny = npos;
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		m_rot.alpha = 1.0;		
+		m_rot.K = 20.0;		
+		m_rot.tolerance = 0.05;	
+		m_values[m_nvalues].alpha = m_rot.alpha;
+		m_values[m_nvalues].feedback = m_rot.K;
+		m_values[m_nvalues].tolerance = m_rot.tolerance;
+		m_values[m_nvalues].id = ID_ROTATION;
+		if (m_outputControl & CTL_ROTATIONX) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,3)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONX;
+			if (m_outputDynamic & CTL_ROTATIONX)
+				nrotCache++;
+		}
+		if (m_outputControl & CTL_ROTATIONY) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,4)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONY;
+			if (m_outputDynamic & CTL_ROTATIONY)
+				nrotCache++;
+		}
+		if (m_outputControl & CTL_ROTATIONZ) {
+		    m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
+			m_Cf(_nc++,5)=1.0;
+			m_rotData[nrot++].id = ID_ROTATIONZ;
+			if (m_outputDynamic & CTL_ROTATIONZ)
+				nrotCache++;
+		}
+		m_values[m_nvalues].number = nrot;
+		m_values[m_nvalues++].values = m_rotData;
+		m_rot.firsty = npos;
+		m_rot.ny = nrot;
+	}
+	assert(_nc == m_nc);
+    m_Jf=e_identity_matrix(6,6);
+	m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
+}
+
+CopyPose::~CopyPose()
+{
+}
+
+bool CopyPose::initialise(Frame& init_pose)
+{
+    m_externalPose = m_internalPose = init_pose;
+	updateJacobian();
+    return true;
+}
+
+void CopyPose::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
+{
+	m_internalPose = m_externalPose = _external_pose;
+	updateJacobian();
+}
+
+void CopyPose::initCache(Cache *_cache)
+{
+    m_cache = _cache;
+    m_poseCCh = -1;
+    if (m_cache) {
+        // create one channel for the coordinates
+        m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double));
+        // don't save initial value, it will be recomputed from external pose
+        //pushPose(0);
+    }
+}
+
+double* CopyPose::pushValues(double* item, ControlState* _state, unsigned int mask)
+{
+	ControlState::ControlValue* _yval;
+	int i;
+
+	*item++ = _state->alpha;
+	*item++ = _state->K;
+	*item++ = _state->tolerance;
+
+	for (i=0, _yval=_state->output; i<_state->ny; mask<<=1) {
+		if (m_outputControl & mask) {
+			if (m_outputDynamic & mask) {
+				*item++ = _yval->yd;
+				*item++ = _yval->yddot;
+			}
+			_yval++;
+			i++;
+		}
+	}
+	return item;
+}
+
+void CopyPose::pushPose(CacheTS timestamp)
+{
+    if (m_poseCCh >= 0) {
+		if (m_poseCacheSize) {
+			double buf[maxPoseCacheSize];
+			double *item = buf;
+			if (m_outputDynamic & CTL_POSITION)
+				item = pushValues(item, &m_pos, CTL_POSITIONX);
+			if (m_outputDynamic & CTL_ROTATION)
+				item = pushValues(item, &m_rot, CTL_ROTATIONX);
+			m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon);
+		} else
+			m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon);
+		m_poseCTs = timestamp;
+    }
+}
+
+double* CopyPose::restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask)
+{
+	ConstraintSingleValue* _data;
+	ControlState::ControlValue* _yval;
+	int i, j;
+
+	_values->alpha = _state->alpha = *item++;
+	_values->feedback = _state->K = *item++;
+	_values->tolerance = _state->tolerance = *item++;
+
+	for (i=_state->firsty, j=i+_state->ny, _yval=_state->output, _data=_values->values; i<j; mask<<=1) {
+		if (m_outputControl & mask) {
+			m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
+			if (m_outputDynamic & mask) {
+				_data->yd = _yval->yd = *item++;
+				_data->yddot = _yval->yddot = *item++;
+			}
+			_data++;
+			_yval++;
+			i++;
+		}
+	}
+	return item;
+}
+
+bool CopyPose::popPose(CacheTS timestamp)
+{
+	bool found = false;
+    if (m_poseCCh >= 0) {
+        double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
+		if (item) {
+			found = true;
+			if (timestamp != m_poseCTs) {
+				int i=0;
+				if (m_outputControl & CTL_POSITION) {
+					if (m_outputDynamic & CTL_POSITION) {
+						item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
+					}
+					i++;
+				}
+				if (m_outputControl & CTL_ROTATION) {
+					if (m_outputDynamic & CTL_ROTATION) {
+						item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
+					}
+					i++;
+				}
+				m_poseCTs = timestamp;
+				item = NULL;
+			}
+        }
+    }
+    return found;
+}
+
+void CopyPose::interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp)
+{
+	ControlState::ControlValue* _yval;
+	int i;
+
+	for (i=0, _yval=_state->output; i<_state->ny; mask <<= 1) {
+		if (m_outputControl & mask) {
+			if (m_outputDynamic & mask) {
+				if (timestamp.substep && timestamp.interpolate) {
+					_yval->yd += _yval->yddot*timestamp.realTimestep;
+				} else {
+					_yval->yd = _yval->nextyd;
+					_yval->yddot = _yval->nextyddot;
+				}
+			}
+			i++;
+			_yval++;
+		}
+	}
+}
+
+void CopyPose::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache) {
+        pushPose(timestamp.cacheTimestamp);
+	}
+}
+
+void CopyPose::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		if (m_outputDynamic & CTL_POSITION)
+			interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
+		if (m_outputDynamic & CTL_ROTATION)
+			interpolateOutput(&m_rot, CTL_ROTATIONX, timestamp);
+	}
+	pushCache(timestamp);
+}
+
+void CopyPose::updateJacobian()
+{
+    //Jacobian is always identity at the start of the constraint chain
+	//instead of going through complicated jacobian operation, implemented direct formula
+	//m_Jf(1,3) = m_internalPose.p.z();
+	//m_Jf(2,3) = -m_internalPose.p.y();
+	//m_Jf(0,4) = -m_internalPose.p.z();
+	//m_Jf(2,4) = m_internalPose.p.x();
+	//m_Jf(0,5) = m_internalPose.p.y();
+	//m_Jf(1,5) = -m_internalPose.p.x();
+}
+
+void CopyPose::updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep)
+{
+	int id = (mask == CTL_ROTATIONX) ? ID_ROTATIONX : ID_POSITIONX;
+	ControlState::ControlValue* _yval;
+	ConstraintSingleValue* _data;
+	int i, j, k;
+    int action = 0;
+
+    if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
+        _state->alpha = _values->alpha;
+        action |= ACT_ALPHA;
+    }
+    if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
+        _state->tolerance = _values->tolerance;
+        action |= ACT_TOLERANCE;
+    }
+    if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
+        _state->K = _values->feedback;
+        action |= ACT_FEEDBACK;
+    }
+	for (i=_state->firsty, j=_state->firsty+_state->ny, _yval=_state->output; i<j; mask <<= 1, id++) {
+		if (m_outputControl & mask) {
+			if (action)
+				m_Wy(i) = _state->alpha/*/(_state->tolerance*_state->K)*/;
+			// check if this controlled output is provided
+			for (k=0, _data=_values->values; k<_values->number; k++, _data++) {
+				if (_data->id == id) {
+					switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
+					case 0:
+						// no indication, keep current values
+						break;
+					case ACT_VELOCITY:
+						// only the velocity is given estimate the new value by integration
+						_data->yd = _yval->yd+_data->yddot*timestep;
+						// walkthrough
+					case ACT_VALUE:
+						_yval->nextyd = _data->yd;
+						// if the user sets the value, we assume future velocity is zero
+						// (until the user changes the value again)
+						_yval->nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
+						if (timestep>0.0) {
+							_yval->yddot = (_data->yd-_yval->yd)/timestep;
+						} else {
+							// allow the user to change target instantenously when this function
+							// if called from setControlParameter with timestep = 0
+							_yval->yd = _yval->nextyd;
+							_yval->yddot = _yval->nextyddot;
+						}
+						break;
+					case (ACT_VALUE|ACT_VELOCITY):
+						// the user should not set the value and velocity at the same time.
+						// In this case, we will assume that he wants to set the future value
+						// and we compute the current value to match the velocity
+						_yval->yd = _data->yd - _data->yddot*timestep;
+						_yval->nextyd = _data->yd;
+						_yval->nextyddot = _data->yddot;
+						if (timestep>0.0) {
+							_yval->yddot = (_data->yd-_yval->yd)/timestep;
+						} else {
+							_yval->yd = _yval->nextyd;
+							_yval->yddot = _yval->nextyddot;
+						}
+						break;
+					}
+				}
+			}
+			_yval++;
+			i++;
+		}
+	}
+}
+
+
+bool CopyPose::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
+{
+	while (_nvalues > 0) {
+		if (_values->id >= ID_POSITION && _values->id <= ID_POSITIONZ && (m_outputControl & CTL_POSITION)) {
+			updateState(_values, &m_pos, CTL_POSITIONX, timestep);
+		} 
+		if (_values->id >= ID_ROTATION && _values->id <= ID_ROTATIONZ && (m_outputControl & CTL_ROTATION)) {
+			updateState(_values, &m_rot, CTL_ROTATIONX, timestep);
+		}
+		_values++;
+		_nvalues--;
+	}
+    return true;
+}
+
+void CopyPose::updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask)
+{
+	ConstraintSingleValue* _data;
+	ControlState::ControlValue* _yval;
+	int i, j;
+
+	_values->action = 0;
+
+	for (i=_state->firsty, j=0, _yval=_state->output, _data=_values->values; j<3; j++, mask<<=1) {
+		if (m_outputControl & mask) {
+			*(double*)&_data->y = vel(j);
+			*(double*)&_data->ydot = m_ydot(i);
+			_data->yd = _yval->yd;
+			_data->yddot = _yval->yddot;
+			_data->action = 0;
+			i++;
+			_data++;
+			_yval++;
+		}
+	}
+}
+
+void CopyPose::updateOutput(Vector& vel, ControlState* _state, unsigned int mask)
+{
+	ControlState::ControlValue* _yval;
+	int i, j;
+	double coef=1.0;
+	if (mask & CTL_POSITION) {
+		// put a limit on position error
+		double len=0.0;
+		for (j=0, _yval=_state->output; j<3; j++) {
+			if (m_outputControl & (mask<<j)) {
+				len += KDL::sqr(_yval->yd-vel(j));
+				_yval++;
+			}
+		}
+		len = KDL::sqrt(len);
+		if (len > m_maxerror)
+			coef = m_maxerror/len;
+	}
+	for (i=_state->firsty, j=0, _yval=_state->output; j<3; j++) {
+		if (m_outputControl & (mask<<j)) {
+			m_ydot(i)=_yval->yddot+_state->K*coef*(_yval->yd-vel(j));
+			_yval++;
+			i++;
+		}
+	}
+}
+
+void CopyPose::updateControlOutput(const Timestamp& timestamp)
+{
+    //IMO this should be done, no idea if it is enough (wrt Distance impl)
+	Twist y = diff(F_identity, m_internalPose);
+	bool found = true;
+	if (!timestamp.substep) {
+		if (!timestamp.reiterate) {
+			found = popPose(timestamp.cacheTimestamp);
+		}
+	}
+	if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
+		// initialize first callback the application to get the current values
+		int i=0;
+		if (m_outputControl & CTL_POSITION) {
+			updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
+		}
+		if (m_outputControl & CTL_ROTATION) {
+			updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
+		}
+		if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
+			setControlParameters(m_values, m_nvalues, (found && timestamp.interpolate)?timestamp.realTimestep:0.0);
+		}
+	}
+	if (m_outputControl & CTL_POSITION) {
+		updateOutput(y.vel, &m_pos, CTL_POSITIONX);
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		updateOutput(y.rot, &m_rot, CTL_ROTATIONX);
+	}
+}
+
+const ConstraintValues* CopyPose::getControlParameters(unsigned int* _nvalues)
+{
+	Twist y = diff(m_internalPose,F_identity);
+	int i=0;
+	if (m_outputControl & CTL_POSITION) {
+		updateValues(y.vel, &m_values[i++], &m_pos, CTL_POSITIONX);
+	}
+	if (m_outputControl & CTL_ROTATION) {
+		updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
+	}
+	if (_nvalues)
+		*_nvalues=m_nvalues; 
+	return m_values; 
+}
+
+double CopyPose::getMaxTimestep(double& timestep)
+{
+	// CopyPose should not have any limit on linear velocity: 
+	// in case the target is out of reach, this can be very high.
+	// We will simply limit on rotation
+	e_scalar maxChidot = m_chidot.block(3,0,3,1).cwise().abs().maxCoeff();
+	if (timestep*maxChidot > m_maxDeltaChi) {
+		timestep = m_maxDeltaChi/maxChidot;
+	}
+	return timestep;
+}
+
+}
diff --git a/intern/itasc/CopyPose.hpp b/intern/itasc/CopyPose.hpp
new file mode 100644
index 00000000000..3a3f60a9f37
--- /dev/null
+++ b/intern/itasc/CopyPose.hpp
@@ -0,0 +1,99 @@
+/* $Id: CopyPose.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * CopyPose.h
+ *
+ *  Created on: Mar 17, 2009
+ *      Author: benoit bolsee
+ */
+
+#ifndef COPYPOSE_H_
+#define COPYPOSE_H_
+
+#include "ConstraintSet.hpp"
+namespace iTaSC{
+
+using namespace KDL;
+
+class CopyPose: public iTaSC::ConstraintSet
+{
+protected:
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateJacobian();
+    virtual bool initialise(Frame& init_pose);
+    virtual void initCache(Cache *_cache);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
+	virtual double getMaxTimestep(double& timestep);
+
+public:
+    enum ID {		// constraint ID in callback and setControlParameter
+		ID_POSITION=0,
+        ID_POSITIONX=1,
+        ID_POSITIONY=2,
+        ID_POSITIONZ=3,
+		ID_ROTATION=4,
+        ID_ROTATIONX=5,
+        ID_ROTATIONY=6,
+        ID_ROTATIONZ=7,
+    };
+	enum CTL {		// control ID in constructor to specify which output is constrainted
+		CTL_NONE=0x00,
+        CTL_POSITIONX=0x01,		// the bit order is important: it matches the y output order
+        CTL_POSITIONY=0x02,
+        CTL_POSITIONZ=0x04,
+		CTL_POSITION=0x07,
+        CTL_ROTATIONX=0x08,
+        CTL_ROTATIONY=0x10,
+        CTL_ROTATIONZ=0x20,
+		CTL_ROTATION=0x38,
+		CTL_ALL=0x3F,
+	};
+
+	// use a combination of CTL_.. in control_output to specify which 
+    CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
+    virtual ~CopyPose();
+
+    virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
+    virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
+
+private:
+    struct ConstraintSingleValue m_posData[3];	// index = controlled output in X,Y,Z order
+    struct ConstraintSingleValue m_rotData[3];
+    struct ConstraintValues m_values[2];		// index = group of controlled output, in position, rotation order
+    Cache* m_cache;
+    int m_poseCCh;
+    CacheTS m_poseCTs;
+	unsigned int m_poseCacheSize;
+	unsigned int m_outputDynamic;	// combination of CTL_... determine which variables are dynamically controlled by the application
+	unsigned int m_outputControl;	// combination of CTL_... determine which output are constrained
+	unsigned int m_nvalues;		// number of elements used in m_values[]
+	double m_maxerror;
+
+	struct ControlState {
+		int firsty;			// first y index
+		int ny;				// number of y in output
+		double alpha;
+		double K;
+		double tolerance;
+		struct ControlValue {
+			double yddot;
+			double yd;
+			double nextyd;
+			double nextyddot;
+		} output[3];		// inded numbex = same as m_rotData
+	} m_rot, m_pos;
+
+    void pushPose(CacheTS timestamp);
+    bool popPose(CacheTS timestamp);
+	int nBitsOn(unsigned int v)
+		{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
+	double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	double* pushValues(double* item, ControlState* _state, unsigned int mask);
+	void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
+	void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
+	void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
+	void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
+
+};
+}
+#endif /* COPYROTATION_H_ */
diff --git a/intern/itasc/Distance.cpp b/intern/itasc/Distance.cpp
new file mode 100644
index 00000000000..03fa1762567
--- /dev/null
+++ b/intern/itasc/Distance.cpp
@@ -0,0 +1,322 @@
+/* $Id: Distance.cpp 20603 2009-06-03 15:17:52Z ben2610 $
+ * Distance.cpp
+ *
+ *  Created on: Jan 30, 2009
+ *      Author: rsmits
+ */
+
+#include "Distance.hpp"
+#include "kdl/kinfam_io.hpp"
+#include <math.h>
+#include <malloc.h>
+#include <string.h>
+
+namespace iTaSC
+{
+// a distance constraint is characterized by 5 values: alpha, tolerance, K, yd, yddot
+static const unsigned int distanceCacheSize = sizeof(double)*5 + sizeof(e_scalar)*6;
+
+Distance::Distance(double armlength, double accuracy, unsigned int maximum_iterations):
+    ConstraintSet(1,accuracy,maximum_iterations),
+    m_chiKdl(6),m_jac(6),m_cache(NULL),
+	m_distCCh(-1),m_distCTs(0)
+{
+    m_chain.addSegment(Segment(Joint(Joint::RotZ)));
+    m_chain.addSegment(Segment(Joint(Joint::RotX)));
+    m_chain.addSegment(Segment(Joint(Joint::TransY)));
+    m_chain.addSegment(Segment(Joint(Joint::RotZ)));
+    m_chain.addSegment(Segment(Joint(Joint::RotY)));
+    m_chain.addSegment(Segment(Joint(Joint::RotX)));
+
+	m_fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
+	m_jacsolver = new KDL::ChainJntToJacSolver(m_chain);
+    m_Cf(0,2)=1.0;
+	m_alpha = 1.0;
+	m_tolerance = 0.05;
+	m_maxerror = armlength/2.0;
+	m_K = 20.0;
+	m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+	m_yddot = m_nextyddot = 0.0;
+	m_yd = m_nextyd = KDL::epsilon;
+	memset(&m_data, 0, sizeof(m_data));
+	// initialize the data with normally fixed values
+	m_data.id = ID_DISTANCE;
+	m_values.id = ID_DISTANCE;
+	m_values.number = 1;
+	m_values.alpha = m_alpha;
+	m_values.feedback = m_K;
+	m_values.tolerance = m_tolerance;
+	m_values.values = &m_data;
+}
+
+Distance::~Distance()
+{
+    delete m_fksolver;
+    delete m_jacsolver;
+}
+
+bool Distance::computeChi(Frame& pose)
+{
+	double dist, alpha, beta, gamma;
+	dist = pose.p.Norm();
+	Rotation basis;
+	if (dist < KDL::epsilon) {
+		// distance is almost 0, no need for initial rotation
+		m_chi(0) = 0.0;
+		m_chi(1) = 0.0;
+	} else {
+		// find the XZ angles that bring the Y axis to point to init_pose.p
+		Vector axis(pose.p/dist);
+		beta = 0.0;
+		if (fabs(axis(2)) > 1-KDL::epsilon) {
+			// direction is aligned on Z axis, just rotation on X
+			alpha = 0.0;
+			gamma = KDL::sign(axis(2))*KDL::PI/2;
+		} else {
+			alpha = -KDL::atan2(axis(0), axis(1));
+			gamma = KDL::atan2(axis(2), KDL::sqrt(KDL::sqr(axis(0))+KDL::sqr(axis(1))));
+		}
+		// rotation after first 2 joints
+		basis = Rotation::EulerZYX(alpha, beta, gamma);
+		m_chi(0) = alpha;
+		m_chi(1) = gamma;
+	}
+	m_chi(2) = dist;
+	basis = basis.Inverse()*pose.M;
+	basis.GetEulerZYX(alpha, beta, gamma);
+	// alpha = rotation on Z
+	// beta = rotation on Y
+	// gamma = rotation on X in that order
+	// it corresponds to the joint order, so just assign
+	m_chi(3) = alpha;
+	m_chi(4) = beta;
+	m_chi(5) = gamma;
+	return true;
+}
+
+bool Distance::initialise(Frame& init_pose)
+{
+	// we will initialize m_chi to values that match the pose
+    m_externalPose=init_pose;
+	computeChi(m_externalPose);
+	// get current Jf and update internal pose
+    updateJacobian();
+	return true;
+}
+
+bool Distance::closeLoop()
+{
+	if (!Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold)){
+		computeChi(m_externalPose);
+		updateJacobian();
+	}
+	return true;
+}
+
+void Distance::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_distCCh = -1;
+	if (m_cache) {
+		// create one channel for the coordinates
+		m_distCCh = m_cache->addChannel(this, "Xf", distanceCacheSize);
+		// save initial constraint in cache position 0
+		pushDist(0);
+	}
+}
+
+void Distance::pushDist(CacheTS timestamp)
+{
+	if (m_distCCh >= 0) {
+		double *item = (double*)m_cache->addCacheItem(this, m_distCCh, timestamp, NULL, distanceCacheSize);
+		if (item) {
+			*item++ = m_K;
+			*item++ = m_tolerance;
+			*item++ = m_yd;
+			*item++ = m_yddot;
+			*item++ = m_alpha;
+			memcpy(item, &m_chi[0], 6*sizeof(e_scalar));
+		}
+		m_distCTs = timestamp;
+	}
+}
+
+bool Distance::popDist(CacheTS timestamp)
+{
+	if (m_distCCh >= 0) {
+		double *item = (double*)m_cache->getPreviousCacheItem(this, m_distCCh, &timestamp);
+		if (item && timestamp != m_distCTs) {
+			m_values.feedback = m_K = *item++;
+			m_values.tolerance = m_tolerance = *item++;
+			m_yd = *item++;
+			m_yddot = *item++;
+			m_values.alpha = m_alpha = *item++;
+			memcpy(&m_chi[0], item, 6*sizeof(e_scalar));
+			m_distCTs = timestamp;
+			m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	return true;
+}
+
+void Distance::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache)
+		pushDist(timestamp.cacheTimestamp);
+}
+
+void Distance::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		//the internal pose and Jf is already up to date (see model_update)
+		//update the desired output based on yddot
+		if (timestamp.substep) {
+			m_yd += m_yddot*timestamp.realTimestep;
+			if (m_yd < KDL::epsilon)
+				m_yd = KDL::epsilon;
+		} else {
+			m_yd = m_nextyd;
+			m_yddot = m_nextyddot;
+		}
+	}
+	pushCache(timestamp);
+}
+
+void Distance::updateJacobian()
+{
+    for(unsigned int i=0;i<6;i++)
+        m_chiKdl(i)=m_chi(i);
+
+    m_fksolver->JntToCart(m_chiKdl,m_internalPose);
+    m_jacsolver->JntToJac(m_chiKdl,m_jac);
+    changeRefPoint(m_jac,-m_internalPose.p,m_jac);
+    for(unsigned int i=0;i<6;i++)
+        for(unsigned int j=0;j<6;j++)
+            m_Jf(i,j)=m_jac(i,j);
+}
+
+bool Distance::setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep)
+{
+	int action = 0;
+	int i;
+	ConstraintSingleValue* _data;
+
+	while (_nvalues > 0) {
+		if (_values->id == ID_DISTANCE) {
+			if ((_values->action & ACT_ALPHA) && _values->alpha >= 0.0) {
+				m_alpha = _values->alpha;
+				action |= ACT_ALPHA;
+			}
+			if ((_values->action & ACT_TOLERANCE) && _values->tolerance > KDL::epsilon) {
+				m_tolerance = _values->tolerance;
+				action |= ACT_TOLERANCE;
+			}
+			if ((_values->action & ACT_FEEDBACK) && _values->feedback > KDL::epsilon) {
+				m_K = _values->feedback;
+				action |= ACT_FEEDBACK;
+			}
+			for (_data = _values->values, i=0; i<_values->number; i++, _data++) {
+				if (_data->id == ID_DISTANCE) {
+					switch (_data->action & (ACT_VALUE|ACT_VELOCITY)) {
+					case 0:
+						// no indication, keep current values
+						break;
+					case ACT_VELOCITY:
+						// only the velocity is given estimate the new value by integration
+						_data->yd = m_yd+_data->yddot*timestep;
+						// walkthrough for negative value correction
+					case ACT_VALUE:
+						// only the value is given, estimate the velocity from previous value
+						if (_data->yd < KDL::epsilon)
+							_data->yd = KDL::epsilon;
+						m_nextyd = _data->yd;
+						// if the user sets the value, we assume future velocity is zero
+						// (until the user changes the value again)
+						m_nextyddot = (_data->action & ACT_VALUE) ? 0.0 : _data->yddot;
+						if (timestep>0.0) {
+							m_yddot = (_data->yd-m_yd)/timestep;
+						} else {
+							// allow the user to change target instantenously when this function
+							// if called from setControlParameter with timestep = 0
+							m_yddot = m_nextyddot;
+							m_yd = m_nextyd;
+						}
+						break;
+					case (ACT_VALUE|ACT_VELOCITY):
+						// the user should not set the value and velocity at the same time.
+						// In this case, we will assume that he want to set the future value
+						// and we compute the current value to match the velocity
+						if (_data->yd < KDL::epsilon)
+							_data->yd = KDL::epsilon;
+						m_yd = _data->yd - _data->yddot*timestep;
+						if (m_yd < KDL::epsilon)
+							m_yd = KDL::epsilon;
+						m_nextyd = _data->yd;
+						m_nextyddot = _data->yddot;
+						if (timestep>0.0) {
+							m_yddot = (_data->yd-m_yd)/timestep;
+						} else {
+							m_yd = m_nextyd;
+							m_yddot = m_nextyddot;
+						}
+						break;
+					}
+				}
+			}
+		}
+		_nvalues--;
+		_values++;
+	}
+	if (action & (ACT_TOLERANCE|ACT_FEEDBACK|ACT_ALPHA)) {
+		// recompute the weight
+		m_Wy(0) = m_alpha/*/(m_tolerance*m_K)*/;
+	}
+	return true;
+}
+
+const ConstraintValues* Distance::getControlParameters(unsigned int* _nvalues)
+{
+	*(double*)&m_data.y = m_chi(2);
+	*(double*)&m_data.ydot = m_ydot(0);
+	m_data.yd = m_yd;
+	m_data.yddot = m_yddot;
+	m_data.action = 0;
+	m_values.action = 0;
+	if (_nvalues) 
+		*_nvalues=1; 
+	return &m_values; 
+}
+
+void Distance::updateControlOutput(const Timestamp& timestamp)
+{
+	bool cacheAvail = true;
+	if (!timestamp.substep) {
+		if (!timestamp.reiterate)
+			cacheAvail = popDist(timestamp.cacheTimestamp);
+	}
+	if (m_constraintCallback && (m_substep || (!timestamp.reiterate && !timestamp.substep))) {
+		// initialize first callback the application to get the current values
+		*(double*)&m_data.y = m_chi(2);
+		*(double*)&m_data.ydot = m_ydot(0);
+		m_data.yd = m_yd;
+		m_data.yddot = m_yddot;
+		m_data.action = 0;
+		m_values.action = 0;
+		if ((*m_constraintCallback)(timestamp, &m_values, 1, m_constraintParam)) {
+			setControlParameters(&m_values, 1, timestamp.realTimestep);
+		}
+	}
+	if (!cacheAvail || !timestamp.interpolate) {
+		// first position in cache: set the desired output immediately as we cannot interpolate
+		m_yd = m_nextyd;
+		m_yddot = m_nextyddot;
+	}
+	double error = m_yd-m_chi(2);
+	if (KDL::Norm(error) > m_maxerror)
+		error = KDL::sign(error)*m_maxerror;
+    m_ydot(0)=m_yddot+m_K*error;
+}
+
+}
diff --git a/intern/itasc/Distance.hpp b/intern/itasc/Distance.hpp
new file mode 100644
index 00000000000..1366693743e
--- /dev/null
+++ b/intern/itasc/Distance.hpp
@@ -0,0 +1,62 @@
+/* $Id: Distance.hpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * Distance.hpp
+ *
+ *  Created on: Jan 30, 2009
+ *      Author: rsmits
+ */
+
+#ifndef DISTANCE_HPP_
+#define DISTANCE_HPP_
+
+#include "ConstraintSet.hpp"
+#include "kdl/chain.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
+
+namespace iTaSC
+{
+
+class Distance: public iTaSC::ConstraintSet
+{
+protected:
+    virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+    virtual void updateJacobian();
+    virtual bool initialise(Frame& init_pose);
+	virtual void initCache(Cache *_cache);
+    virtual void updateControlOutput(const Timestamp& timestamp);
+	virtual bool closeLoop();
+
+public:
+	enum ID {
+		ID_DISTANCE=1,
+	};
+    Distance(double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
+    virtual ~Distance();
+
+	virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
+	virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
+
+private:
+	bool computeChi(Frame& pose);
+    KDL::Chain m_chain;
+    KDL::ChainFkSolverPos_recursive* m_fksolver;
+    KDL::ChainJntToJacSolver* m_jacsolver;
+    KDL::JntArray m_chiKdl;
+    KDL::Jacobian m_jac;
+	struct ConstraintSingleValue m_data;
+	struct ConstraintValues m_values;
+	Cache* m_cache;
+	int m_distCCh;
+	CacheTS m_distCTs;
+	double m_maxerror;
+
+	void pushDist(CacheTS timestamp);
+	bool popDist(CacheTS timestamp);
+
+    double m_alpha,m_yddot,m_yd,m_nextyd,m_nextyddot,m_K,m_tolerance;
+};
+
+}
+
+#endif /* DISTANCE_HPP_ */
diff --git a/intern/itasc/FixedObject.cpp b/intern/itasc/FixedObject.cpp
new file mode 100644
index 00000000000..1360c3c152b
--- /dev/null
+++ b/intern/itasc/FixedObject.cpp
@@ -0,0 +1,70 @@
+/* $Id: FixedObject.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * FixedObject.cpp
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#include "FixedObject.hpp"
+
+namespace iTaSC{
+
+
+FixedObject::FixedObject():UncontrolledObject(),
+	m_finalized(false), m_nframe(0)
+{
+}
+
+FixedObject::~FixedObject() 
+{
+	m_frameArray.clear();
+}
+
+int FixedObject::addFrame(const std::string& name, const Frame& frame)
+{
+	if (m_finalized)
+		return -1;
+	FrameList::iterator it;
+	unsigned int i;
+	for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
+		if (it->first == name) {
+			// this frame will replace the old frame
+			it->second = frame;
+			return i;
+		}
+	}
+	m_frameArray.push_back(FrameList::value_type(name,frame));
+	return m_nframe++;
+}
+
+int FixedObject::addEndEffector(const std::string& name)
+{
+	// verify that this frame name exist
+	FrameList::iterator it;
+	unsigned int i;
+	for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
+		if (it->first == name) {
+			return i;
+		}
+	}
+	return -1;
+}
+
+void FixedObject::finalize()
+{
+	if (m_finalized)
+		return;
+	initialize(0, m_nframe);
+	m_finalized = true;
+}
+
+const Frame& FixedObject::getPose(const unsigned int frameIndex)
+{
+	if (frameIndex < m_nframe) {
+		return m_frameArray[frameIndex].second;
+	} else {
+		return F_identity;
+	}
+}
+
+}
diff --git a/intern/itasc/FixedObject.hpp b/intern/itasc/FixedObject.hpp
new file mode 100644
index 00000000000..01ab3355259
--- /dev/null
+++ b/intern/itasc/FixedObject.hpp
@@ -0,0 +1,45 @@
+/* $Id: FixedObject.hpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * FixedObject.h
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#ifndef FIXEDOBJECT_HPP_
+#define FIXEDOBJECT_HPP_
+
+#include "UncontrolledObject.hpp"
+#include <vector>
+
+
+namespace iTaSC{
+
+class FixedObject: public UncontrolledObject {
+public:
+    FixedObject();
+    virtual ~FixedObject();
+
+	int addFrame(const std::string& name, const Frame& frame);
+
+	virtual void updateCoordinates(const Timestamp& timestamp) {};
+	virtual int addEndEffector(const std::string& name);
+	virtual void finalize();
+	virtual const Frame& getPose(const unsigned int frameIndex);
+	virtual void updateKinematics(const Timestamp& timestamp) {};
+	virtual void pushCache(const Timestamp& timestamp) {};
+	virtual void initCache(Cache *_cache) {};
+
+protected:
+	virtual void updateJacobian() {}
+private:
+    typedef std::vector<std::pair<std::string, Frame> > FrameList;
+
+	bool m_finalized;
+	unsigned int m_nframe;
+	FrameList m_frameArray;
+
+};
+
+}
+
+#endif /* FIXEDOBJECT_H_ */
diff --git a/intern/itasc/Makefile b/intern/itasc/Makefile
new file mode 100644
index 00000000000..463f7763cd2
--- /dev/null
+++ b/intern/itasc/Makefile
@@ -0,0 +1,53 @@
+#
+# $Id: Makefile 19907 2009-04-23 13:41:59Z 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/$(SOURCEDIR)
+
+include nan_compile.mk
+
+CPPFLAGS += -I.
+CPPFLAGS += -I../../extern/Eigen2
+
+install: all debug
+	@[ -d $(NAN_ITASC) ] || mkdir $(NAN_ITASC)
+	@[ -d $(NAN_ITASC)/lib ] || mkdir $(NAN_ITASC)/lib
+	@[ -d $(NAN_ITASC)/lib/debug ] || mkdir $(NAN_ITASC)/lib/debug
+	@../tools/cpifdiff.sh $(DIR)/libitasc.a $(NAN_ITASC)/lib/
+	@../tools/cpifdiff.sh $(DIR)/debug/libitasc.a $(NAN_ITASC)/lib/debug/
+ifeq ($(OS),darwin)
+	ranlib $(NAN_ITASC)/lib/libitasc.a
+	ranlib $(NAN_ITASC)/lib/debug/libitasc.a
+endif
+##############################
+DIRS = kdl
+SOURCEDIR = intern/$(LIBNAME)
+include nan_subdirs.mk
diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp
new file mode 100644
index 00000000000..545f9bd38e9
--- /dev/null
+++ b/intern/itasc/MovingFrame.cpp
@@ -0,0 +1,157 @@
+/* $Id: MovingFrame.cpp 20853 2009-06-13 12:29:46Z ben2610 $
+ * MovingFrame.cpp
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#include "MovingFrame.hpp"
+#include <malloc.h>
+#include <string.h>
+namespace iTaSC{
+
+static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double);
+
+MovingFrame::MovingFrame(const Frame& frame):UncontrolledObject(),
+	m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0)
+{
+	m_internalPose = m_nextPose = frame;
+	initialize(6, 1);
+	e_matrix& Ju = m_JuArray[0];
+	Ju = e_identity_matrix(6,6);
+}
+
+MovingFrame::~MovingFrame()
+{
+}
+
+void MovingFrame::finalize()
+{
+	updateJacobian();
+}
+
+void MovingFrame::initCache(Cache *_cache)
+{
+	m_cache = _cache;
+	m_poseCCh = -1;
+	if (m_cache) {
+		m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize*sizeof(double));
+		// don't store the initial pose, it's causing unnecessary large velocity on the first step
+		//pushInternalFrame(0);
+	}
+}
+
+void MovingFrame::pushInternalFrame(CacheTS timestamp)
+{
+	if (m_poseCCh >= 0) {
+		double buf[frameCacheSize];
+		memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data));
+		memcpy(&buf[sizeof(m_internalPose.p.data)/sizeof(double)], m_internalPose.M.data, sizeof(m_internalPose.M.data));
+
+		m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon);
+		m_poseCTs = timestamp;
+	}
+}
+
+// load pose just preceeding timestamp
+// return false if no cache position was found
+bool MovingFrame::popInternalFrame(CacheTS timestamp)
+{
+	if (m_poseCCh >= 0) {
+		char *item;
+		item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
+		if (item && m_poseCTs != timestamp) {
+			memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data));
+			item += sizeof(m_internalPose.p.data);
+			memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data));
+			m_poseCTs = timestamp;
+			// changing the starting pose, recompute the jacobian
+			updateJacobian();
+		}
+		return (item) ? true : false;
+	}
+	// in case of no cache, there is always a previous position
+	return true;
+}
+
+bool MovingFrame::setFrame(const Frame& frame)
+{
+	m_internalPose = m_nextPose = frame;
+	return true;
+}
+
+bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param)
+{
+	m_function = _function;
+	m_param = _param;
+	return true;
+}
+
+void MovingFrame::updateCoordinates(const Timestamp& timestamp)
+{
+	// don't compute the velocity during substepping, it is assumed constant.
+	if (!timestamp.substep) {
+		bool cacheAvail = true;
+		if (!timestamp.reiterate) {
+			cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
+			if (m_function)
+				(*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
+		}
+		// only compute velocity if we have a previous pose
+		if (cacheAvail && timestamp.interpolate) {
+			unsigned int iXu;
+			m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
+			for (iXu=0; iXu<6; iXu++)
+				m_xudot(iXu) = m_velocity(iXu);
+		} else if (!timestamp.reiterate) {
+			// new position is forced, no velocity as we cannot interpolate
+			m_internalPose = m_nextPose;
+			m_velocity = Twist::Zero();
+			m_xudot = e_zero_vector(6);
+			// recompute the jacobian
+			updateJacobian();
+		}
+	}
+}
+
+void MovingFrame::pushCache(const Timestamp& timestamp)
+{
+	if (!timestamp.substep && timestamp.cache)
+		pushInternalFrame(timestamp.cacheTimestamp);
+}
+
+void MovingFrame::updateKinematics(const Timestamp& timestamp)
+{
+	if (timestamp.interpolate) {
+		if (timestamp.substep) {
+			// during substepping, update the internal pose from velocity information
+			Twist localvel = m_internalPose.M.Inverse(m_velocity);
+			m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep);
+		} else {
+			m_internalPose = m_nextPose;
+		}
+		// m_internalPose is updated, recompute the jacobian
+		updateJacobian();
+	}
+	pushCache(timestamp);
+}
+
+void MovingFrame::updateJacobian()
+{
+	Twist m_jac;
+	e_matrix& Ju = m_JuArray[0];
+
+    //Jacobian is always identity at position on the object, 
+	//we ust change the reference to the world.
+	//instead of going through complicated jacobian operation, implemented direct formula
+	Ju(1,3) = m_internalPose.p.z();
+	Ju(2,3) = -m_internalPose.p.y();
+	Ju(0,4) = -m_internalPose.p.z();
+	Ju(2,4) = m_internalPose.p.x();
+	Ju(0,5) = m_internalPose.p.y();
+	Ju(1,5) = -m_internalPose.p.x();
+	// remember that this object has moved
+	m_updated = true;
+}
+
+}
diff --git a/intern/itasc/MovingFrame.hpp b/intern/itasc/MovingFrame.hpp
new file mode 100644
index 00000000000..edaa3136a13
--- /dev/null
+++ b/intern/itasc/MovingFrame.hpp
@@ -0,0 +1,48 @@
+/* $Id: MovingFrame.hpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * MovingFrame.h
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#ifndef MOVINGFRAME_HPP_
+#define MOVINGFRAME_HPP_
+
+#include "UncontrolledObject.hpp"
+#include <vector>
+
+
+namespace iTaSC{
+
+typedef bool (*MovingFrameCallback)(const Timestamp& timestamp, const Frame& _current, Frame& _next, void *param);
+
+class MovingFrame: public UncontrolledObject {
+public:
+    MovingFrame(const Frame& frame=F_identity);
+    virtual ~MovingFrame();
+
+	bool setFrame(const Frame& frame);
+	bool setCallback(MovingFrameCallback _function, void* _param);
+
+	virtual void updateCoordinates(const Timestamp& timestamp);
+	virtual void updateKinematics(const Timestamp& timestamp);
+    virtual void pushCache(const Timestamp& timestamp);
+	virtual void initCache(Cache *_cache);
+	virtual void finalize();
+protected:
+	virtual void updateJacobian();
+
+private:
+	void pushInternalFrame(CacheTS timestamp);
+	bool popInternalFrame(CacheTS timestamp);
+	MovingFrameCallback m_function;
+	void* m_param;
+	Frame m_nextPose;
+	Twist m_velocity;
+	int m_poseCCh;		// cache channel for pose
+	unsigned int m_poseCTs;
+};
+
+}
+
+#endif /* MOVINGFRAME_H_ */
diff --git a/intern/itasc/Object.hpp b/intern/itasc/Object.hpp
new file mode 100644
index 00000000000..5c312cab768
--- /dev/null
+++ b/intern/itasc/Object.hpp
@@ -0,0 +1,48 @@
+/* $Id: Object.hpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * Object.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef OBJECT_HPP_
+#define OBJECT_HPP_
+
+#include "Cache.hpp"
+#include "kdl/frames.hpp"
+#include <string>
+
+namespace iTaSC{
+
+class WorldObject;
+
+class Object {
+public:
+    enum ObjectType {Controlled, UnControlled};
+	static WorldObject world;
+
+private:
+    ObjectType m_type;
+protected:
+	Cache *m_cache;
+	KDL::Frame m_internalPose;
+	bool m_updated;
+    virtual void updateJacobian()=0;
+public:
+    Object(ObjectType _type):m_type(_type), m_cache(NULL), m_internalPose(F_identity), m_updated(false) {};
+    virtual ~Object(){};
+
+	virtual int addEndEffector(const std::string& name){return 0;};
+	virtual void finalize(){};
+	virtual const KDL::Frame& getPose(const unsigned int end_effector=0){return m_internalPose;};
+    virtual const ObjectType getType(){return m_type;};
+    virtual const unsigned int getNrOfCoordinates(){return 0;};
+    virtual void updateKinematics(const Timestamp& timestamp)=0;
+    virtual void pushCache(const Timestamp& timestamp)=0;
+	virtual void initCache(Cache *_cache) = 0;
+	bool updated() {return m_updated;};
+	void updated(bool val) {m_updated=val;};
+};
+
+}
+#endif /* OBJECT_HPP_ */
diff --git a/intern/itasc/SConscript b/intern/itasc/SConscript
new file mode 100644
index 00000000000..9e11b6c7119
--- /dev/null
+++ b/intern/itasc/SConscript
@@ -0,0 +1,11 @@
+#!/usr/bin/python
+Import ('env')
+
+sources = env.Glob('*.cpp')
+sources += env.Glob('kdl/*.cpp')
+sources += env.Glob('kdl/utilities/*.cpp')
+
+incs = '. ../../extern/Eigen2'
+
+env.BlenderLib ('bf_ITASC', sources, Split(incs), [], libtype=['intern','player'], priority=[20,100] )
+
diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp
new file mode 100644
index 00000000000..c50769e9c1d
--- /dev/null
+++ b/intern/itasc/Scene.cpp
@@ -0,0 +1,543 @@
+/* $Id: Scene.cpp 20874 2009-06-14 13:50:34Z ben2610 $
+ * Scene.cpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#include "Scene.hpp"
+#include "ControlledObject.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+#include <cstdio>
+
+namespace iTaSC {
+
+class SceneLock : public ControlledObject::JointLockCallback {
+private:
+	Scene* m_scene;
+	Range  m_qrange;
+
+public:
+	SceneLock(Scene* scene) :
+	  m_scene(scene), m_qrange(0,0) {}
+	virtual ~SceneLock() {}
+
+	void setRange(Range& range)
+	{
+		m_qrange = range;
+	}
+	// lock a joint, no need to update output
+	virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
+	{
+		q_nr += m_qrange.start;
+		project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+	}
+	// lock a joint and update output in view of reiteration
+	virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot)
+	{
+		q_nr += m_qrange.start;
+		project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+		// update the ouput vector so that the movement of this joint will be 
+		// taken into account and we can put the joint back in its initial position
+		// which means that the jacobian doesn't need to be changed
+		for (unsigned int i=0 ;i<ndof ; ++i, ++q_nr) {
+			m_scene->m_ydot -= m_scene->m_A.col(q_nr)*qdot[i];
+		}
+	}
+};
+
+Scene::Scene():
+	m_A(), m_B(), m_Atemp(), m_Wq(), m_Jf(), m_Jq(), m_Ju(), m_Cf(), m_Cq(), m_Jf_inv(),
+	m_Vf(),m_Uf(), m_Wy(), m_ydot(), m_qdot(), m_xdot(), m_Sf(),m_tempf(),
+	m_ncTotal(0),m_nqTotal(0),m_nuTotal(0),m_nsets(0),
+	m_solver(NULL),m_cache(NULL) 
+{
+	m_minstep = 0.01;
+	m_maxstep = 0.06;
+}
+
+Scene::~Scene() 
+{
+	ConstraintMap::iterator constraint_it;
+	while ((constraint_it = constraints.begin()) != constraints.end()) {
+		delete constraint_it->second;
+		constraints.erase(constraint_it);
+	}
+	ObjectMap::iterator object_it;
+	while ((object_it = objects.begin()) != objects.end()) {
+		delete object_it->second;
+		objects.erase(object_it);
+	}
+}
+
+bool Scene::setParam(SceneParam paramId, double value)
+{
+	switch (paramId) {
+	case MIN_TIMESTEP:
+		m_minstep = value;
+		break;
+	case MAX_TIMESTEP:		
+		m_maxstep = value;
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame)
+{
+	// finalize the object before adding
+	object->finalize();
+    //Check if Object is controlled or uncontrolled.
+    if(object->getType()==Object::Controlled){
+		int baseFrameIndex = base->addEndEffector(baseFrame);
+		if (baseFrameIndex < 0)
+			return false;
+		std::pair<ObjectMap::iterator, bool> result;
+		if (base->getNrOfCoordinates() == 0) {
+			// base is fixed object, no coordinate range
+			result = objects.insert(ObjectMap::value_type(
+					name, new Object_struct(object,base,baseFrameIndex,
+						Range(m_nqTotal,object->getNrOfCoordinates()),
+						Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
+						Range(0,0))));
+		} else {
+			// base is a moving object, must be in list already
+		    ObjectMap::iterator base_it;
+			for (base_it=objects.begin(); base_it != objects.end(); base_it++) {
+				if (base_it->second->object == base)
+					break;
+			}
+			if (base_it == objects.end())
+				return false;
+			result = objects.insert(ObjectMap::value_type(
+					name, new Object_struct(object,base,baseFrameIndex,
+						Range(m_nqTotal,object->getNrOfCoordinates()),
+						Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
+						base_it->second->coordinaterange)));
+		}
+		if (!result.second) {
+			return false;
+		}
+        m_nqTotal+=object->getNrOfCoordinates();
+        m_ncTotal+=((ControlledObject*)object)->getNrOfConstraints();
+        return true;
+    }
+    if(object->getType()==Object::UnControlled){
+		if ((WorldObject*)base != &Object::world)
+			return false;
+		std::pair<ObjectMap::iterator,bool> result = objects.insert(ObjectMap::value_type(
+			name,new Object_struct(object,base,0,
+						Range(0,0),
+						Range(0,0),
+						Range(m_nuTotal,object->getNrOfCoordinates()))));
+        if(!result.second)
+            return false;
+        m_nuTotal+=object->getNrOfCoordinates();
+        return true;
+    }
+    return false;
+}
+
+bool Scene::addConstraintSet(const std::string& name,ConstraintSet* task,const std::string& object1,const std::string& object2, const std::string& ee1, const std::string& ee2)
+{
+    //Check if objects exist:
+    ObjectMap::iterator object1_it = objects.find(object1);
+    ObjectMap::iterator object2_it = objects.find(object2);
+    if(object1_it==objects.end()||object2_it==objects.end())
+        return false;
+	int ee1_index = object1_it->second->object->addEndEffector(ee1);
+	int ee2_index = object2_it->second->object->addEndEffector(ee2);
+	if (ee1_index < 0 || ee2_index < 0)
+		return false;
+	std::pair<ConstraintMap::iterator,bool> result = 
+		constraints.insert(ConstraintMap::value_type(name,new ConstraintSet_struct(
+			task,object1_it,ee1_index,object2_it,ee2_index,
+			Range(m_ncTotal,task->getNrOfConstraints()),Range(6*m_nsets,6))));
+    if(!result.second)
+        return false;
+    m_ncTotal+=task->getNrOfConstraints();
+    m_nsets+=1;
+    return true;
+}
+
+bool Scene::addSolver(Solver* _solver){
+    if(m_solver==NULL){
+        m_solver=_solver;
+        return true;
+    }
+    else
+        return false;
+}
+
+bool Scene::addCache(Cache* _cache){
+    if(m_cache==NULL){
+        m_cache=_cache;
+        return true;
+    }
+    else
+        return false;
+}
+
+bool Scene::initialize(){
+
+    //prepare all matrices:
+	if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
+		return false;
+
+    m_A = e_zero_matrix(m_ncTotal,m_nqTotal);
+	if (m_nuTotal > 0) {
+		m_B = e_zero_matrix(m_ncTotal,m_nuTotal);
+		m_xdot = e_zero_vector(m_nuTotal);
+		m_Ju = e_zero_matrix(6*m_nsets,m_nuTotal);
+	}
+    m_Atemp = e_zero_matrix(m_ncTotal,6*m_nsets);
+    m_ydot = e_zero_vector(m_ncTotal);
+    m_qdot = e_zero_vector(m_nqTotal);
+    m_Wq = e_zero_matrix(m_nqTotal,m_nqTotal);
+    m_Wy = e_zero_vector(m_ncTotal);
+    m_Jq = e_zero_matrix(6*m_nsets,m_nqTotal);
+    m_Jf = e_zero_matrix(6*m_nsets,6*m_nsets);
+    m_Jf_inv = m_Jf;
+    m_Cf = e_zero_matrix(m_ncTotal,m_Jf.rows());
+    m_Cq = e_zero_matrix(m_ncTotal,m_nqTotal);
+
+    bool result=true;
+	// finalize all objects
+	for (ObjectMap::iterator it=objects.begin(); it!=objects.end(); ++it) {
+		Object_struct* os = it->second;
+
+		os->object->initCache(m_cache);
+		if (os->constraintrange.count > 0)
+			project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
+	}
+
+	m_ytask.resize(m_ncTotal);
+	bool toggle=true;
+	int cnt = 0;
+    //Initialize all ConstraintSets:
+    for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
+        //Calculate the external pose:
+		ConstraintSet_struct* cs = it->second;
+		Frame external_pose;
+		getConstraintPose(cs->task, cs, external_pose);
+        result&=cs->task->initialise(external_pose);
+		cs->task->initCache(m_cache);
+		for (int i=0; i<cs->constraintrange.count; i++, cnt++) {
+			m_ytask[cnt] = toggle;
+		}
+		toggle = !toggle;
+		project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
+    }
+
+    if(m_solver!=NULL)
+        m_solver->init(m_nqTotal,m_ncTotal,m_ytask);
+    else
+        return false;
+
+
+    return result;
+}
+
+bool Scene::getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose)
+{
+	// function called from constraint when they need to get the external pose
+	ConstraintSet_struct* cs = (ConstraintSet_struct*)_param;
+	// verification, the pointer MUST match
+	assert (constraint == cs->task);
+	Object_struct* ob1 = cs->object1->second;
+	Object_struct* ob2 = cs->object2->second;
+	//Calculate the external pose:
+	_pose=(ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index)).Inverse()*(ob2->base->getPose(ob2->baseFrameIndex)*ob2->object->getPose(cs->ee2index));
+	return true;
+}
+
+bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, bool reiterate, bool cache, bool interpolate)
+{
+	// we must have valid timestep and timestamp
+	if (timestamp < KDL::epsilon || timestep < 0.0)
+		return false;
+	Timestamp ts;
+	ts.realTimestamp = timestamp;
+	// initially we start with the full timestep to allow velocity estimation over the full interval
+	ts.realTimestep = timestep;
+	setCacheTimestamp(ts);
+	ts.substep = 0;
+	// for reiteration don't load cache
+	// reiteration=additional iteration with same timestamp if application finds the convergence not good enough
+	ts.reiterate = (reiterate) ? 1 : 0;
+	ts.interpolate = (interpolate) ? 1 : 0;
+	ts.cache = (cache) ? 1 : 0;
+	ts.update = 1;
+	ts.numstep = (numsubstep & 0xFF);
+	bool autosubstep = (numsubstep == 0) ? true : false;
+	if (numsubstep < 1)
+		numsubstep = 1;
+	double timesubstep = timestep/numsubstep;
+	double timeleft = timestep;
+
+	if (timeleft == 0.0) {
+		// this special case correspond to a request to cache data
+		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
+			it->second->object->pushCache(ts);
+		}
+		//Update the Constraints
+		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
+			it->second->task->pushCache(ts);
+		}
+		return true;
+	}
+
+	double maxqdot;
+	e_scalar nlcoef;
+	SceneLock lockCallback(this);
+	Frame external_pose;
+	bool locked;
+
+	// initially we keep timestep unchanged so that update function compute the velocity over
+	while (numsubstep > 0) {
+		// get objects
+		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
+			Object_struct* os = it->second;
+			if (os->object->getType()==Object::Controlled) {
+				((ControlledObject*)(os->object))->updateControlOutput(ts);
+				if (os->constraintrange.count > 0) {
+					project(m_ydot, os->constraintrange) = ((ControlledObject*)(os->object))->getControlOutput();
+					project(m_Wy, os->constraintrange) = ((ControlledObject*)(os->object))->getWy();
+					// project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
+				}
+				if (os->jointrange.count > 0) {
+					project(m_Wq,os->jointrange,os->jointrange) = ((ControlledObject*)(os->object))->getWq();
+				}
+			}
+			if (os->object->getType()==Object::UnControlled && ((UncontrolledObject*)os->object)->getNrOfCoordinates() != 0) {
+	            ((UncontrolledObject*)(os->object))->updateCoordinates(ts);
+				if (!ts.substep) {
+					// velocity of uncontrolled object remains constant during substepping
+					project(m_xdot,os->coordinaterange) = ((UncontrolledObject*)(os->object))->getXudot();
+				}
+			}
+		}
+
+		//get new Constraints values
+		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it) {
+			ConstraintSet_struct* cs = it->second;
+			Object_struct* ob1 = cs->object1->second;
+			Object_struct* ob2 = cs->object2->second;
+
+			if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() || ob2->object->updated()) {
+				// the object from which the constraint depends have changed position
+				// recompute the constraint pose
+				getConstraintPose(cs->task, cs, external_pose);
+				cs->task->initialise(external_pose);
+			}
+			cs->task->updateControlOutput(ts);
+	        project(m_ydot,cs->constraintrange)=cs->task->getControlOutput();
+			if (!ts.substep || cs->task->substep()) {
+				project(m_Wy,cs->constraintrange)=(cs->task)->getWy();
+				//project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
+			}
+
+			project(m_Jf,cs->featurerange,cs->featurerange)=cs->task->getJf();
+			//std::cout << "Jf = " << Jf << std::endl;
+			//Transform the reference frame of this jacobian to the world reference frame
+			Eigen::Block<e_matrix> Jf_part = project(m_Jf,cs->featurerange,cs->featurerange);
+			changeBase(Jf_part,ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index));
+			//std::cout << "Jf_w = " << Jf << std::endl;
+
+			//calculate the inverse of Jf
+			KDL::svd_eigen_HH(project(m_Jf,cs->featurerange,cs->featurerange),m_Uf,m_Sf,m_Vf,m_tempf);
+			for(unsigned int i=0;i<6;++i)
+				if(m_Sf(i)<KDL::epsilon)
+					m_Uf.col(i).setConstant(0.0);
+				else
+					m_Uf.col(i)*=(1/m_Sf(i));
+			project(m_Jf_inv,cs->featurerange,cs->featurerange)=(m_Vf*m_Uf.transpose()).lazy();
+
+			//Get the robotjacobian associated with this constraintset
+			//Each jacobian is expressed in robot base frame => convert to world reference
+			//and negate second robot because it is taken reversed when closing the loop:
+			if(ob1->object->getType()==Object::Controlled){
+				project(m_Jq,cs->featurerange,ob1->jointrange) = (((ControlledObject*)(ob1->object))->getJq(cs->ee1index));
+				//Transform the reference frame of this jacobian to the world reference frame:
+				Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob1->jointrange);
+				changeBase(Jq_part,ob1->base->getPose(ob1->baseFrameIndex));
+				// if the base of this object is moving, get the Ju part
+				if (ob1->base->getNrOfCoordinates() != 0) {
+					// Ju is already computed for world reference frame
+					project(m_Ju,cs->featurerange,ob1->coordinaterange)=ob1->base->getJu(ob1->baseFrameIndex);
+				}
+			} else if (ob1->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob1->object)->getNrOfCoordinates() != 0) {
+				// object1 is uncontrolled moving object
+				project(m_Ju,cs->featurerange,ob1->coordinaterange)=((UncontrolledObject*)ob1->object)->getJu(cs->ee1index);
+			}
+			if(ob2->object->getType()==Object::Controlled){
+				//Get the robotjacobian associated with this constraintset
+				// process a special case where object2 and object1 are equal but using different end effector
+				if (ob1->object == ob2->object) {
+					// we must create a temporary matrix
+					e_matrix JqTemp(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
+					//Transform the reference frame of this jacobian to the world reference frame:
+					changeBase(JqTemp,ob2->base->getPose(ob2->baseFrameIndex));
+					// substract in place
+					project(m_Jq,cs->featurerange,ob2->jointrange) -= JqTemp;
+				} else {
+					project(m_Jq,cs->featurerange,ob2->jointrange) = -(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
+					//Transform the reference frame of this jacobian to the world reference frame:
+					Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob2->jointrange);
+					changeBase(Jq_part,ob2->base->getPose(ob2->baseFrameIndex));
+				}
+				if (ob2->base->getNrOfCoordinates() != 0) {
+					// if base is the same as first object or first object base, 
+					// that portion of m_Ju has been set already => substract inplace
+					if (ob2->base == ob1->base || ob2->base == ob1->object) {
+						project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ob2->base->getJu(ob2->baseFrameIndex);
+					} else {
+						project(m_Ju,cs->featurerange,ob2->coordinaterange) = -ob2->base->getJu(ob2->baseFrameIndex);
+					}
+				}
+			} else if (ob2->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob2->object)->getNrOfCoordinates() != 0) {
+				if (ob2->object == ob1->base || ob2->object == ob1->object) {
+					project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
+				} else {
+					project(m_Ju,cs->featurerange,ob2->coordinaterange) = -((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
+				}
+			}
+		}
+
+	    //Calculate A
+		m_Atemp=(m_Cf*m_Jf_inv).lazy();
+		m_A = m_Cq-(m_Atemp*m_Jq).lazy();
+		if (m_nuTotal > 0) {
+			m_B=(m_Atemp*m_Ju).lazy();
+			m_ydot += (m_B*m_xdot).lazy();
+		}
+
+		//Call the solver with A, Wq, Wy, ydot to solver qdot:
+		if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
+			// this should never happen
+			return false;
+		//send result to the objects
+		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
+			Object_struct* os = it->second;
+			if(os->object->getType()==Object::Controlled)
+				((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
+		}
+		// compute the constraint velocity
+		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
+			ConstraintSet_struct* cs = it->second;
+			Object_struct* ob1 = cs->object1->second;
+			Object_struct* ob2 = cs->object2->second;
+			//Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
+			e_vector6 external_vel = e_zero_vector(6);
+			if (ob1->jointrange.count > 0)
+				external_vel += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange)).lazy();
+			if (ob2->jointrange.count > 0)
+				external_vel += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange)).lazy();
+			if (ob1->coordinaterange.count > 0)
+				external_vel += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange)).lazy();
+			if (ob2->coordinaterange.count > 0)
+				external_vel += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange)).lazy();
+			//the twist caused by the constraint must be opposite because of the closed loop
+			//estimate the velocity of the joints using the inverse jacobian
+			e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel);
+			cs->task->setJointVelocity(estimated_chidot);
+		}
+
+		if (autosubstep) {
+			// automatic computing of substep based on maximum joint change
+			// and joint limit gain variation
+			// We will pass the joint velocity to each object and they will recommend a maximum timestep
+			timesubstep = timeleft;
+			// get armature max joint velocity to estimate the maximum duration of integration
+			maxqdot = m_qdot.cwise().abs().maxCoeff();
+			double maxsubstep = nlcoef*m_maxstep;
+			if (maxsubstep < m_minstep)
+				maxsubstep = m_minstep;
+			if (timesubstep > maxsubstep)
+				timesubstep = maxsubstep;
+			for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
+				Object_struct* os = it->second;
+				if(os->object->getType()==Object::Controlled)
+					((ControlledObject*)(os->object))->getMaxTimestep(timesubstep);
+			}
+			for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
+				ConstraintSet_struct* cs = it->second;
+				cs->task->getMaxTimestep(timesubstep);
+			}
+			// use substep that are even dividers of timestep for more regularity
+			maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666);
+			timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep);
+			if (timesubstep >= timeleft-(m_minstep/2.0)) {
+				timesubstep = timeleft;
+				numsubstep = 1;
+				timeleft = 0.;
+			} else {
+				numsubstep = 2;
+				timeleft -= timesubstep;
+			}
+		}
+		if (numsubstep > 1) {
+			ts.substep = 1;
+		} else {
+			// set substep to false for last iteration so that controlled output 
+			// can be updated in updateKinematics() and model_update)() before next call to Secne::update()
+			ts.substep = 0;
+		}
+		// change timestep so that integration is done correctly
+		ts.realTimestep = timesubstep;
+
+		do {
+			ObjectMap::iterator it;
+			Object_struct* os;
+			locked = false;
+			for(it=objects.begin();it!=objects.end();++it){
+				os = it->second;
+				if (os->object->getType()==Object::Controlled) {
+					lockCallback.setRange(os->jointrange);
+					if (((ControlledObject*)os->object)->updateJoint(ts, lockCallback)) {
+						// this means one of the joint was locked and we must rerun
+						// the solver to update the remaining joints
+						locked = true;
+						break;
+					}
+				}
+			}
+			if (locked) {
+				// Some rows of m_Wq have been cleared so that the corresponding joint will not move
+				if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
+					// this should never happen
+					return false;
+
+				//send result to the objects
+				for(it=objects.begin();it!=objects.end();++it) {
+					os = it->second;
+					if(os->object->getType()==Object::Controlled)
+						((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
+				}
+			}
+		} while (locked);
+
+		//Update the Objects
+		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
+			it->second->object->updateKinematics(ts);
+			// mark this object not updated since the constraint will be updated anyway
+			// this flag is only useful to detect external updates
+			it->second->object->updated(false);
+		}
+		//Update the Constraints
+		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
+			ConstraintSet_struct* cs = it->second;
+			//Calculate the external pose:
+			getConstraintPose(cs->task, cs, external_pose);
+			cs->task->modelUpdate(external_pose,ts);
+			// update the constraint output and cache
+			cs->task->updateKinematics(ts);
+		}
+		numsubstep--;
+	}
+	return true;
+}
+
+}
diff --git a/intern/itasc/Scene.hpp b/intern/itasc/Scene.hpp
new file mode 100644
index 00000000000..a2d63361d95
--- /dev/null
+++ b/intern/itasc/Scene.hpp
@@ -0,0 +1,104 @@
+/* $Id: Scene.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * Scene.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef SCENE_HPP_
+#define SCENE_HPP_
+
+#include "eigen_types.hpp"
+
+#include "WorldObject.hpp"
+#include "ConstraintSet.hpp"
+#include "Solver.hpp"
+
+#include <map>
+
+namespace iTaSC {
+
+class SceneLock;
+
+class Scene {
+	friend class SceneLock;	
+public:
+	enum SceneParam {
+		MIN_TIMESTEP	= 0,
+		MAX_TIMESTEP,
+
+		COUNT
+	};
+
+
+	Scene();
+    virtual ~Scene();
+
+	bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame="");
+	bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2="");
+    bool addSolver(Solver* _solver);
+    bool addCache(Cache* _cache);
+    bool initialize();
+    bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true);
+	bool setParam(SceneParam paramId, double value);
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+    e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv;
+	e_matrix6 m_Vf,m_Uf;
+    e_vector m_Wy,m_ydot,m_qdot,m_xdot;
+	e_vector6 m_Sf,m_tempf;
+	double m_minstep;
+	double m_maxstep;
+    unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
+	std::vector<bool> m_ytask;
+
+    Solver* m_solver;
+	Cache* m_cache;
+
+
+    struct Object_struct{
+        Object* object;
+        UncontrolledObject* base;
+		unsigned int baseFrameIndex;
+        Range constraintrange;
+        Range jointrange;
+		Range coordinaterange;	// Xu range of base when object is controlled
+										// Xu range of object when object is uncontrolled 
+
+        Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
+            object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
+            {};
+    };
+    typedef std::map<std::string,Object_struct*> ObjectMap;
+
+    struct ConstraintSet_struct{
+        ConstraintSet* task;
+        ObjectMap::iterator object1;
+        ObjectMap::iterator object2;
+        Range constraintrange;
+        Range featurerange;
+		unsigned int ee1index;
+		unsigned int ee2index;
+        ConstraintSet_struct(ConstraintSet* _task,
+			ObjectMap::iterator _object1,unsigned int _ee1index,
+			ObjectMap::iterator _object2,unsigned int _ee2index,
+			Range nc_range,Range coord_range):
+				task(_task),
+				object1(_object1),object2(_object2),
+				constraintrange(nc_range),featurerange(coord_range),
+				ee1index(_ee1index), ee2index(_ee2index)
+            {};
+    };
+    typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
+
+    ObjectMap objects;
+    ConstraintMap constraints;
+
+	static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
+};
+
+}
+
+#endif /* SCENE_HPP_ */
diff --git a/intern/itasc/Solver.hpp b/intern/itasc/Solver.hpp
new file mode 100644
index 00000000000..e3aa1e1abc8
--- /dev/null
+++ b/intern/itasc/Solver.hpp
@@ -0,0 +1,33 @@
+/* $Id: Solver.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * Solver.hpp
+ *
+ *  Created on: Jan 8, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef SOLVER_HPP_
+#define SOLVER_HPP_
+
+#include <vector>
+#include "eigen_types.hpp"
+
+namespace iTaSC{
+
+class Solver{
+public:
+	enum SolverParam {
+		DLS_QMAX = 0,
+		DLS_LAMBDA_MAX,
+		DLS_EPSILON
+	};
+    virtual ~Solver(){};
+
+	// gc = grouping of constraint output , 
+	//      size of vector = nc, alternance of true / false to indicate the grouping of output
+	virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)=0;
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)=0;
+	virtual void setParam(SolverParam param, double value)=0;
+};
+
+}
+#endif /* SOLVER_HPP_ */
diff --git a/intern/itasc/UncontrolledObject.cpp b/intern/itasc/UncontrolledObject.cpp
new file mode 100644
index 00000000000..e05a8682d20
--- /dev/null
+++ b/intern/itasc/UncontrolledObject.cpp
@@ -0,0 +1,43 @@
+/* $Id: UncontrolledObject.cpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * UncontrolledObject.cpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#include "UncontrolledObject.hpp"
+
+namespace iTaSC{
+
+UncontrolledObject::UncontrolledObject():Object(UnControlled),
+	m_nu(0), m_nf(0), m_xudot()
+{
+}
+
+UncontrolledObject::~UncontrolledObject() 
+{
+}
+
+void UncontrolledObject::initialize(unsigned int _nu, unsigned int _nf)
+{
+	assert (_nf >= 1);
+	m_nu = _nu;
+	m_nf = _nf;
+	if (_nu > 0)
+		m_xudot = e_zero_vector(_nu);
+	// clear all Jacobian if any
+	m_JuArray.clear();
+	// reserve one more to have an zero matrix handy
+	if (m_nu > 0)
+		m_JuArray.resize(m_nf+1, e_zero_matrix(6,m_nu));
+}
+
+const e_matrix& UncontrolledObject::getJu(unsigned int frameIndex) const
+{
+	assert (m_nu > 0);
+	return m_JuArray[(frameIndex>m_nf)?m_nf:frameIndex];
+}
+
+
+
+}
diff --git a/intern/itasc/UncontrolledObject.hpp b/intern/itasc/UncontrolledObject.hpp
new file mode 100644
index 00000000000..3b693a0b2ed
--- /dev/null
+++ b/intern/itasc/UncontrolledObject.hpp
@@ -0,0 +1,37 @@
+/* $Id: UncontrolledObject.hpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * UncontrolledObject.h
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef UNCONTROLLEDOBJECT_HPP_
+#define UNCONTROLLEDOBJECT_HPP_
+
+#include "eigen_types.hpp"
+
+#include "Object.hpp"
+namespace iTaSC{
+
+class UncontrolledObject: public Object {
+protected:
+	unsigned int m_nu, m_nf;
+	e_vector m_xudot;
+	std::vector<e_matrix> m_JuArray;
+
+public:
+    UncontrolledObject();
+    virtual ~UncontrolledObject();
+
+	virtual void initialize(unsigned int _nu, unsigned int _nf);
+	virtual const e_matrix& getJu(unsigned int frameIndex) const;
+    virtual const e_vector& getXudot() const {return m_xudot;}
+	virtual void updateCoordinates(const Timestamp& timestamp)=0;
+    virtual const unsigned int getNrOfCoordinates(){return m_nu;};
+    virtual const unsigned int getNrOfFrames(){return m_nf;};
+
+};
+
+}
+
+#endif /* UNCONTROLLEDOBJECT_H_ */
diff --git a/intern/itasc/WDLSSolver.cpp b/intern/itasc/WDLSSolver.cpp
new file mode 100644
index 00000000000..91278c7ad3b
--- /dev/null
+++ b/intern/itasc/WDLSSolver.cpp
@@ -0,0 +1,85 @@
+/* $Id: WDLSSolver.cpp 20749 2009-06-09 11:27:30Z ben2610 $
+ * WDLSSolver.hpp.cpp
+ *
+ *  Created on: Jan 8, 2009
+ *      Author: rubensmits
+ */
+
+#include "WDLSSolver.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+
+namespace iTaSC {
+
+WDLSSolver::WDLSSolver() : m_lambda(0.5), m_epsilon(0.1) 
+{
+	// maximum joint velocity
+	m_qmax = 50.0;
+}
+
+WDLSSolver::~WDLSSolver() {
+}
+
+bool WDLSSolver::init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc)
+{
+	m_ns = std::min(nc,nq);
+    m_AWq = e_zero_matrix(nc,nq);
+    m_WyAWq = e_zero_matrix(nc,nq);
+    m_U = e_zero_matrix(nc,nq);
+	m_S = e_zero_vector(std::max(nc,nq));
+    m_temp = e_zero_vector(nq);
+    m_V = e_zero_matrix(nq,nq);
+    m_WqV = e_zero_matrix(nq,nq);
+	m_Wy_ydot = e_zero_vector(nc);
+    return true;
+}
+
+bool WDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
+{
+	double alpha, vmax, norm;
+	// Create the Weighted jacobian
+    m_AWq = A*Wq;
+	for (int i=0; i<Wy.size(); i++)
+		m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
+
+    // Compute the SVD of the weighted jacobian
+	int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+    if(ret<0)
+        return false;
+
+    m_WqV = (Wq*m_V).lazy();
+
+    //Wy*ydot
+	m_Wy_ydot = Wy.cwise() * ydot;
+    //S^-1*U'*Wy*ydot
+	e_scalar maxDeltaS = e_scalar(0.0);
+	e_scalar prevS = e_scalar(0.0);
+	e_scalar maxS = e_scalar(1.0);
+	e_scalar S, lambda;
+	qdot.setZero();
+	for(int i=0;i<m_ns;++i) {
+		S = m_S(i);
+		if (S <= KDL::epsilon)
+			break;
+		if (i > 0 && (prevS-S) > maxDeltaS) {
+			maxDeltaS = (prevS-S);
+			maxS = prevS;
+		}
+		lambda = (S < m_epsilon) ? (e_scalar(1.0)-KDL::sqr(S/m_epsilon))*m_lambda*m_lambda : e_scalar(0.0);
+		alpha = m_U.col(i).dot(m_Wy_ydot)*S/(S*S+lambda);
+		vmax = m_WqV.col(i).cwise().abs().maxCoeff();
+		norm = fabs(alpha*vmax);
+		if (norm > m_qmax) {
+			qdot += m_WqV.col(i)*(alpha*m_qmax/norm);
+		} else {
+			qdot += m_WqV.col(i)*alpha;
+		}
+		prevS = S;
+	}
+	if (maxDeltaS == e_scalar(0.0))
+		nlcoef = e_scalar(KDL::epsilon);
+	else
+		nlcoef = (maxS-maxDeltaS)/maxS;
+    return true;
+}
+
+}
diff --git a/intern/itasc/WDLSSolver.hpp b/intern/itasc/WDLSSolver.hpp
new file mode 100644
index 00000000000..4418e73675c
--- /dev/null
+++ b/intern/itasc/WDLSSolver.hpp
@@ -0,0 +1,47 @@
+/* $Id: WDLSSolver.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * WDLSSolver.hpp
+ *
+ *  Created on: Jan 8, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef WDLSSOLVER_HPP_
+#define WDLSSOLVER_HPP_
+
+#include "Solver.hpp"
+
+namespace iTaSC {
+
+class WDLSSolver: public iTaSC::Solver {
+private:
+    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
+    e_vector m_S,m_temp,m_Wy_ydot;
+    double m_lambda;
+    double m_epsilon;
+	double m_qmax;
+	int m_ns;
+public:
+    WDLSSolver();
+    virtual ~WDLSSolver();
+
+    virtual bool init(unsigned int nq, unsigned int nc, const std::vector<bool>& gc);
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
+	virtual void setParam(SolverParam param, double value)
+	{
+		switch (param) {
+		case DLS_QMAX:
+			m_qmax = value;
+			break;
+		case DLS_LAMBDA_MAX:
+			m_lambda = value;
+			break;
+		case DLS_EPSILON:
+			m_epsilon = value;
+			break;
+		}	
+	}
+};
+
+}
+
+#endif /* WDLSSOLVER_HPP_ */
diff --git a/intern/itasc/WSDLSSolver.cpp b/intern/itasc/WSDLSSolver.cpp
new file mode 100644
index 00000000000..1f99ad08334
--- /dev/null
+++ b/intern/itasc/WSDLSSolver.cpp
@@ -0,0 +1,122 @@
+/* $Id: WSDLSSolver.cpp 20749 2009-06-09 11:27:30Z ben2610 $
+ * WDLSSolver.hpp.cpp
+ *
+ *  Created on: Jan 8, 2009
+ *      Author: rubensmits
+ */
+
+#include "WSDLSSolver.hpp"
+#include "kdl/utilities/svd_eigen_HH.hpp"
+#include <cstdio>
+
+namespace iTaSC {
+
+WSDLSSolver::WSDLSSolver() :
+	m_ns(0), m_nc(0), m_nq(0)
+
+{
+	// default maximum speed: 50 rad/s
+	m_qmax = 50.0;
+}
+
+WSDLSSolver::~WSDLSSolver() {
+}
+
+bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc)
+{
+	if (_nc == 0 || _nq == 0 || gc.size() != _nc)
+		return false;
+	m_nc = _nc;
+	m_nq = _nq;
+	m_ns = std::min(m_nc,m_nq);
+    m_AWq = e_zero_matrix(m_nc,m_nq);
+    m_WyAWq = e_zero_matrix(m_nc,m_nq);
+    m_U = e_zero_matrix(m_nc,m_nq);
+	m_S = e_zero_vector(std::max(m_nc,m_nq));
+    m_temp = e_zero_vector(m_nq);
+    m_V = e_zero_matrix(m_nq,m_nq);
+    m_WqV = e_zero_matrix(m_nq,m_nq);
+	m_Wy_ydot = e_zero_vector(m_nc);
+	m_ytask = gc;
+    return true;
+}
+
+bool WSDLSSolver::solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef)
+{
+	unsigned int i, j, l;
+	e_scalar N, M;
+
+	// Create the Weighted jacobian
+    m_AWq = (A*Wq).lazy();
+	for (i=0; i<m_nc; i++)
+		m_WyAWq.row(i) = Wy(i)*m_AWq.row(i);
+
+    // Compute the SVD of the weighted jacobian
+	int ret = KDL::svd_eigen_HH(m_WyAWq,m_U,m_S,m_V,m_temp);
+    if(ret<0)
+        return false;
+
+	m_Wy_ydot = Wy.cwise() * ydot;
+    m_WqV = (Wq*m_V).lazy();
+	qdot.setZero();
+	e_scalar maxDeltaS = e_scalar(0.0);
+	e_scalar prevS = e_scalar(0.0);
+	e_scalar maxS = e_scalar(1.0);
+	for(i=0;i<m_ns;++i) {
+		e_scalar norm, mag, alpha, _qmax, Sinv, vmax, damp;
+		e_scalar S = m_S(i);
+		bool prev;
+		if (S < KDL::epsilon)
+			break;
+		Sinv = e_scalar(1.)/S;
+		if (i > 0) {
+			if ((prevS-S) > maxDeltaS) {
+				maxDeltaS = (prevS-S);
+				maxS = prevS;
+			}
+		}
+		N = M = e_scalar(0.);
+		for (l=0, prev=m_ytask[0], norm=e_scalar(0.); l<m_nc; l++) {
+			if (prev == m_ytask[l]) {
+				norm += m_U(l,i)*m_U(l,i);
+			} else {
+				N += std::sqrt(norm);
+				norm = m_U(l,i)*m_U(l,i);
+			}
+			prev = m_ytask[l];
+		}
+		N += std::sqrt(norm);
+		for (j=0; j<m_nq; j++) {
+			for (l=0, prev=m_ytask[0], norm=e_scalar(0.), mag=e_scalar(0.); l<m_nc; l++) {
+				if (prev == m_ytask[l]) {
+					norm += m_WyAWq(l,j)*m_WyAWq(l,j);
+				} else {
+					mag += std::sqrt(norm);
+					norm = m_WyAWq(l,j)*m_WyAWq(l,j);
+				}
+				prev = m_ytask[l];
+			}
+			mag += std::sqrt(norm);
+			M += fabs(m_V(j,i))*mag;
+		}
+		M *= Sinv;
+		alpha = m_U.col(i).dot(m_Wy_ydot);
+		_qmax = (N < M) ? m_qmax*N/M : m_qmax;
+		vmax = m_WqV.col(i).cwise().abs().maxCoeff();
+		norm = fabs(Sinv*alpha*vmax);
+		if (norm > _qmax) {
+			damp = Sinv*alpha*_qmax/norm;
+		} else {
+			damp = Sinv*alpha;
+		}
+		qdot += m_WqV.col(i)*damp;
+		prevS = S;
+	}
+	if (maxDeltaS == e_scalar(0.0))
+		nlcoef = e_scalar(KDL::epsilon);
+	else
+		nlcoef = (maxS-maxDeltaS)/maxS;
+    return true;
+}
+
+}
diff --git a/intern/itasc/WSDLSSolver.hpp b/intern/itasc/WSDLSSolver.hpp
new file mode 100644
index 00000000000..90f89f4e853
--- /dev/null
+++ b/intern/itasc/WSDLSSolver.hpp
@@ -0,0 +1,40 @@
+/* $Id: WSDLSSolver.hpp 20622 2009-06-04 12:47:59Z ben2610 $
+ * WSDLSSolver.hpp
+ *
+ *  Created on: Mar 26, 2009
+ *      Author: benoit bolsee
+ */
+
+#ifndef WSDLSSOLVER_HPP_
+#define WSDLSSOLVER_HPP_
+
+#include "Solver.hpp"
+
+namespace iTaSC {
+
+class WSDLSSolver: public iTaSC::Solver {
+private:
+    e_matrix m_AWq,m_WyAWq,m_U,m_V,m_WqV;
+    e_vector m_S,m_temp,m_Wy_ydot;
+	std::vector<bool> m_ytask;
+	e_scalar m_qmax;
+	unsigned int m_ns, m_nc, m_nq;
+public:
+    WSDLSSolver();
+    virtual ~WSDLSSolver();
+
+    virtual bool init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc);
+    virtual bool solve(const e_matrix& A, const e_vector& Wy, const e_vector& ydot, const e_matrix& Wq, e_vector& qdot, e_scalar& nlcoef);
+	virtual void setParam(SolverParam param, double value)
+	{
+		switch (param) {
+		case DLS_QMAX:
+			m_qmax = value;
+			break;
+		}
+	}
+};
+
+}
+
+#endif /* WSDLSSOLVER_HPP_ */
diff --git a/intern/itasc/WorldObject.cpp b/intern/itasc/WorldObject.cpp
new file mode 100644
index 00000000000..ba3f8549f06
--- /dev/null
+++ b/intern/itasc/WorldObject.cpp
@@ -0,0 +1,26 @@
+/* $Id: WorldObject.cpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * WorldObject.cpp
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#include "WorldObject.hpp"
+
+namespace iTaSC{
+
+/* special singleton to be used as base for uncontrolled object */
+WorldObject Object::world;
+
+WorldObject::WorldObject():UncontrolledObject()
+{
+	initialize(0,1);
+	m_internalPose = Frame::Identity();
+}
+
+WorldObject::~WorldObject() 
+{
+}
+
+
+}
diff --git a/intern/itasc/WorldObject.hpp b/intern/itasc/WorldObject.hpp
new file mode 100644
index 00000000000..b309545a843
--- /dev/null
+++ b/intern/itasc/WorldObject.hpp
@@ -0,0 +1,30 @@
+/* $Id: WorldObject.hpp 19907 2009-04-23 13:41:59Z ben2610 $
+ * WorldObject.h
+ *
+ *  Created on: Feb 10, 2009
+ *      Author: benoitbolsee
+ */
+
+#ifndef WORLDOBJECT_HPP_
+#define WORLDOBJECT_HPP_
+
+#include "UncontrolledObject.hpp"
+namespace iTaSC{
+
+class WorldObject: public UncontrolledObject {
+public:
+    WorldObject();
+    virtual ~WorldObject();
+
+	virtual void updateCoordinates(const Timestamp& timestamp) {};
+	virtual void updateKinematics(const Timestamp& timestamp) {};
+	virtual void pushCache(const Timestamp& timestamp) {};
+	virtual void initCache(Cache *_cache) {};
+protected:
+	virtual void updateJacobian() {}
+
+};
+
+}
+
+#endif /* WORLDOBJECT_H_ */
diff --git a/intern/itasc/eigen_types.cpp b/intern/itasc/eigen_types.cpp
new file mode 100644
index 00000000000..a1b28e01210
--- /dev/null
+++ b/intern/itasc/eigen_types.cpp
@@ -0,0 +1,12 @@
+/* $Id: eigen_types.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * eigen_types.cpp
+ *
+ *  Created on: March 19, 2009
+ *      Author: benoit bolsee
+ */
+
+#include "eigen_types.hpp"
+
+const KDL::Frame iTaSC::F_identity(Rotation::Identity(),Vector::Zero());
+
+
diff --git a/intern/itasc/eigen_types.hpp b/intern/itasc/eigen_types.hpp
new file mode 100644
index 00000000000..fe46f8b6bb3
--- /dev/null
+++ b/intern/itasc/eigen_types.hpp
@@ -0,0 +1,84 @@
+/* $Id: eigen_types.hpp 19905 2009-04-23 13:29:54Z ben2610 $
+ * eigen_types.hpp
+ *
+ *  Created on: March 6, 2009
+ *      Author: benoit bolsee
+ */
+
+#ifndef EIGEN_TYPES_HPP_
+#define EIGEN_TYPES_HPP_
+
+#include <Eigen/Core>
+#include "kdl/frames.hpp"
+#include "kdl/tree.hpp"
+#include "kdl/chain.hpp"
+#include "kdl/jacobian.hpp"
+#include "kdl/jntarray.hpp"
+
+
+namespace iTaSC{
+
+using KDL::Twist;
+using KDL::Frame;
+using KDL::Joint;
+using KDL::Inertia;
+using KDL::SegmentMap;
+using KDL::Tree;
+using KDL::JntArray;
+using KDL::Jacobian;
+using KDL::Segment;
+using KDL::Rotation;
+using KDL::Vector;
+using KDL::Vector2;
+using KDL::Chain;
+
+extern const Frame F_identity;
+
+#define e_scalar double
+#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
+#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
+#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
+#define e_matrix6 Eigen::Matrix<e_scalar,6,6>
+#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
+#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
+#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
+#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
+#define e_vector6 Eigen::Matrix<e_scalar,6,1>
+#define e_vector3 Eigen::Matrix<e_scalar,3,1>
+
+class Range {
+public:
+	int start;
+	int count;
+	Range(int _start, int _count) { start = _start; count=_count; }
+	Range(const Range& other) { start=other.start; count=other.count; }
+};
+
+template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
+{
+	return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
+}
+
+template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
+{
+	return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
+}
+
+template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {
+
+    if (J.rows() != 6)
+        return -1;
+    for (int j = 0; j < J.cols(); ++j) {
+		typename Derived::ColXpr Jj = J.col(j);
+		Twist arg;
+        for(unsigned int i=0;i<6;++i)
+            arg(i)=Jj[i];
+		Twist tmp(T*arg);
+        for(unsigned int i=0;i<6;++i)
+            Jj[i]=e_scalar(tmp(i));
+    }
+    return 0;
+}
+
+}
+#endif /* UBLAS_TYPES_HPP_ */
diff --git a/intern/itasc/kdl/Makefile b/intern/itasc/kdl/Makefile
new file mode 100644
index 00000000000..e87795fd3b7
--- /dev/null
+++ b/intern/itasc/kdl/Makefile
@@ -0,0 +1,42 @@
+#
+# $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
+
+##############################
+DIRS = utilities
+SOURCEDIR = intern/$(LIBNAME)/kdl
+include nan_subdirs.mk
diff --git a/intern/itasc/kdl/chain.cpp b/intern/itasc/kdl/chain.cpp
new file mode 100644
index 00000000000..638366c96be
--- /dev/null
+++ b/intern/itasc/kdl/chain.cpp
@@ -0,0 +1,75 @@
+// 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
+
+#include "chain.hpp"
+
+namespace KDL {
+    using namespace std;
+
+    Chain::Chain():
+        segments(0),
+        nrOfJoints(0),
+        nrOfSegments(0)
+    {
+    }
+
+    Chain::Chain(const Chain& in):nrOfJoints(0),
+                                  nrOfSegments(0)
+    {
+        for(unsigned int i=0;i<in.getNrOfSegments();i++)
+            this->addSegment(in.getSegment(i));
+    }
+
+    Chain& Chain::operator=(const Chain& arg)
+    {
+        nrOfJoints=0;
+        nrOfSegments=0;
+        segments.resize(0);
+        for(unsigned int i=0;i<arg.nrOfSegments;i++)
+            addSegment(arg.getSegment(i));
+        return *this;
+
+    }
+
+    void Chain::addSegment(const Segment& segment)
+    {
+        segments.push_back(segment);
+        nrOfSegments++;
+		nrOfJoints += segment.getJoint().getNDof();
+    }
+
+    void Chain::addChain(const Chain& chain)
+    {
+        for(unsigned int i=0;i<chain.getNrOfSegments();i++)
+            this->addSegment(chain.getSegment(i));
+    }
+
+    const Segment& Chain::getSegment(unsigned int nr) const
+    {
+        return segments[nr];
+    }
+
+    Chain::~Chain()
+    {
+    }
+
+}
+
diff --git a/intern/itasc/kdl/chain.hpp b/intern/itasc/kdl/chain.hpp
new file mode 100644
index 00000000000..0d40690202a
--- /dev/null
+++ b/intern/itasc/kdl/chain.hpp
@@ -0,0 +1,95 @@
+// 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
+
+#ifndef KDL_CHAIN_HPP
+#define KDL_CHAIN_HPP
+
+#include "segment.hpp"
+#include <string>
+
+namespace KDL {
+    /**
+	  * \brief This class encapsulates a <strong>serial</strong> kinematic
+	  * interconnection structure. It is build out of segments.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Chain {
+    private:
+        std::vector<Segment> segments;
+        unsigned int nrOfJoints;
+        unsigned int nrOfSegments;
+    public:
+        /**
+         * The constructor of a chain, a new chain is always empty.
+         *
+         */
+        Chain();
+        Chain(const Chain& in);
+        Chain& operator = (const Chain& arg);
+
+        /**
+         * Adds a new segment to the <strong>end</strong> of the chain.
+         *
+         * @param segment The segment to add
+         */
+        void addSegment(const Segment& segment);
+        /**
+         * Adds a complete chain to the <strong>end</strong> of the chain
+         * The added chain is copied.
+         *
+         * @param chain The chain to add
+         */
+        void addChain(const Chain& chain);
+
+        /**
+         * Request the total number of joints in the chain.\n
+         * <strong> Important:</strong> It is not the
+         * same as the total number of segments since a segment does not
+         * need to have a joint. This function is important when
+         * creating a KDL::JntArray to use with this chain.
+         * @return total nr of joints
+         */
+        unsigned int getNrOfJoints()const {return nrOfJoints;};
+        /**
+         * Request the total number of segments in the chain.
+         * @return total number of segments
+         */
+        unsigned int getNrOfSegments()const {return nrOfSegments;};
+
+        /**
+         * Request the nr'd segment of the chain. There is no boundary
+         * checking.
+         *
+         * @param nr the nr of the segment starting from 0
+         *
+         * @return a constant reference to the nr'd segment
+         */
+        const Segment& getSegment(unsigned int nr)const;
+
+        virtual ~Chain();
+    };
+
+
+
+}//end of namespace KDL
+
+#endif
diff --git a/intern/itasc/kdl/chainfksolver.hpp b/intern/itasc/kdl/chainfksolver.hpp
new file mode 100644
index 00000000000..fa6f625ee9d
--- /dev/null
+++ b/intern/itasc/kdl/chainfksolver.hpp
@@ -0,0 +1,107 @@
+// 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
+
+#ifndef KDL_CHAIN_FKSOLVER_HPP
+#define KDL_CHAIN_FKSOLVER_HPP
+
+#include "chain.hpp"
+#include "framevel.hpp"
+#include "frameacc.hpp"
+#include "jntarray.hpp"
+#include "jntarrayvel.hpp"
+#include "jntarrayacc.hpp"
+
+namespace KDL {
+
+    /**
+	  * \brief This <strong>abstract</strong> class encapsulates a
+	  * solver for the forward position kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    //Forward definition
+    class ChainFkSolverPos {
+    public:
+        /**
+         * Calculate forward position kinematics for a KDL::Chain,
+         * from joint coordinates to cartesian pose.
+         *
+         * @param q_in input joint coordinates
+         * @param p_out reference to output cartesian pose
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
+        virtual ~ChainFkSolverPos(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward velocity kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+    class ChainFkSolverVel {
+    public:
+        /**
+         * Calculate forward position and velocity kinematics, from
+         * joint coordinates to cartesian coordinates.
+         *
+         * @param q_in input joint coordinates (position and velocity)
+         * @param out output cartesian coordinates (position and velocity)
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
+
+        virtual ~ChainFkSolverVel(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward acceleration kinematics for a KDL::Chain.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    class ChainFkSolverAcc {
+    public:
+        /**
+         * Calculate forward position, velocity and accelaration
+         * kinematics, from joint coordinates to cartesian coordinates
+         *
+         * @param q_in input joint coordinates (position, velocity and
+         * acceleration
+         @param out output cartesian coordinates (position, velocity
+         * and acceleration
+         *
+         * @return if < 0 something went wrong
+         */
+    virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
+
+        virtual ~ChainFkSolverAcc()=0;
+    };
+
+
+}//end of namespace KDL
+
+#endif
diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.cpp b/intern/itasc/kdl/chainfksolverpos_recursive.cpp
new file mode 100644
index 00000000000..46c29c9c6e0
--- /dev/null
+++ b/intern/itasc/kdl/chainfksolverpos_recursive.cpp
@@ -0,0 +1,61 @@
+// Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
+// 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
+
+#include "chainfksolverpos_recursive.hpp"
+#include <iostream>
+
+namespace KDL {
+
+    ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain):
+        chain(_chain)
+    {
+    }
+
+    int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
+    {
+		unsigned int segNr = (unsigned int)segmentNr;
+        if(segmentNr<0)
+             segNr=chain.getNrOfSegments();
+
+        p_out = Frame::Identity();
+
+        if(q_in.rows()!=chain.getNrOfJoints())
+            return -1;
+        else if(segNr>chain.getNrOfSegments())
+            return -1;
+        else{
+            int j=0;
+            for(unsigned int i=0;i<segNr;i++){
+                p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j));
+				j+=chain.getSegment(i).getJoint().getNDof();
+            }
+            return 0;
+        }
+    }
+
+
+    ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
+    {
+    }
+
+
+}
diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.hpp b/intern/itasc/kdl/chainfksolverpos_recursive.hpp
new file mode 100644
index 00000000000..72cdab0b28b
--- /dev/null
+++ b/intern/itasc/kdl/chainfksolverpos_recursive.hpp
@@ -0,0 +1,50 @@
+// 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
+
+#ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
+#define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
+
+#include "chainfksolver.hpp"
+
+namespace KDL {
+
+    /**
+     * Implementation of a recursive forward position kinematics
+     * algorithm to calculate the position transformation from joint
+     * space to Cartesian space of a general kinematic chain (KDL::Chain).
+     *
+     * @ingroup KinematicFamily
+     */
+    class ChainFkSolverPos_recursive : public ChainFkSolverPos
+    {
+    public:
+        ChainFkSolverPos_recursive(const Chain& chain);
+        ~ChainFkSolverPos_recursive();
+
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
+
+    private:
+        const Chain chain;
+    };
+
+}
+
+#endif
diff --git a/intern/itasc/kdl/chainjnttojacsolver.cpp b/intern/itasc/kdl/chainjnttojacsolver.cpp
new file mode 100644
index 00000000000..4a801c041f3
--- /dev/null
+++ b/intern/itasc/kdl/chainjnttojacsolver.cpp
@@ -0,0 +1,80 @@
+// 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
+
+#include "chainjnttojacsolver.hpp"
+
+namespace KDL
+{
+    ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain):
+        chain(_chain)
+    {
+    }
+
+    ChainJntToJacSolver::~ChainJntToJacSolver()
+    {
+    }
+
+    int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
+    {
+        assert(q_in.rows()==chain.getNrOfJoints()&&
+               q_in.rows()==jac.columns());
+
+
+		Frame T_local, T_joint;
+        T_total = Frame::Identity();
+        SetToZero(t_local);
+
+		int i=chain.getNrOfSegments()-1;
+		unsigned int q_nr = chain.getNrOfJoints();
+
+		//Lets recursively iterate until we are in the root segment
+		while (i >= 0) {
+			const Segment& segment = chain.getSegment(i);
+			int ndof = segment.getJoint().getNDof();
+			q_nr -= ndof;
+
+	        //get the pose of the joint.
+			T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+			// combine with the tip to have the tip pose
+			T_local = T_joint*segment.getFrameToTip();
+			//calculate new T_end:
+			T_total = T_local * T_total;
+
+			for (int dof=0; dof<ndof; dof++) {
+				// combine joint rotation with tip position to get a reference frame for the joint
+				T_joint.p = T_local.p;
+				// in which the twist can be computed (needed for NDof joint)
+				t_local = segment.twist(T_joint, 1.0, dof);
+				//transform the endpoint of the local twist to the global endpoint:
+				t_local = t_local.RefPoint(T_total.p - T_local.p);
+				//transform the base of the twist to the endpoint
+				t_local = T_total.M.Inverse(t_local);
+				//store the twist in the jacobian:
+				jac.twists[q_nr+dof] = t_local;
+			}
+			i--;
+		}//endwhile
+		//Change the base of the complete jacobian from the endpoint to the base
+		changeBase(jac, T_total.M, jac);
+        return 0;
+    }
+}
+
diff --git a/intern/itasc/kdl/chainjnttojacsolver.hpp b/intern/itasc/kdl/chainjnttojacsolver.hpp
new file mode 100644
index 00000000000..98f9d06ee0d
--- /dev/null
+++ b/intern/itasc/kdl/chainjnttojacsolver.hpp
@@ -0,0 +1,65 @@
+// 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
+
+#ifndef KDL_CHAINJNTTOJACSOLVER_HPP
+#define KDL_CHAINJNTTOJACSOLVER_HPP
+
+#include "frames.hpp"
+#include "jacobian.hpp"
+#include "jntarray.hpp"
+#include "chain.hpp"
+
+namespace KDL
+{
+    /**
+     * @brief  Class to calculate the jacobian of a general
+     * KDL::Chain, it is used by other solvers. It should not be used
+     * outside of KDL.
+     *
+     *
+     */
+
+    class ChainJntToJacSolver
+    {
+    public:
+        ChainJntToJacSolver(const Chain& chain);
+        ~ChainJntToJacSolver();
+        /**
+         * Calculate the jacobian expressed in the base frame of the
+         * chain, with reference point at the end effector of the
+         * *chain. The alghoritm is similar to the one used in
+         * KDL::ChainFkSolverVel_recursive
+         *
+         * @param q_in input joint positions
+         * @param jac output jacobian
+         *
+         * @return always returns 0
+         */
+        int JntToJac(const JntArray& q_in,Jacobian& jac);
+
+    private:
+        const Chain chain;
+        Twist t_local;
+        Frame T_total;
+    };
+}
+#endif
+
diff --git a/intern/itasc/kdl/frameacc.cpp b/intern/itasc/kdl/frameacc.cpp
new file mode 100644
index 00000000000..85c4179379a
--- /dev/null
+++ b/intern/itasc/kdl/frameacc.cpp
@@ -0,0 +1,26 @@
+/*****************************************************************************
+ *  \author
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Id: frameacc.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ *		$Name:  $
+ ****************************************************************************/
+
+
+#include "frameacc.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+    #include "frameacc.inl"
+#endif
+
+}
+
diff --git a/intern/itasc/kdl/frameacc.hpp b/intern/itasc/kdl/frameacc.hpp
new file mode 100644
index 00000000000..4157237222e
--- /dev/null
+++ b/intern/itasc/kdl/frameacc.hpp
@@ -0,0 +1,259 @@
+/*****************************************************************************
+ * \file
+ *      This file contains the definition of classes for a
+ *      Rall Algebra of (subset of) the classes defined in frames,
+ *      i.e. classes that contain a set (value,derivative,2nd derivative)
+ *      and define operations on that set
+ *      this classes are usefull for automatic differentiation ( <-> symbolic diff ,
+ *      <-> numeric diff).
+ *      Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
+ *      Look at the corresponding classes Vector Rotation Frame Twist and
+ *      Wrench for the semantics of the methods.
+ *
+ *      It also contains the 2nd derivative <-> RFrames.h
+ *
+ *  \author
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Id: frameacc.hpp 19905 2009-04-23 13:29:54Z ben2610 $
+ *      $Name:  $
+ ****************************************************************************/
+
+#ifndef RRFRAMES_H
+#define RRFRAMES_H
+
+
+#include "utilities/rall2d.h"
+#include "frames.hpp"
+
+
+
+namespace KDL {
+
+class TwistAcc;
+typedef Rall2d<double,double,double> doubleAcc;
+
+
+class VectorAcc
+{
+public:
+    Vector p;   //!< position vector
+    Vector v;   //!< velocity vector
+    Vector dv;  //!< acceleration vector
+public:
+    VectorAcc():p(),v(),dv() {}
+    explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
+    VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
+    VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
+        p(_p),v(_v),dv(_dv) {}
+    IMETHOD VectorAcc& operator = (const VectorAcc& arg);
+    IMETHOD VectorAcc& operator = (const Vector& arg);
+    IMETHOD VectorAcc& operator += (const VectorAcc& arg);
+    IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
+    IMETHOD static VectorAcc Zero();
+    IMETHOD void ReverseSign();
+    IMETHOD doubleAcc Norm();
+    IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
+    IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
+    IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
+    IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
+    IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
+
+    IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
+    IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
+
+
+    IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
+    IMETHOD friend VectorAcc operator - (const VectorAcc& r);
+    IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
+    IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
+    IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
+};
+
+
+
+class RotationAcc
+{
+public:
+    Rotation R;     //!< rotation matrix
+    Vector   w;     //!< angular velocity vector
+    Vector   dw;    //!< angular acceration vector
+public:
+    RotationAcc():R(),w() {}
+    explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
+    RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
+        R(_R),w(_w),dw(_dw) {}
+    IMETHOD RotationAcc& operator = (const RotationAcc& arg);
+    IMETHOD RotationAcc& operator = (const Rotation& arg);
+    IMETHOD static RotationAcc Identity();
+    IMETHOD RotationAcc Inverse() const;
+    IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
+    IMETHOD VectorAcc Inverse(const Vector& arg) const;
+    IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const Vector& arg) const;
+
+    //  Rotations
+    // The SetRot.. functions set the value of *this to the appropriate rotation matrix.
+    // The Rot... static functions give the value of the appropriate rotation matrix back.
+    // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+    // IMETHOD void DoRotX(const doubleAcc& angle);
+    // IMETHOD void DoRotY(const doubleAcc& angle);
+    // IMETHOD void DoRotZ(const doubleAcc& angle);
+    // IMETHOD static RRotation RotX(const doubleAcc& angle);
+    // IMETHOD static RRotation RotY(const doubleAcc& angle);
+    // IMETHOD static RRotation RotZ(const doubleAcc& angle);
+
+    // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
+    // Along an arbitrary axes.  The norm of rotvec is neglected.
+    // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
+    // rotvec has arbitrary norm
+    // rotation around a constant vector !
+    // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
+    // rotvec is normalized.
+    // rotation around a constant vector !
+
+    IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
+    IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
+    IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
+    IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
+    IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
+    IMETHOD TwistAcc Inverse(const Twist& arg) const;
+    IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
+    IMETHOD TwistAcc operator * (const Twist& arg) const;
+};
+
+
+
+
+class FrameAcc
+{
+public:
+    RotationAcc M;   //!< Rotation,angular velocity, and angular acceleration of frame.
+    VectorAcc   p;   //!< Translation, velocity and acceleration of origin.
+public:
+    FrameAcc(){}
+    explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
+    FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
+        M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
+    FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
+
+    IMETHOD FrameAcc& operator = (const FrameAcc& arg);
+    IMETHOD FrameAcc& operator = (const Frame& arg);
+    IMETHOD static FrameAcc Identity();
+    IMETHOD FrameAcc Inverse() const;
+    IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
+    IMETHOD VectorAcc operator*(const Vector& arg) const;
+    IMETHOD VectorAcc Inverse(const Vector& arg) const;
+    IMETHOD Frame GetFrame() const;
+    IMETHOD Twist GetTwist() const;
+    IMETHOD Twist GetAccTwist() const;
+    IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
+    IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
+    IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
+    IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
+
+    IMETHOD TwistAcc  Inverse(const TwistAcc& arg) const;
+    IMETHOD TwistAcc  Inverse(const Twist& arg) const;
+    IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
+    IMETHOD TwistAcc operator * (const Twist& arg) const;
+};
+
+
+
+
+
+
+
+
+//very similar to Wrench class.
+class TwistAcc
+{
+public:
+    VectorAcc vel;       //!< translational velocity and its 1st and 2nd derivative
+    VectorAcc rot;       //!< rotational velocity and its 1st and 2nd derivative
+public:
+
+     TwistAcc():vel(),rot() {};
+     TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
+
+     IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
+     IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
+
+     IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
+     IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
+
+     IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
+     IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
+
+     IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
+     IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
+
+     IMETHOD friend void SetToZero(TwistAcc& v);
+
+     static IMETHOD TwistAcc Zero();
+
+     IMETHOD void ReverseSign();
+
+     IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
+     // Changes the reference point of the RTwist.
+     // The RVector v_base_AB is expressed in the same base as the RTwist
+     // The RVector v_base_AB is a RVector from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+
+     IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
+     IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
+     IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
+
+
+     IMETHOD Twist GetTwist() const;
+     IMETHOD Twist GetTwistDot() const;
+
+    friend class RotationAcc;
+    friend class FrameAcc;
+
+};
+
+
+
+
+
+
+
+#ifdef KDL_INLINE
+#include "frameacc.inl"
+#endif
+
+}
+
+
+
+
+
+#endif
diff --git a/intern/itasc/kdl/frameacc.inl b/intern/itasc/kdl/frameacc.inl
new file mode 100644
index 00000000000..a8ea35ad436
--- /dev/null
+++ b/intern/itasc/kdl/frameacc.inl
@@ -0,0 +1,598 @@
+/*****************************************************************************
+ * \file  
+ *      provides inline functions of rrframes.h
+ *       
+ *  \author 
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version 
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Id: frameacc.inl 19905 2009-04-23 13:29:54Z ben2610 $
+ *      $Name:  $ 
+ ****************************************************************************/
+
+
+
+
+/////////////////// VectorAcc /////////////////////////////////////
+
+VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
+}
+
+VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
+}
+VectorAcc operator + (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1+r2.p,r2.v,r2.dv);
+}
+
+VectorAcc operator - (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
+}
+VectorAcc operator + (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p+r2,r1.v,r1.dv);
+}
+
+VectorAcc operator - (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p-r2, r1.v, r1.dv);
+}
+
+// unary -
+VectorAcc operator - (const VectorAcc& r) {
+    return VectorAcc(-r.p,-r.v,-r.dv);
+}
+
+// cross prod.
+VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.p*r2.p, 
+                    r1.p*r2.v+r1.v*r2.p,
+                    r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv
+                    );
+}
+
+VectorAcc operator * (const VectorAcc& r1,const Vector& r2) {
+    return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
+}
+
+VectorAcc operator * (const Vector& r1,const VectorAcc& r2) {
+    return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
+}
+
+
+
+// scalar mult.
+VectorAcc operator * (double r1,const VectorAcc& r2) {
+    return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
+}
+
+VectorAcc operator * (const VectorAcc& r1,double r2) {
+    return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
+}
+
+VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) {
+    return VectorAcc(r1.t*r2.p, 
+                   r1.t*r2.v  + r1.d*r2.p,
+                   r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
+                   );
+}
+
+VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) {
+    return VectorAcc(r1.t*r2.p, 
+               r1.t*r2.v  + r1.d*r2.p,
+               r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
+               );
+}
+
+VectorAcc& VectorAcc::operator = (const VectorAcc& arg) {
+    p=arg.p;
+    v=arg.v;
+    dv=arg.dv;
+    return *this;
+}
+
+VectorAcc& VectorAcc::operator = (const Vector& arg) {
+    p=arg;
+    v=Vector::Zero();
+    dv=Vector::Zero();
+    return *this;
+}
+
+VectorAcc& VectorAcc::operator += (const VectorAcc& arg) {
+    p+=arg.p;
+    v+=arg.v;
+    dv+= arg.dv;
+    return *this;
+}
+VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) {
+    p-=arg.p;
+    v-=arg.v;
+    dv-=arg.dv;
+    return *this;
+}
+
+VectorAcc VectorAcc::Zero() {
+    return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
+}
+
+void VectorAcc::ReverseSign() {
+    p.ReverseSign();
+    v.ReverseSign();
+    dv.ReverseSign();
+}
+
+doubleAcc VectorAcc::Norm() {
+    doubleAcc res;
+    res.t  = p.Norm();
+    res.d  = dot(p,v)/res.t;
+    res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
+    return res;
+}
+
+doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) {
+    return doubleAcc( dot(lhs.p,rhs.p),
+                    dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p),
+                    dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p)
+                    );
+}
+
+doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) {
+    return doubleAcc( dot(lhs.p,rhs),
+                     dot(lhs.v,rhs),
+                     dot(lhs.dv,rhs)
+                    );
+}
+
+doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) {
+    return doubleAcc( dot(lhs,rhs.p),
+                    dot(lhs,rhs.v),
+                    dot(lhs,rhs.dv)
+                    );
+}
+
+
+bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) {
+    return (Equal(r1.p,r2.p,eps) 
+         && Equal(r1.v,r2.v,eps)
+         && Equal(r1.dv,r2.dv,eps)
+         );
+}
+
+bool Equal(const Vector& r1,const VectorAcc& r2,double eps) {
+    return (Equal(r1,r2.p,eps) 
+         && Equal(Vector::Zero(),r2.v,eps)
+         && Equal(Vector::Zero(),r2.dv,eps)
+         );
+}
+
+bool Equal(const VectorAcc& r1,const Vector& r2,double eps) {
+    return (Equal(r1.p,r2,eps) 
+         && Equal(r1.v,Vector::Zero(),eps)
+         && Equal(r1.dv,Vector::Zero(),eps)
+         );
+}
+
+VectorAcc operator / (const VectorAcc& r1,double r2) {
+    return r1*(1.0/r2);
+}
+
+VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) {
+    return r2*(1.0/r1);
+}
+
+
+
+/////////////////// RotationAcc /////////////////////////////////////
+
+RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) {
+    return RotationAcc( r1.R  * r2.R, 
+                      r1.w  + r1.R*r2.w,  
+                      r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw 
+                      );
+}
+
+RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) {
+    return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
+}
+
+RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) {
+    return RotationAcc( r1.R*r2, r1.w, r1.dw );
+}
+
+RotationAcc& RotationAcc::operator = (const RotationAcc& arg) {
+    R=arg.R;
+    w=arg.w;
+    dw=arg.dw;
+    return *this;
+}
+RotationAcc& RotationAcc::operator = (const Rotation& arg) {
+    R = arg;
+    w = Vector::Zero();
+    dw = Vector::Zero();
+    return *this;
+}
+
+RotationAcc RotationAcc::Identity() {
+    return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
+}
+
+RotationAcc RotationAcc::Inverse() const {
+    return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
+}
+
+VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const {
+    VectorAcc tmp;
+    tmp.p  = R.Inverse(arg.p);
+    tmp.v  = R.Inverse(arg.v - w * arg.p);
+    tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v));
+    return tmp;
+}
+
+VectorAcc RotationAcc::Inverse(const Vector& arg) const {
+    VectorAcc tmp;
+    tmp.p  = R.Inverse(arg);
+    tmp.v  = R.Inverse(-w*arg);
+    tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v));
+    return tmp;
+}
+
+
+VectorAcc RotationAcc::operator*(const VectorAcc& arg) const {
+    VectorAcc tmp;
+    tmp.p = R*arg.p;
+    tmp.dv = R*arg.v;
+    tmp.v = w*tmp.p + tmp.dv;
+    tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv;
+    return tmp;
+}
+
+VectorAcc operator*(const Rotation& R,const VectorAcc& x) {
+    return VectorAcc(R*x.p,R*x.v,R*x.dv);
+}
+
+VectorAcc RotationAcc::operator*(const Vector& arg) const {
+    VectorAcc tmp;
+    tmp.p = R*arg;
+    tmp.v = w*tmp.p;
+    tmp.dv = dw*tmp.p + w*tmp.v;
+    return tmp;
+}
+
+/*
+        // = Rotations
+        // The Rot... static functions give the value of the appropriate rotation matrix back.
+        // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+
+        void RRotation::DoRotX(const RDouble& angle) {
+            w+=R*Vector(angle.grad,0,0);
+            R.DoRotX(angle.t);
+        }
+RotationAcc RotationAcc::RotX(const doubleAcc& angle) {
+    return RotationAcc(Rotation::RotX(angle.t),
+                      Vector(angle.d,0,0),
+                      Vector(angle.dd,0,0)
+                      );
+}
+
+        void RRotation::DoRotY(const RDouble& angle) {
+            w+=R*Vector(0,angle.grad,0);
+            R.DoRotY(angle.t);
+        }
+RotationAcc RotationAcc::RotY(const doubleAcc& angle) {
+    return RotationAcc(
+              Rotation::RotX(angle.t),
+              Vector(0,angle.d,0),
+              Vector(0,angle.dd,0)
+            );
+}
+
+        void RRotation::DoRotZ(const RDouble& angle) {
+            w+=R*Vector(0,0,angle.grad);
+            R.DoRotZ(angle.t);
+        }
+RotationAcc RotationAcc::RotZ(const doubleAcc& angle) {
+    return RotationAcc(
+                Rotation::RotZ(angle.t),
+                Vector(0,0,angle.d),
+                Vector(0,0,angle.dd)
+            );
+}
+
+
+        RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) 
+        // rotvec has arbitrary norm
+        // rotation around a constant vector !
+        {
+            Vector v = rotvec.Normalize();
+            return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad);
+        }
+
+        RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) 
+            // rotvec is normalized.
+        {
+            return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
+        }
+
+*/
+
+bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) {
+    return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) );
+}
+bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) {
+    return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && 
+            Equal(Vector::Zero(),r2.dw,eps) );
+}
+bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) {
+    return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && 
+            Equal(r1.dw,Vector::Zero(),eps) );
+}
+
+
+// Methods and operators related to FrameAcc
+// They all delegate most of the work to RotationAcc and VectorAcc
+FrameAcc& FrameAcc::operator = (const FrameAcc& arg) {
+    M=arg.M;
+    p=arg.p;
+    return *this;
+}
+
+FrameAcc FrameAcc::Identity() {
+    return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
+}
+
+
+FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs)
+{
+    return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+
+VectorAcc FrameAcc::operator *(const VectorAcc & arg) const
+{
+    return M*arg+p;
+}
+VectorAcc FrameAcc::operator *(const Vector & arg) const
+{
+    return M*arg+p;
+}
+
+VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+VectorAcc FrameAcc::Inverse(const Vector& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+FrameAcc FrameAcc::Inverse() const
+{
+    return FrameAcc(M.Inverse(),-M.Inverse(p));
+}
+
+FrameAcc& FrameAcc::operator =(const Frame & arg)
+{ 
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+
+bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const Frame& r1,const FrameAcc& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const FrameAcc& r1,const Frame& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+
+
+Frame FrameAcc::GetFrame() const {
+    return Frame(M.R,p.p);
+}
+
+
+Twist FrameAcc::GetTwist() const {
+    return Twist(p.v,M.w);
+}
+
+
+Twist FrameAcc::GetAccTwist() const {
+    return Twist(p.dv,M.dw);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+TwistAcc TwistAcc::Zero()
+{
+    return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
+}
+
+
+void TwistAcc::ReverseSign()
+{   
+    vel.ReverseSign();
+    rot.ReverseSign();
+}
+
+TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB)
+     // Changes the reference point of the TwistAcc.
+     // The RVector v_base_AB is expressed in the same base as the TwistAcc
+     // The RVector v_base_AB is a RVector from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+{
+    return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
+}
+
+TwistAcc& TwistAcc::operator-=(const TwistAcc& arg)
+{
+    vel-=arg.vel;
+    rot -=arg.rot;
+    return *this;
+}
+
+TwistAcc& TwistAcc::operator+=(const TwistAcc& arg)
+{
+    vel+=arg.vel;
+    rot +=arg.rot;
+    return *this;
+}
+
+
+TwistAcc operator*(const TwistAcc& lhs,double rhs)
+{
+    return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistAcc operator*(double lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistAcc operator/(const TwistAcc& lhs,double rhs)
+{
+    return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs)
+{
+    return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs)
+{
+    return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+
+// addition of TwistAcc's
+TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
+}
+
+TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs)
+{
+    return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
+}
+
+// unary -
+TwistAcc operator-(const TwistAcc& arg) 
+{
+    return TwistAcc(-arg.vel,-arg.rot);
+}
+
+
+
+
+
+TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const
+{
+    return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistAcc RotationAcc::operator * (const TwistAcc& arg) const
+{
+    return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
+}
+
+TwistAcc RotationAcc::Inverse(const Twist& arg) const
+{
+    return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistAcc RotationAcc::operator * (const Twist& arg) const
+{
+    return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
+}
+
+
+TwistAcc FrameAcc::operator * (const TwistAcc& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistAcc FrameAcc::operator * (const Twist& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+TwistAcc FrameAcc::Inverse(const Twist& arg) const
+{
+    TwistAcc tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+Twist TwistAcc::GetTwist() const {
+    return Twist(vel.p,rot.p);
+}
+
+Twist TwistAcc::GetTwistDot() const {
+    return Twist(vel.v,rot.v);
+}
+
+bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const Twist& a,const TwistAcc& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const TwistAcc& a,const Twist& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+
diff --git a/intern/itasc/kdl/frames.cpp b/intern/itasc/kdl/frames.cpp
new file mode 100644
index 00000000000..7dcc39f2cd4
--- /dev/null
+++ b/intern/itasc/kdl/frames.cpp
@@ -0,0 +1,389 @@
+/***************************************************************************
+                        frames.cxx -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   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                                *
+ *                                                                         *
+ ***************************************************************************/
+
+#include "frames.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+#include "frames.inl"
+#endif
+
+void Frame::Make4x4(double * d)
+{
+    int i;
+    int j;
+    for (i=0;i<3;i++) {
+        for (j=0;j<3;j++)
+            d[i*4+j]=M(i,j);
+        d[i*4+3] = p(i)/1000;
+    }
+    for (j=0;j<3;j++)
+        d[12+j] = 0.;
+    d[15] = 1;
+}
+
+Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
+// returns Modified Denavit-Hartenberg parameters (According to Craig)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,       -st,     0,
+                    st*ca,  ct*ca,   -sa,
+                    st*sa,  ct*sa,    ca   ),
+                 Vector(
+                    a,      -sa*d,  ca*d   )
+            );
+}
+
+Frame Frame::DH(double a,double alpha,double d,double theta)
+// returns Denavit-Hartenberg parameters (Non-Modified DH)
+{
+    double ct,st,ca,sa;
+    ct = cos(theta);
+    st = sin(theta);
+    sa = sin(alpha);
+    ca = cos(alpha);
+    return Frame(Rotation(
+                    ct,    -st*ca,   st*sa,
+                    st,     ct*ca,  -ct*sa,
+                     0,        sa,      ca   ),
+                 Vector(
+                    a*ct,      a*st,  d   )
+            );
+}
+
+double Vector2::Norm() const
+{
+    double tmp0 = fabs(data[0]);
+    double tmp1 = fabs(data[1]);
+    if (tmp0 >= tmp1) {
+		if (tmp1 == 0)
+			return 0;
+        return tmp0*sqrt(1+sqr(tmp1/tmp0));
+    } else {
+        return tmp1*sqrt(1+sqr(tmp0/tmp1));
+    }
+}
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector2::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector2(1,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+// do some effort not to lose precision
+double Vector::Norm() const
+{
+    double tmp1;
+    double tmp2;
+    tmp1 = fabs(data[0]);
+    tmp2 = fabs(data[1]);
+    if (tmp1 >= tmp2) {
+        tmp2=fabs(data[2]);
+        if (tmp1 >= tmp2) {
+        	if (tmp1 == 0) {
+        		// only to everything exactly zero case, all other are handled correctly
+        		return 0;
+        	}
+            return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
+        } else {
+            return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    } else {
+        tmp1=fabs(data[2]);
+        if (tmp2 > tmp1) {
+            return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
+        } else {
+            return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
+        }
+    }
+}
+
+// makes v a unitvector and returns the norm of v.
+// if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+// if this is not good, check the return value of this method.
+double Vector::Normalize(double eps) {
+	double v = this->Norm();
+	if (v < eps) {
+		*this = Vector(1,0,0);
+		return v;
+	} else {
+		*this = (*this)/v;
+		return v;
+	}
+}
+
+
+bool Equal(const Rotation& a,const Rotation& b,double eps) {
+    return (Equal(a.data[0],b.data[0],eps) &&
+            Equal(a.data[1],b.data[1],eps) &&
+            Equal(a.data[2],b.data[2],eps) &&
+            Equal(a.data[3],b.data[3],eps) &&
+            Equal(a.data[4],b.data[4],eps) &&
+            Equal(a.data[5],b.data[5],eps) &&
+            Equal(a.data[6],b.data[6],eps) &&
+            Equal(a.data[7],b.data[7],eps) &&
+            Equal(a.data[8],b.data[8],eps)    );
+}
+
+void Rotation::Ortho()
+{
+	double n;
+	n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; 
+	n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; 
+	n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; 
+}
+
+Rotation operator *(const Rotation& lhs,const Rotation& rhs)
+// Complexity : 27M+27A
+{
+    return Rotation(
+        lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
+        lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
+        lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
+        lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
+        lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
+        lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
+        lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
+        lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
+        lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
+    );
+
+}
+
+
+Rotation Rotation::RPY(double roll,double pitch,double yaw)
+    {
+        double ca1,cb1,cc1,sa1,sb1,sc1;
+        ca1 = cos(yaw); sa1 = sin(yaw);
+        cb1 = cos(pitch);sb1 = sin(pitch);
+        cc1 = cos(roll);sc1 = sin(roll);
+        return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
+                   sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
+                   -sb1,cb1*sc1,cb1*cc1);
+    }
+
+// Gives back a rotation matrix specified with RPY convention
+void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
+    {
+        if (fabs(data[6]) > 1.0 - epsilon ) {
+            roll = -sign(data[6]) * atan2(data[1], data[4]);
+            pitch= -sign(data[6]) * PI / 2;
+            yaw  = 0.0 ;
+        } else {
+            roll  = atan2(data[7], data[8]);
+            pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
+            yaw   = atan2(data[3], data[0]);
+        }
+    }
+
+Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
+        double sa,ca,sb,cb,sg,cg;
+        sa  = sin(Alfa);ca = cos(Alfa);
+        sb  = sin(Beta);cb = cos(Beta);
+        sg  = sin(Gamma);cg = cos(Gamma);
+        return Rotation( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
+                 sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
+                 -sb*cg ,                sb*sg,              cb
+                );
+
+     }
+
+
+void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const {
+        if (fabs(data[6]) < epsilon ) {
+            alfa=0.0;
+            if (data[8]>0) {
+                beta = 0.0;
+                gamma= atan2(-data[1],data[0]);
+            } else {
+                beta = PI;
+                gamma= atan2(data[1],-data[0]);
+            }
+        } else {
+            alfa=atan2(data[5], data[2]);
+            beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
+            gamma=atan2(data[7], -data[6]);
+        }
+ }
+
+Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    Vector rotvec = rotaxis;
+	rotvec.Normalize();
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+    }
+
+Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
+    // rotvec should be normalized !
+    // The formula is
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+    double ct = cos(angle);
+    double st = sin(angle);
+    double vt = 1-ct;
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0),
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+}
+
+
+
+Vector Rotation::GetRot() const
+         // Returns a vector with the direction of the equiv. axis
+         // and its norm is angle
+     {
+       Vector axis  = Vector((data[7]-data[5]),
+			     (data[2]-data[6]),
+			     (data[3]-data[1]) )/2;
+
+       double sa    = axis.Norm();
+       double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+       double alfa;
+       if (sa > epsilon)
+           alfa = ::atan2(sa,ca)/sa;
+	   else {
+		   if (ca < 0.0) {
+			   alfa = KDL::PI;
+			   axis.data[0] = 0.0;
+			   axis.data[1] = 0.0;
+			   axis.data[2] = 0.0;
+			   if (data[0] > 0.0) {
+				   axis.data[0] = 1.0;
+			   } else if (data[4] > 0.0) {
+				   axis.data[1] = 1.0;
+			   } else {
+				   axis.data[2] = 1.0;
+			   }
+		   } else {
+			   alfa = 0.0;
+		   }
+	   }
+       return axis * alfa;
+     }
+
+Vector2 Rotation::GetXZRot() const
+{
+	// [0,1,0] x Y
+	Vector2 axis(data[7], -data[1]);
+	double norm = axis.Normalize();
+	if (norm < epsilon) {
+		norm = (data[4] < 0.0) ? PI : 0.0;
+	} else {
+		norm = acos(data[4]);
+	}
+	return axis*norm;
+}
+
+
+/** Returns the rotation angle around the equiv. axis
+ * @param axis the rotation axis is returned in this variable
+ * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
+ *                                         to be +/- Z-axis
+ *               in the case of angle == PI : 2 solutions, positive Z-component
+ *                                            of the axis is choosen.
+ * @result returns the rotation angle (between [0..PI] )
+ * /todo :
+ *   Check corresponding routines in rframes and rrframes
+ */
+double Rotation::GetRotAngle(Vector& axis,double eps) const {
+	double ca    = (data[0]+data[4]+data[8]-1)/2.0;
+	if (ca>1-eps) {
+		// undefined choose the Z-axis, and angle 0
+		axis = Vector(0,0,1);
+		return 0;
+	}
+	if (ca < -1+eps) {
+		// two solutions, choose a positive Z-component of the axis
+		double z = sqrt( (data[8]+1)/2 );
+		double x = (data[2])/2/z;
+		double y = (data[5])/2/z;
+		axis = Vector( x,y,z  );
+		return PI;
+	}
+	double angle = acos(ca);
+	double sa    = sin(angle);
+	axis  = Vector((data[7]-data[5])/2/sa,
+                       (data[2]-data[6])/2/sa,
+                       (data[3]-data[1])/2/sa  );
+	return angle;
+}
+
+bool operator==(const Rotation& a,const Rotation& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+    return ( a.data[0]==b.data[0] &&
+             a.data[1]==b.data[1] &&
+             a.data[2]==b.data[2] &&
+             a.data[3]==b.data[3] &&
+             a.data[4]==b.data[4] &&
+             a.data[5]==b.data[5] &&
+             a.data[6]==b.data[6] &&
+             a.data[7]==b.data[7] &&
+             a.data[8]==b.data[8]  );
+#endif
+}
+}
diff --git a/intern/itasc/kdl/frames.hpp b/intern/itasc/kdl/frames.hpp
new file mode 100644
index 00000000000..20590c5303e
--- /dev/null
+++ b/intern/itasc/kdl/frames.hpp
@@ -0,0 +1,1097 @@
+/***************************************************************************
+                        frames.hpp `-  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   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
+ * \warning
+ *       Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of
+ *          p2=A*B*C*p1
+ *
+ * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS
+ *
+ * \verbatim
+ *      A naming convention of objects of the type defined in this file :
+ *          (1) Frame : F...
+ *              Rotation : R ...
+ *          (2) Twist    : T ...
+ *              Wrench   : W ...
+ *              Vector   : V ...
+ *      This prefix is followed by :
+ *      for category (1) :
+ *          F_A_B : w.r.t. frame A, frame B expressed
+ *          ( each column of F_A_B corresponds to an axis of B,
+ *            expressed w.r.t. frame A )
+ *          in mathematical convention :
+ *                   A
+ *         F_A_B ==    F
+ *                   B
+ *
+ *      for category (2) :
+ *          V_B   : a vector expressed w.r.t. frame B
+ *
+ *      This can also be prepended by a name :
+ *          e.g. : temporaryV_B
+ *
+ *      With this convention one can write :
+ *
+ *      F_A_B = F_B_A.Inverse();
+ *      F_A_C = F_A_B * F_B_C;
+ *      V_B   = F_B_C * V_C;    // both translation and rotation
+ *      V_B   = R_B_C * V_C;    // only rotation
+ * \endverbatim
+ *
+ * \par CONVENTIONS FOR WHEN USED WITH ROBOTS :
+ *
+ * \verbatim
+ *       world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]')
+ *       mp    : represents mounting plate of a robot
+ *               (i.e. everything before MP is constructed by robot manufacturer
+ *                    everything after MP is tool )
+ *       tf    : represents task frame of a robot
+ *               (i.e. frame in which motion and force control is expressed)
+ *       sf    : represents sensor frame of a robot
+ *               (i.e. frame at which the forces measured by the force sensor
+ *               are expressed )
+ *
+ *          Frame F_world_mp=...;
+ *          Frame F_mp_sf(..)
+ *          Frame F_mp_tf(,.)
+ *
+ *          Wrench are measured in sensor frame SF, so one could write :
+ *                Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf );
+ * \endverbatim
+ *
+ * \par CONVENTIONS REGARDING UNITS :
+ *      Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec
+ *
+ * \par Twist and Wrench transformations
+ * 3 different types of transformations do exist for the twists
+ * and wrenches.
+ *
+ * \verbatim
+ *      1) Frame * Twist or Frame * Wrench :
+ *              this transforms both the velocity/force reference point
+ *             and the basis to which the twist/wrench are expressed.
+ *      2) Rotation * Twist or Rotation * Wrench :
+ *              this transforms the basis to which the twist/wrench are
+ *              expressed, but leaves the reference point intact.
+ *      3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB)
+ *              this transforms only the reference point. v is expressed
+ *              in the same base as the twist/wrench and points from the
+ *              old reference point to the new reference point.
+ * \endverbatim
+ *
+ * \par Complexity
+ *  Sometimes the amount of work is given in the documentation
+ *  e.g. 6M+3A means 6 multiplications and 3 additions.
+ *
+ *  \author
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ ****************************************************************************/
+#ifndef KDL_FRAMES_H
+#define KDL_FRAMES_H
+
+
+#include "utilities/kdl-config.h"
+#include "utilities/utility.h"
+
+/////////////////////////////////////////////////////////////
+
+namespace KDL {
+
+
+
+class Vector;
+class Rotation;
+class Frame;
+class Wrench;
+class Twist;
+class Vector2;
+class Rotation2;
+class Frame2;
+
+
+
+/**
+ * \brief A concrete implementation of a 3 dimensional vector class
+ */
+class Vector
+{
+public:
+    double data[3];
+     //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that
+     inline Vector() {data[0]=data[1]=data[2] = 0.0;}
+
+     //! Constructs a vector out of the three values x, y and z
+     inline Vector(double x,double y, double z);
+
+     //! Constructs a vector out of an array of three values x, y and z
+     inline Vector(double* xyz);
+
+     //! Constructs a vector out of an array of three values x, y and z
+     inline Vector(float* xyz);
+
+	 //! Assignment operator. The normal copy by value semantics.
+     inline Vector(const Vector& arg);
+
+	 //! store vector components in array
+	 inline void GetValue(double* xyz) const;
+
+	 //! Assignment operator. The normal copy by value semantics.
+     inline Vector& operator = ( const Vector& arg);
+
+     //! Access to elements, range checked when NDEBUG is not set, from 0..2
+     inline double operator()(int index) const;
+
+     //! Access to elements, range checked when NDEBUG is not set, from 0..2
+     inline double& operator() (int index);
+
+	 //! Equivalent to double operator()(int index) const
+     double operator[] ( int index ) const
+       {
+	 return this->operator() ( index );
+       }
+
+	 //! Equivalent to double& operator()(int index)
+     double& operator[] ( int index )
+       {
+	 return this->operator() ( index );
+       }
+
+     inline double x() const;
+     inline double y() const;
+     inline double z() const;
+     inline void x(double);
+     inline void y(double);
+     inline void z(double);
+
+     //! Reverses the sign of the Vector object itself
+     inline void ReverseSign();
+
+
+     //! subtracts a vector from the Vector object itself
+     inline Vector& operator-=(const Vector& arg);
+
+
+     //! Adds a vector from the Vector object itself
+     inline Vector& operator +=(const Vector& arg);
+
+     //! Scalar multiplication is defined
+     inline friend Vector operator*(const Vector& lhs,double rhs);
+     //! Scalar multiplication is defined
+     inline friend Vector operator*(double lhs,const Vector& rhs);
+     //! Scalar division is defined
+
+     inline friend Vector operator/(const Vector& lhs,double rhs);
+     inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
+     inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
+     inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
+     inline friend Vector operator-(const Vector& arg);
+     inline friend double dot(const Vector& lhs,const Vector& rhs);
+
+     //! To have a uniform operator to put an element to zero, for scalar values
+     //! and for objects.
+     inline friend void SetToZero(Vector& v);
+
+     //! @return a zero vector
+     inline static Vector Zero();
+
+   /** Normalizes this vector and returns it norm
+	* makes v a unitvector and returns the norm of v.
+	* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+	* if this is not good, check the return value of this method.
+	*/
+     double Normalize(double eps=epsilon);
+
+     //!    @return the norm of the vector
+     double Norm() const;
+
+
+
+     //! a 3D vector where the 2D vector v is put in the XY plane
+     inline void Set2DXY(const Vector2& v);
+     //! a 3D vector where the 2D vector v is put in the YZ plane
+     inline void Set2DYZ(const Vector2& v);
+     //! a 3D vector where the 2D vector v is put in the ZX plane
+     inline void Set2DZX(const Vector2& v);
+     //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
+     inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
+
+	 //! return a normalized vector
+	 inline friend Vector Normalize(const Vector& a, double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     inline friend bool operator==(const Vector& a,const Vector& b);
+	 //! The literal inequality operator!=().
+     inline friend bool operator!=(const Vector& a,const Vector& b);
+
+     friend class Rotation;
+     friend class Frame;
+};
+
+
+/**
+  \brief represents rotations in 3 dimensional space.
+
+  This class represents a rotation matrix with the following
+  conventions :
+ \verbatim
+     Suppose V2 = R*V,                                    (1)
+     V is expressed in frame B
+     V2 is expressed in frame A
+     This matrix R consists of 3 collumns [ X,Y,Z ],
+     X,Y, and Z contain the axes of frame B, expressed in frame A
+     Because of linearity expr(1) is valid.
+ \endverbatim
+   This class only represents rotational_interpolation, not translation
+ Two interpretations are possible for rotation angles.
+ * if you rotate with angle around X frame A to have frame B,
+   then the result of SetRotX is equal to frame B expressed wrt A.
+     In code:
+ \verbatim
+      Rotation R;
+      F_A_B = R.SetRotX(angle);
+ \endverbatim
+ * Secondly, if you take the following code :
+ \verbatim
+      Vector p,p2; Rotation R;
+      R.SetRotX(angle);
+      p2 = R*p;
+ \endverbatim
+   then the frame p2 is rotated around X axis with (-angle).
+   Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot
+ \par type
+  Concrete implementation
+*/
+class Rotation
+{
+public:
+    double data[9];
+
+    inline Rotation() {
+		*this = Rotation::Identity();
+	}
+    inline Rotation(double Xx,double Yx,double Zx,
+                double Xy,double Yy,double Zy,
+                double Xz,double Yz,double Zz);
+    inline Rotation(const Vector& x,const Vector& y,const Vector& z);
+    // default copy constructor is sufficient
+
+	inline void setValue(float* oglmat);
+	inline void getValue(float* oglmat) const;
+
+     inline Rotation& operator=(const Rotation& arg);
+
+     //!  Defines a multiplication R*V between a Rotation R and a Vector V.
+     //! Complexity : 9M+6A
+     inline Vector operator*(const Vector& v) const;
+
+     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
+     inline double& operator()(int i,int j);
+
+     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
+     inline double operator() (int i,int j) const;
+
+     friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
+
+     //! Sets the value of *this to its inverse.
+     inline void SetInverse();
+
+     //! Gives back the inverse rotation matrix of *this.
+     inline Rotation Inverse() const;
+
+     //! The same as R.Inverse()*v but more efficient.
+     inline Vector Inverse(const Vector& v) const;
+
+     //! The same as R.Inverse()*arg but more efficient.
+     inline Wrench Inverse(const Wrench& arg) const;
+
+     //! The same as R.Inverse()*arg but more efficient.
+     inline Twist Inverse(const Twist& arg) const;
+
+     //! Gives back an identity rotaton matrix
+     inline static Rotation Identity();
+
+
+// = Rotations
+    //! The Rot... static functions give the value of the appropriate rotation matrix back.
+    inline static Rotation RotX(double angle);
+    //! The Rot... static functions give the value of the appropriate rotation matrix back.
+    inline static Rotation RotY(double angle);
+    //! The Rot... static functions give the value of the appropriate rotation matrix back.
+    inline static Rotation RotZ(double angle);
+    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
+    //! DoRot... functions are only defined when they can be executed more efficiently
+    inline void DoRotX(double angle);
+    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
+    //! DoRot... functions are only defined when they can be executed more efficiently
+    inline void DoRotY(double angle);
+    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
+    //! DoRot... functions are only defined when they can be executed more efficiently
+    inline void DoRotZ(double angle);
+
+    //! Along an arbitrary axes.  It is not necessary to normalize rotaxis.
+    //! returns identity rotation matrix in the case that the norm of rotaxis
+    //! is to small to be used.
+    // @see Rot2 if you want to handle this error in another way.
+    static Rotation Rot(const Vector& rotaxis,double angle);
+
+	//! Along an arbitrary axes.  rotvec should be normalized.
+    static Rotation Rot2(const Vector& rotvec,double angle);
+	
+	// make sure the matrix is a pure rotation (no scaling)
+	void Ortho();
+
+    //! Returns a vector with the direction of the equiv. axis
+    //! and its norm is angle
+    Vector GetRot() const;
+
+    //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the
+    //! Y axis onto the Matrix Y axis and its norm is angle
+    Vector2 GetXZRot() const;
+
+	/** Returns the rotation angle around the equiv. axis
+	 * @param axis the rotation axis is returned in this variable
+	 * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
+	 *                                         to be +/- Z-axis
+	 *               in the case of angle == PI : 2 solutions, positive Z-component
+	 *                                            of the axis is choosen.
+	 * @result returns the rotation angle (between [0..PI] )
+	 */
+	double GetRotAngle(Vector& axis,double eps=epsilon) const;
+
+
+    //! Gives back a rotation matrix specified with EulerZYZ convention :
+    //!  First rotate around Z with alfa,
+    //!  then around the new Y with beta, then around
+    //!  new Z with gamma.
+    static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
+
+    //! Gives back the EulerZYZ convention description of the rotation matrix :
+    //!  First rotate around Z with alfa,
+    //!  then around the new Y with beta, then around
+    //!  new Z with gamma.
+    //!
+    //! Variables are bound by
+    //!  (-PI <= alfa <= PI),
+    //! (0 <= beta <= PI),
+    //!  (-PI <= alfa <= PI)
+    void GetEulerZYZ(double& alfa,double& beta,double& gamma) const;
+
+
+    //! Sets the value of this object to a rotation specified with RPY convention:
+    //! first rotate around X with roll, then around the
+    //!               old Y with pitch, then around old Z with alfa
+    static Rotation RPY(double roll,double pitch,double yaw);
+
+    //! Gives back a vector in RPY coordinates, variables are bound by
+    //!   -PI <= roll <= PI
+    //!    -PI <= Yaw  <= PI
+    //!   -PI/2 <= PITCH <= PI/2
+    //!
+    //!  convention : first rotate around X with roll, then around the
+    //!               old Y with pitch, then around old Z with alfa
+    void GetRPY(double& roll,double& pitch,double& yaw) const;
+
+
+    //! Gives back a rotation matrix specified with EulerZYX convention :
+    //!  First rotate around Z with alfa,
+    //!  then around the new Y with beta, then around
+    //!  new X with gamma.
+    //!
+    //! closely related to RPY-convention
+    inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
+        return RPY(Gamma,Beta,Alfa);
+    }
+
+    //! GetEulerZYX gets the euler ZYX parameters of a rotation :
+    //!  First rotate around Z with alfa,
+    //!  then around the new Y with beta, then around
+    //!  new X with gamma.
+    //!
+    //! Range of the results of GetEulerZYX :
+    //!   -PI <= alfa <= PI
+    //!    -PI <= gamma <= PI
+    //!   -PI/2 <= beta <= PI/2
+    //!
+    //! Closely related to RPY-convention.
+    inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
+        GetRPY(Gamma,Beta,Alfa);
+    }
+
+     //! Transformation of the base to which the twist is expressed.
+     //! Complexity : 18M+12A
+     //! @see Frame*Twist for a transformation that also transforms
+     //! the velocity reference point.
+     inline Twist operator * (const Twist& arg) const;
+
+     //! Transformation of the base to which the wrench is expressed.
+     //! Complexity : 18M+12A
+     //! @see Frame*Wrench for a transformation that also transforms
+     //! the force reference point.
+     inline Wrench operator * (const Wrench& arg) const;
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline Vector UnitX() const {
+         return Vector(data[0],data[3],data[6]);
+     }
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline void UnitX(const Vector& X) {
+        data[0] = X(0);
+        data[3] = X(1);
+        data[6] = X(2);
+     }
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline Vector UnitY() const {
+         return Vector(data[1],data[4],data[7]);
+     }
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline void UnitY(const Vector& X) {
+        data[1] = X(0);
+        data[4] = X(1);
+        data[7] = X(2);
+     }
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline Vector UnitZ() const {
+         return Vector(data[2],data[5],data[8]);
+     }
+
+     //! Access to the underlying unitvectors of the rotation matrix
+     inline void UnitZ(const Vector& X) {
+        data[2] = X(0);
+        data[5] = X(1);
+        data[8] = X(2);
+     }
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     friend bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     friend bool operator==(const Rotation& a,const Rotation& b);
+	 //! The literal inequality operator!=()
+     friend bool operator!=(const Rotation& a,const Rotation& b);
+
+     friend class Frame;
+};
+    bool operator==(const Rotation& a,const Rotation& b);
+
+
+
+/**
+	\brief represents a frame transformation in 3D space (rotation + translation)
+
+    if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B)
+    then V2 = Frame.M*V1+Frame.p
+
+    Frame.M contains columns that represent the axes of frame B wrt frame A
+    Frame.p contains the origin of frame B expressed in frame A.
+*/
+class Frame {
+public:
+    Vector p;       //!< origine of the Frame
+    Rotation M;     //!< Orientation of the Frame
+
+public:
+
+     inline Frame(const Rotation& R,const Vector& V);
+
+     //! The rotation matrix defaults to identity
+     explicit inline Frame(const Vector& V);
+     //! The position matrix defaults to zero
+     explicit inline Frame(const Rotation& R);
+
+	 inline void setValue(float* oglmat);
+	 inline void getValue(float* oglmat) const;
+
+     inline Frame() {}
+     //! The copy constructor. Normal copy by value semantics.
+     inline Frame(const Frame& arg);
+
+     //! Reads data from an double array
+     //\TODO should be formulated as a constructor
+     void Make4x4(double* d);
+
+     //!  Treats a frame as a 4x4 matrix and returns element i,j
+     //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
+     inline double operator()(int i,int j);
+
+     //!  Treats a frame as a 4x4 matrix and returns element i,j
+     //!    Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
+     inline double operator() (int i,int j) const;
+
+	 // = Inverse
+     //! Gives back inverse transformation of a Frame
+     inline Frame Inverse() const;
+
+     //! The same as p2=R.Inverse()*p but more efficient.
+     inline Vector Inverse(const Vector& arg) const;
+
+     //! The same as p2=R.Inverse()*p but more efficient.
+     inline Wrench Inverse(const Wrench& arg) const;
+
+     //! The same as p2=R.Inverse()*p but more efficient.
+     inline Twist  Inverse(const Twist& arg) const;
+
+     //! Normal copy-by-value semantics.
+     inline Frame& operator = (const Frame& arg);
+
+     //! Transformation of the base to which the vector
+     //! is expressed.
+     inline Vector operator * (const Vector& arg) const;
+
+     //! Transformation of both the force reference point
+     //! and of the base to which the wrench is expressed.
+     //! look at Rotation*Wrench operator for a transformation
+     //! of only the base to which the twist is expressed.
+     //!
+     //! Complexity : 24M+18A
+     inline Wrench operator * (const Wrench& arg) const;
+
+     //! Transformation of both the velocity reference point
+     //! and of the base to which the twist is expressed.
+     //! look at Rotation*Twist for a transformation of only the
+     //! base to which the twist is expressed.
+     //!
+     //! Complexity : 24M+18A
+     inline Twist operator * (const Twist& arg) const;
+
+     //! Composition of two frames.
+     inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
+
+     //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
+     inline static Frame Identity();
+
+     //! The twist <t_this> is expressed wrt the current
+     //! frame.  This frame is integrated into an updated frame with
+     //! <samplefrequency>.  Very simple first order integration rule.
+     inline void Integrate(const Twist& t_this,double frequency);
+
+    /*
+    // DH_Craig1989 : constructs a transformationmatrix
+    // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
+    // described in the Craigs book: Craig, J. J.,Introduction to
+    // Robotics: Mechanics and Control, Addison-Wesley,
+    // isbn:0-201-10326-5, 1986.
+    //
+    // Note that the frame is a redundant way to express the information
+    // in the DH-convention.
+    // \verbatim
+    // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
+    //
+    //  axis i-1 is connected by link i-1 to axis i numbering axis 1
+    //  to axis n link 0 (immobile base) to link n
+    //
+    //  link length a(i-1) length of the mutual perpendicular line
+    //  (normal) between the 2 axes.  This normal runs from (i-1) to
+    //  (i) axis.
+    //
+    //  link twist alpha(i-1): construct plane perpendicular to the
+    //  normal project axis(i-1) and axis(i) into plane angle from
+    //  (i-1) to (i) measured in the direction of the normal
+    //
+    //  link offset d(i) signed distance between normal (i-1) to (i)
+    //  and normal (i) to (i+1) along axis i joint angle theta(i)
+    //  signed angle between normal (i-1) to (i) and normal (i) to
+    //  (i+1) along axis i
+    //
+    //   First and last joints : a(0)= a(n) = 0
+    //   alpha(0) = alpha(n) = 0
+    //
+    //   PRISMATIC : theta(1) = 0 d(1) arbitrarily
+    //
+    //   REVOLUTE : theta(1) arbitrarily d(1) = 0
+    //
+    //   Not unique : if intersecting joint axis 2 choices for normal
+    //   Frame assignment of the DH convention : Z(i-1) follows axis
+    //   (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
+    //   Y(i-1) follows out of Z(i-1) and X(i-1)
+    //
+    //     a(i-1)     = distance from Z(i-1) to Z(i) along X(i-1)
+    //     alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
+    //     d(i)       = distance from X(i-1) to X(i) along Z(i)
+    //     theta(i)   = angle between X(i-1) to X(i) along X(i)
+    // \endverbatim
+    */
+     static Frame DH_Craig1989(double a,double alpha,double d,double theta);
+
+    // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
+    // the Denavit-Hartenberg convention as described in the original
+    // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
+    // notation for lower-pair mechanisms based on matrices, ASME
+    // Journal of Applied Mechanics, 23:215-221, 1955.
+
+     static Frame DH(double a,double alpha,double d,double theta);
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     inline friend bool operator==(const Frame& a,const Frame& b);
+	 //! The literal inequality operator!=().
+     inline friend bool operator!=(const Frame& a,const Frame& b);
+};
+
+/**
+ * \brief represents both translational and rotational velocities.
+ *
+ * This class represents a twist.  A twist is the combination of translational
+ * velocity and rotational velocity applied at one point.
+*/
+class Twist {
+public:
+    Vector vel; //!< The velocity of that point
+    Vector rot; //!< The rotational velocity of that point.
+public:
+
+    //! The default constructor initialises to Zero via the constructor of Vector.
+    Twist():vel(),rot() {};
+
+    Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
+
+    inline Twist& operator-=(const Twist& arg);
+    inline Twist& operator+=(const Twist& arg);
+    //! index-based access to components, first vel(0..2), then rot(3..5)
+    inline double& operator()(int i);
+
+    //! index-based access to components, first vel(0..2), then rot(3..5)
+    //! For use with a const Twist
+    inline double operator()(int i) const;
+
+     double operator[] ( int index ) const
+       {
+	 return this->operator() ( index );
+       }
+
+     double& operator[] ( int index )
+       {
+	 return this->operator() ( index );
+       }
+
+     inline friend Twist operator*(const Twist& lhs,double rhs);
+     inline friend Twist operator*(double lhs,const Twist& rhs);
+     inline friend Twist operator/(const Twist& lhs,double rhs);
+     inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
+     inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
+     inline friend Twist operator-(const Twist& arg);
+     inline friend double dot(const Twist& lhs,const Wrench& rhs);
+     inline friend double dot(const Wrench& rhs,const Twist& lhs);
+     inline friend void SetToZero(Twist& v);
+
+
+     //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero())
+     static inline Twist Zero();
+
+     //! Reverses the sign of the twist
+     inline void ReverseSign();
+
+     //! Changes the reference point of the twist.
+     //! The vector v_base_AB is expressed in the same base as the twist
+     //! The vector v_base_AB is a vector from the old point to
+     //! the new point.
+     //!
+     //! Complexity : 6M+6A
+     inline Twist RefPoint(const Vector& v_base_AB) const;
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     inline friend bool operator==(const Twist& a,const Twist& b);
+	 //! The literal inequality operator!=().
+     inline friend bool operator!=(const Twist& a,const Twist& b);
+
+// = Friends
+    friend class Rotation;
+    friend class Frame;
+
+};
+
+/**
+ * 	\brief represents both translational and rotational acceleration.
+ *
+ * 	This class represents an acceleration twist.  A acceleration twist is
+ * 	the combination of translational
+ *	acceleration and rotational acceleration applied at one point.
+*/
+/*
+class AccelerationTwist {
+public:
+    Vector trans; //!< The translational acceleration of that point
+    Vector rot; //!< The rotational acceleration of that point.
+public:
+
+    //! The default constructor initialises to Zero via the constructor of Vector.
+    AccelerationTwist():trans(),rot() {};
+
+    AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
+
+    inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
+    inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
+    //! index-based access to components, first vel(0..2), then rot(3..5)
+    inline double& operator()(int i);
+
+    //! index-based access to components, first vel(0..2), then rot(3..5)
+    //! For use with a const AccelerationTwist
+    inline double operator()(int i) const;
+
+    double operator[] ( int index ) const
+    {
+    	return this->operator() ( index );
+	}
+
+     double& operator[] ( int index )
+     {
+    	 return this->operator() ( index );
+     }
+
+     inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
+     inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
+     inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
+     inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
+     inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
+     inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
+     //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
+     //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
+     inline friend void SetToZero(AccelerationTwist& v);
+
+
+     //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero())
+     static inline AccelerationTwist Zero();
+
+     //! Reverses the sign of the AccelerationTwist
+     inline void ReverseSign();
+
+     //! Changes the reference point of the AccelerationTwist.
+     //! The vector v_base_AB is expressed in the same base as the AccelerationTwist
+     //! The vector v_base_AB is a vector from the old point to
+     //! the new point.
+     //!
+     //! Complexity : 6M+6A
+     inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
+	 //! The literal inequality operator!=().
+     inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
+
+// = Friends
+    friend class Rotation;
+    friend class Frame;
+
+};
+*/
+/**
+ * \brief represents the combination of a force and a torque.
+ *
+ * This class represents a Wrench.  A Wrench is the force and torque applied at a point
+ */
+class Wrench
+{
+public:
+    Vector force;       //!< Force that is applied at the origin of the current ref frame
+    Vector torque;      //!< Torque that is applied at the origin of the current ref frame
+public:
+
+    //! Does  initialise force and torque to zero via the underlying constructor of Vector
+    Wrench():force(),torque() {};
+    Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
+
+// = Operators
+     inline Wrench& operator-=(const Wrench& arg);
+     inline Wrench& operator+=(const Wrench& arg);
+
+     //! index-based access to components, first force(0..2), then torque(3..5)
+     inline double& operator()(int i);
+
+     //! index-based access to components, first force(0..2), then torque(3..5)
+     //! for use with a const Wrench
+     inline double operator()(int i) const;
+
+     double operator[] ( int index ) const
+       {
+	 return this->operator() ( index );
+       }
+
+     double& operator[] ( int index )
+       {
+	 return this->operator() ( index );
+       }
+
+     //! Scalar multiplication
+     inline friend Wrench operator*(const Wrench& lhs,double rhs);
+     //! Scalar multiplication
+     inline friend Wrench operator*(double lhs,const Wrench& rhs);
+     //! Scalar division
+     inline friend Wrench operator/(const Wrench& lhs,double rhs);
+
+     inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
+     inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
+
+     //! An unary - operator
+     inline friend Wrench operator-(const Wrench& arg);
+
+     //! Sets the Wrench to Zero, to have a uniform function that sets an object or
+     //! double to zero.
+     inline friend void SetToZero(Wrench& v);
+
+     //! @return a zero Wrench
+     static inline Wrench Zero();
+
+     //! Reverses the sign of the current Wrench
+     inline void ReverseSign();
+
+     //! Changes the reference point of the wrench.
+     //! The vector v_base_AB is expressed in the same base as the twist
+     //! The vector v_base_AB is a vector from the old point to
+     //! the new point.
+     //!
+     //! Complexity : 6M+6A
+     inline Wrench RefPoint(const Vector& v_base_AB) const;
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
+
+	 //! The literal equality operator==(), also identical.
+     inline friend bool operator==(const Wrench& a,const Wrench& b);
+	 //! The literal inequality operator!=().
+     inline friend bool operator!=(const Wrench& a,const Wrench& b);
+
+    friend class Rotation;
+    friend class Frame;
+
+
+};
+
+
+//! 2D version of Vector
+class Vector2
+{
+    double data[2];
+public:
+     //! Does not initialise to Zero().
+     Vector2() {data[0]=data[1] = 0.0;}
+     inline Vector2(double x,double y);
+     inline Vector2(const Vector2& arg);
+     inline Vector2(double* xyz);
+     inline Vector2(float* xyz);
+
+     inline Vector2& operator = ( const Vector2& arg);
+
+     //! Access to elements, range checked when NDEBUG is not set, from 0..1
+     inline double operator()(int index) const;
+
+     //! Access to elements, range checked when NDEBUG is not set, from 0..1
+     inline double& operator() (int index);
+
+	 //! store vector components in array
+	 inline void GetValue(double* xy) const;
+
+     inline void ReverseSign();
+     inline Vector2& operator-=(const Vector2& arg);
+     inline Vector2& operator +=(const Vector2& arg);
+
+
+     inline friend Vector2 operator*(const Vector2& lhs,double rhs);
+     inline friend Vector2 operator*(double lhs,const Vector2& rhs);
+     inline friend Vector2 operator/(const Vector2& lhs,double rhs);
+     inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
+     inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
+     inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
+     inline friend Vector2 operator-(const Vector2& arg);
+     inline friend void SetToZero(Vector2& v);
+
+     //! @return a zero 2D vector.
+     inline static Vector2 Zero();
+
+   /** Normalizes this vector and returns it norm
+	* makes v a unitvector and returns the norm of v.
+	* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
+	* if this is not good, check the return value of this method.
+	*/
+     double Normalize(double eps=epsilon);
+
+     //!  @return the norm of the vector
+     inline double Norm() const;
+
+     //! projects v in its XY plane, and sets *this to these values
+     inline void Set3DXY(const Vector& v);
+
+     //! projects v in its YZ plane, and sets *this to these values
+     inline void Set3DYZ(const Vector& v);
+
+     //! projects v in its ZX plane, and sets *this to these values
+     inline void Set3DZX(const Vector& v);
+
+     //! projects v_someframe in the XY plane of F_someframe_XY,
+     //! and sets *this to these values
+     //! expressed wrt someframe.
+     inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
+
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
+
+    friend class Rotation2;
+};
+
+
+//! A 2D Rotation class, for conventions see Rotation. For further documentation
+//! of the methods see Rotation class.
+class Rotation2
+{
+    double s,c;
+    //! c,s represent  cos(angle), sin(angle), this also represents first col. of rot matrix
+    //! from outside, this class behaves as if it would store the complete 2x2 matrix.
+public:
+    //! Default constructor does NOT initialise to Zero().
+    Rotation2() {c=1.0;s=0.0;}
+
+    explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
+
+    Rotation2(double ca,double sa):s(sa),c(ca){}
+
+     inline Rotation2& operator=(const Rotation2& arg);
+     inline Vector2 operator*(const Vector2& v) const;
+     //!    Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set
+     inline double operator() (int i,int j) const;
+
+     inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
+
+     inline void SetInverse();
+     inline Rotation2 Inverse() const;
+     inline Vector2 Inverse(const Vector2& v) const;
+
+     inline void SetIdentity();
+     inline static Rotation2 Identity();
+
+
+     //! The SetRot.. functions set the value of *this to the appropriate rotation matrix.
+     inline void SetRot(double angle);
+
+     //! The Rot... static functions give the value of the appropriate rotation matrix bac
+     inline static Rotation2 Rot(double angle);
+
+     //! Gets the angle (in radians)
+     inline double GetRot() const;
+
+     //! do not use operator == because the definition of Equal(.,.) is slightly
+     //! different.  It compares whether the 2 arguments are equal in an eps-interval
+     inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
+};
+
+//! A 2D frame class, for further documentation see the Frames class
+//! for methods with unchanged semantics.
+class Frame2
+ {
+public:
+    Vector2 p;          //!< origine of the Frame
+    Rotation2 M;        //!< Orientation of the Frame
+
+public:
+
+     inline Frame2(const Rotation2& R,const Vector2& V);
+     explicit inline Frame2(const Vector2& V);
+     explicit inline Frame2(const Rotation2& R);
+     inline Frame2(void);
+     inline Frame2(const Frame2& arg);
+     inline void Make4x4(double* d);
+
+     //!  Treats a frame as a 3x3 matrix and returns element i,j
+     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
+     inline double operator()(int i,int j);
+
+     //!  Treats a frame as a 4x4 matrix and returns element i,j
+     //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
+     inline double operator() (int i,int j) const;
+
+     inline void SetInverse();
+     inline Frame2 Inverse() const;
+     inline Vector2 Inverse(const Vector2& arg) const;
+     inline Frame2& operator = (const Frame2& arg);
+     inline Vector2 operator * (const Vector2& arg);
+     inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
+     inline void SetIdentity();
+     inline void Integrate(const Twist& t_this,double frequency);
+     inline static Frame2 Identity() {
+        Frame2 tmp;
+        tmp.SetIdentity();
+        return tmp;
+     }
+     inline friend bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
+};
+
+IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1);
+IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
+IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
+IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
+IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
+IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1);
+IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1);
+IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1);
+IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
+IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
+#ifdef KDL_INLINE
+//    #include "vector.inl"
+//   #include "wrench.inl"
+    //#include "rotation.inl"
+    //#include "frame.inl"
+    //#include "twist.inl"
+    //#include "vector2.inl"
+    //#include "rotation2.inl"
+    //#include "frame2.inl"
+#include "frames.inl"
+#endif
+
+
+
+}
+
+
+#endif
diff --git a/intern/itasc/kdl/frames.inl b/intern/itasc/kdl/frames.inl
new file mode 100644
index 00000000000..9a176070171
--- /dev/null
+++ b/intern/itasc/kdl/frames.inl
@@ -0,0 +1,1390 @@
+/***************************************************************************
+                        frames.inl -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+ 
+ ***************************************************************************
+ *   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                                *
+ *                                                                         *
+ ***************************************************************************/
+
+
+IMETHOD Vector::Vector(const Vector & arg)
+{
+    data[0] = arg.data[0];
+    data[1] = arg.data[1];
+    data[2] = arg.data[2];
+}
+
+IMETHOD Vector::Vector(double x,double y, double z)
+{
+        data[0]=x;data[1]=y;data[2]=z;
+}
+
+IMETHOD Vector::Vector(double* xyz)
+{
+        data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
+}
+
+IMETHOD Vector::Vector(float* xyz)
+{
+        data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
+}
+
+IMETHOD void Vector::GetValue(double* xyz) const
+{
+        xyz[0]=data[0];xyz[1]=data[1];xyz[2]=data[2];
+}
+
+
+IMETHOD Vector& Vector::operator =(const Vector & arg)
+{
+    data[0] = arg.data[0];
+    data[1] = arg.data[1];
+    data[2] = arg.data[2];
+    return *this;
+}
+
+IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs)
+{
+    Vector tmp;
+    tmp.data[0] = lhs.data[0]+rhs.data[0];
+    tmp.data[1] = lhs.data[1]+rhs.data[1];
+    tmp.data[2] = lhs.data[2]+rhs.data[2];
+    return tmp;
+}
+
+IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs)
+{
+    Vector tmp;
+    tmp.data[0] = lhs.data[0]-rhs.data[0];
+    tmp.data[1] = lhs.data[1]-rhs.data[1];
+    tmp.data[2] = lhs.data[2]-rhs.data[2];
+    return tmp;
+}
+
+IMETHOD double Vector::x() const { return data[0]; }
+IMETHOD double Vector::y() const { return data[1]; }
+IMETHOD double Vector::z() const { return data[2]; }
+
+IMETHOD void Vector::x( double _x ) { data[0] = _x; }
+IMETHOD void Vector::y( double _y ) { data[1] = _y; }
+IMETHOD void Vector::z( double _z ) { data[2] = _z; }
+
+Vector operator *(const Vector& lhs,double rhs) 
+{
+    Vector tmp;
+    tmp.data[0] = lhs.data[0]*rhs;
+    tmp.data[1] = lhs.data[1]*rhs;
+    tmp.data[2] = lhs.data[2]*rhs;
+    return tmp;
+}
+
+Vector operator *(double lhs,const Vector& rhs) 
+{
+    Vector tmp;
+    tmp.data[0] = lhs*rhs.data[0];
+    tmp.data[1] = lhs*rhs.data[1];
+    tmp.data[2] = lhs*rhs.data[2];
+    return tmp;
+}
+
+Vector operator /(const Vector& lhs,double rhs) 
+{
+    Vector tmp;
+    tmp.data[0] = lhs.data[0]/rhs;
+    tmp.data[1] = lhs.data[1]/rhs;
+    tmp.data[2] = lhs.data[2]/rhs;
+    return tmp;
+}
+
+Vector operator *(const Vector & lhs,const Vector& rhs)
+// Complexity : 6M+3A
+{
+    Vector tmp;
+    tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1];
+    tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2];
+    tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0];
+    return tmp;
+}
+
+Vector& Vector::operator +=(const Vector & arg)
+// Complexity : 3A
+{
+    data[0]+=arg.data[0];
+    data[1]+=arg.data[1];
+    data[2]+=arg.data[2];
+    return *this;
+}
+
+Vector& Vector::operator -=(const Vector & arg)
+// Complexity : 3A
+{
+    data[0]-=arg.data[0];
+    data[1]-=arg.data[1];
+    data[2]-=arg.data[2];
+    return *this;
+}
+
+Vector Vector::Zero()
+{
+    return Vector(0,0,0);
+}
+
+double Vector::operator()(int index) const {
+    FRAMES_CHECKI((0<=index)&&(index<=2));
+    return data[index];
+}
+
+double& Vector::operator () (int index)
+{
+    FRAMES_CHECKI((0<=index)&&(index<=2));
+    return data[index];
+}
+
+IMETHOD Vector Normalize(const Vector& a, double eps)
+{
+	double l=a.Norm();
+	return (l<eps) ? Vector(0.0,0.0,0.0) : a/l;
+}
+
+Wrench Frame::operator * (const Wrench& arg) const
+// Complexity : 24M+18A
+{
+    Wrench tmp;
+    tmp.force  = M*arg.force;
+    tmp.torque = M*arg.torque + p*tmp.force;
+    return tmp;
+}
+
+Wrench Frame::Inverse(const Wrench& arg) const
+{
+    Wrench tmp;
+    tmp.force =  M.Inverse(arg.force);
+    tmp.torque = M.Inverse(arg.torque-p*arg.force);
+    return tmp;
+}
+
+
+
+Wrench Rotation::Inverse(const Wrench& arg) const
+{
+    return Wrench(Inverse(arg.force),Inverse(arg.torque));
+}
+
+Twist Rotation::Inverse(const Twist& arg) const
+{
+    return Twist(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+Wrench Wrench::Zero()
+{
+    return Wrench(Vector::Zero(),Vector::Zero());
+}
+
+
+void Wrench::ReverseSign()
+{   
+    torque.ReverseSign();
+    force.ReverseSign();
+}
+
+Wrench Wrench::RefPoint(const Vector& v_base_AB) const
+     // Changes the reference point of the Wrench.
+     // The vector v_base_AB is expressed in the same base as the twist
+     // The vector v_base_AB is a vector from the old point to
+     // the new point.
+{
+    return Wrench(this->force,
+                  this->torque+this->force*v_base_AB
+                  );
+}
+
+
+Wrench& Wrench::operator-=(const Wrench& arg)
+{
+    torque-=arg.torque;
+    force -=arg.force;
+    return *this;
+}
+
+Wrench& Wrench::operator+=(const Wrench& arg)
+{
+    torque+=arg.torque;
+    force +=arg.force;
+    return *this;
+}
+
+double& Wrench::operator()(int i)
+{
+    // assert((0<=i)&&(i<6)); done by underlying routines
+    if (i<3) 
+        return force(i);
+    else
+        return torque(i-3);
+}
+
+double Wrench::operator()(int i) const
+{
+    // assert((0<=i)&&(i<6)); done by underlying routines
+    if (i<3) 
+        return force(i);
+    else
+        return torque(i-3);
+}
+
+
+Wrench operator*(const Wrench& lhs,double rhs)
+{
+    return Wrench(lhs.force*rhs,lhs.torque*rhs);
+}
+
+Wrench operator*(double lhs,const Wrench& rhs)
+{
+    return Wrench(lhs*rhs.force,lhs*rhs.torque);
+}
+
+Wrench operator/(const Wrench& lhs,double rhs)
+{
+    return Wrench(lhs.force/rhs,lhs.torque/rhs);
+}
+
+// addition of Wrench's
+Wrench operator+(const Wrench& lhs,const Wrench& rhs)
+{
+    return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
+}
+
+Wrench operator-(const Wrench& lhs,const Wrench& rhs)
+{
+    return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
+}
+
+// unary -
+Wrench operator-(const Wrench& arg) 
+{
+    return Wrench(-arg.force,-arg.torque);
+}
+
+Twist Frame::operator * (const Twist& arg) const
+// Complexity : 24M+18A
+{
+    Twist tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+Twist Frame::Inverse(const Twist& arg) const
+{
+    Twist tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+Twist Twist::Zero()
+{
+    return Twist(Vector::Zero(),Vector::Zero());
+}
+
+
+void Twist::ReverseSign()
+{   
+    vel.ReverseSign();
+    rot.ReverseSign();
+}
+
+Twist Twist::RefPoint(const Vector& v_base_AB) const
+     // Changes the reference point of the twist.
+     // The vector v_base_AB is expressed in the same base as the twist
+     // The vector v_base_AB is a vector from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+{
+    return Twist(this->vel+this->rot*v_base_AB,this->rot);
+}
+
+Twist& Twist::operator-=(const Twist& arg)
+{
+    vel-=arg.vel;
+    rot -=arg.rot;
+    return *this;
+}
+
+Twist& Twist::operator+=(const Twist& arg)
+{
+    vel+=arg.vel;
+    rot +=arg.rot;
+    return *this;
+}
+
+double& Twist::operator()(int i)
+{
+    // assert((0<=i)&&(i<6)); done by underlying routines
+    if (i<3) 
+        return vel(i);
+    else
+        return rot(i-3);
+}
+
+double Twist::operator()(int i) const
+{
+    // assert((0<=i)&&(i<6)); done by underlying routines
+    if (i<3) 
+        return vel(i);
+    else
+        return rot(i-3);
+}
+
+
+Twist operator*(const Twist& lhs,double rhs)
+{
+    return Twist(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+Twist operator*(double lhs,const Twist& rhs)
+{
+    return Twist(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+Twist operator/(const Twist& lhs,double rhs)
+{
+    return Twist(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+// addition of Twist's
+Twist operator+(const Twist& lhs,const Twist& rhs)
+{
+    return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
+}
+
+Twist operator-(const Twist& lhs,const Twist& rhs)
+{
+    return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
+}
+
+// unary -
+Twist operator-(const Twist& arg) 
+{
+    return Twist(-arg.vel,-arg.rot);
+}
+
+Frame::Frame(const Rotation & R)
+{
+    M=R;
+    p=Vector::Zero();
+}
+
+Frame::Frame(const Vector & V)
+{
+    M = Rotation::Identity();
+    p = V;
+}
+
+Frame::Frame(const Rotation & R, const Vector & V)
+{
+    M = R;
+    p = V;
+}
+
+ Frame operator *(const Frame& lhs,const Frame& rhs)
+// Complexity : 36M+36A
+{
+    return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+
+Vector Frame::operator *(const Vector & arg) const
+{
+    return M*arg+p;
+}
+
+Vector Frame::Inverse(const Vector& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+Frame Frame::Inverse() const
+{
+    return Frame(M.Inverse(),-M.Inverse(p));
+}
+
+
+Frame& Frame::operator =(const Frame & arg)
+{ 
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+
+Frame::Frame(const Frame & arg) :
+    p(arg.p),M(arg.M)
+{}
+
+
+void Vector::ReverseSign()
+{
+    data[0] = -data[0];
+    data[1] = -data[1];
+    data[2] = -data[2];
+}
+
+
+
+Vector operator-(const Vector & arg)
+{
+    Vector tmp;
+    tmp.data[0]=-arg.data[0];
+    tmp.data[1]=-arg.data[1];
+    tmp.data[2]=-arg.data[2];
+    return tmp;
+}
+
+void Vector::Set2DXY(const Vector2& v)
+// a 3D vector where the 2D vector v is put in the XY plane
+{
+    data[0]=v(0);
+    data[1]=v(1);
+    data[2]=0;
+
+}
+void Vector::Set2DYZ(const Vector2& v)
+// a 3D vector where the 2D vector v is put in the YZ plane
+{
+    data[1]=v(0);
+    data[2]=v(1);
+    data[0]=0;
+
+}
+
+void Vector::Set2DZX(const Vector2& v)
+// a 3D vector where the 2D vector v is put in the ZX plane
+{
+    data[2]=v(0);
+    data[0]=v(1);
+    data[1]=0;
+
+}
+
+
+
+
+
+double& Rotation::operator()(int i,int j) {
+    FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
+    return data[i*3+j];
+}
+
+double Rotation::operator()(int i,int j) const {
+    FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
+    return data[i*3+j];
+}
+
+Rotation::Rotation( double Xx,double Yx,double Zx,
+                            double Xy,double Yy,double Zy,
+                            double Xz,double Yz,double Zz)
+{
+    data[0] = Xx;data[1]=Yx;data[2]=Zx;
+    data[3] = Xy;data[4]=Yy;data[5]=Zy;
+    data[6] = Xz;data[7]=Yz;data[8]=Zz;
+}
+
+
+Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) 
+{
+    data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2];
+    data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2];
+    data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
+}
+
+Rotation& Rotation::operator=(const Rotation& arg) {
+    int count=9;
+    while (count--) data[count] = arg.data[count];
+    return *this;
+}
+
+Vector Rotation::operator*(const Vector& v) const {
+// Complexity : 9M+6A
+    return Vector(
+     data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2],
+     data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2],
+     data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2] 
+    );
+}
+
+Twist Rotation::operator * (const Twist& arg) const
+     // Transformation of the base to which the twist is expressed.
+     // look at Frame*Twist for a transformation that also transforms
+     // the velocity reference point.
+     // Complexity : 18M+12A
+{
+    return Twist((*this)*arg.vel,(*this)*arg.rot);
+}
+
+Wrench Rotation::operator * (const Wrench& arg) const
+     // Transformation of the base to which the wrench is expressed.
+     // look at Frame*Twist for a transformation that also transforms
+     // the force reference point.
+{
+    return Wrench((*this)*arg.force,(*this)*arg.torque);
+}
+
+Rotation Rotation::Identity() {
+        return Rotation(1,0,0,0,1,0,0,0,1);
+}
+// *this = *this * ROT(X,angle)
+void Rotation::DoRotX(double angle)
+{
+    double cs = cos(angle);
+    double sn = sin(angle);
+    double x1,x2,x3;
+    x1  = cs* (*this)(0,1) + sn* (*this)(0,2);
+    x2  = cs* (*this)(1,1) + sn* (*this)(1,2);
+    x3  = cs* (*this)(2,1) + sn* (*this)(2,2);
+    (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2);
+    (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2);
+    (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2);
+    (*this)(0,1) = x1;
+    (*this)(1,1) = x2;
+    (*this)(2,1) = x3;
+}
+
+void Rotation::DoRotY(double angle)
+{
+    double cs = cos(angle);
+    double sn = sin(angle);
+    double x1,x2,x3;
+    x1  = cs* (*this)(0,0) - sn* (*this)(0,2);
+    x2  = cs* (*this)(1,0) - sn* (*this)(1,2);
+    x3  = cs* (*this)(2,0) - sn* (*this)(2,2);
+    (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2);
+    (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2);
+    (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2);
+    (*this)(0,0) = x1;
+    (*this)(1,0) = x2;
+    (*this)(2,0) = x3;
+}
+
+void Rotation::DoRotZ(double angle)
+{
+    double cs = cos(angle);
+    double sn = sin(angle);
+    double x1,x2,x3;
+    x1  = cs* (*this)(0,0) + sn* (*this)(0,1);
+    x2  = cs* (*this)(1,0) + sn* (*this)(1,1);
+    x3  = cs* (*this)(2,0) + sn* (*this)(2,1);
+    (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1);
+    (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1);
+    (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1);
+    (*this)(0,0) = x1;
+    (*this)(1,0) = x2;
+    (*this)(2,0) = x3;
+}
+
+
+Rotation Rotation::RotX(double angle) {
+    double cs=cos(angle);
+    double sn=sin(angle);
+    return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
+}
+Rotation Rotation::RotY(double angle) {
+    double cs=cos(angle);
+    double sn=sin(angle);
+    return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
+}
+Rotation Rotation::RotZ(double angle) {
+    double cs=cos(angle);
+    double sn=sin(angle);
+    return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
+}
+
+
+
+
+void Frame::Integrate(const Twist& t_this,double samplefrequency)
+{
+    double n = t_this.rot.Norm()/samplefrequency;
+    if (n<epsilon) {
+        p += M*(t_this.vel/samplefrequency);
+    } else {
+        (*this) = (*this) * 
+            Frame ( Rotation::Rot( t_this.rot, n ),
+                    t_this.vel/samplefrequency
+                );
+    }
+}
+
+Rotation Rotation::Inverse() const
+{
+    Rotation tmp(*this);
+    tmp.SetInverse();
+    return tmp;
+}
+
+Vector Rotation::Inverse(const Vector& v) const {
+    return Vector(
+     data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2],
+     data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2],
+     data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2] 
+    );
+}
+
+void Rotation::setValue(float* oglmat)
+{
+    data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++;
+    data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++;
+    data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat;
+	Ortho();
+}
+
+void Rotation::getValue(float* oglmat) const
+{
+	*oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f;
+	*oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f;
+	*oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f;
+	*oglmat++ = 0.f;            *oglmat++ = 0.f;            *oglmat++ = 0.f;            *oglmat   = 1.f;
+}
+
+void Rotation::SetInverse()
+{
+    double tmp;
+    tmp = data[1];data[1]=data[3];data[3]=tmp;
+    tmp = data[2];data[2]=data[6];data[6]=tmp;
+    tmp = data[5];data[5]=data[7];data[7]=tmp;
+}
+
+
+
+
+
+
+
+double Frame::operator()(int i,int j) {
+    FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
+    if (i==3) {
+        if (j==3)
+            return 1.0;
+        else
+            return 0.0;
+    } else {
+        if (j==3) 
+            return p(i);
+        else
+            return M(i,j);
+
+    }
+}
+
+double Frame::operator()(int i,int j) const {
+    FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
+        if (i==3) {
+        if (j==3)
+            return 1;
+        else
+            return 0;
+    } else {
+        if (j==3) 
+            return p(i);
+        else
+            return M(i,j);
+
+    }
+}
+
+
+Frame Frame::Identity() {
+    return Frame(Rotation::Identity(),Vector::Zero());
+}
+
+
+void Frame::setValue(float* oglmat)
+{
+	M.setValue(oglmat);
+	p.data[0] = oglmat[12];
+	p.data[1] = oglmat[13];
+	p.data[2] = oglmat[14];
+}
+
+void Frame::getValue(float* oglmat) const
+{
+	M.getValue(oglmat);
+	oglmat[12] = (float)p.data[0];
+	oglmat[13] = (float)p.data[1];
+	oglmat[14] = (float)p.data[2];
+}
+
+void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY)
+// a 3D vector where the 2D vector v is put in the XY plane of the frame
+// F_someframe_XY.
+{
+Vector tmp_XY;
+tmp_XY.Set2DXY(v_XY);
+tmp_XY = F_someframe_XY*(tmp_XY);
+}
+
+
+
+
+
+
+
+
+
+//============ 2 dimensional version of the frames objects =============
+IMETHOD Vector2::Vector2(const Vector2 & arg)
+{
+    data[0] = arg.data[0];
+    data[1] = arg.data[1];
+}
+
+IMETHOD Vector2::Vector2(double x,double y)
+{
+        data[0]=x;data[1]=y;
+}
+
+IMETHOD Vector2::Vector2(double* xy)
+{
+        data[0]=xy[0];data[1]=xy[1];
+}
+
+IMETHOD Vector2::Vector2(float* xy)
+{
+        data[0]=xy[0];data[1]=xy[1];
+}
+
+IMETHOD Vector2& Vector2::operator =(const Vector2 & arg)
+{
+    data[0] = arg.data[0];
+    data[1] = arg.data[1];
+    return *this;
+}
+
+IMETHOD void Vector2::GetValue(double* xy) const
+{
+        xy[0]=data[0];xy[1]=data[1];
+}
+
+IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
+{
+    return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
+}
+
+IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs)
+{
+    return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
+}
+
+IMETHOD Vector2 operator *(const Vector2& lhs,double rhs) 
+{
+    return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
+}
+
+IMETHOD Vector2 operator *(double lhs,const Vector2& rhs) 
+{
+    return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
+}
+
+IMETHOD Vector2 operator /(const Vector2& lhs,double rhs) 
+{
+    return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
+}
+
+IMETHOD Vector2& Vector2::operator +=(const Vector2 & arg)
+{
+    data[0]+=arg.data[0];
+    data[1]+=arg.data[1];
+    return *this;
+}
+
+IMETHOD Vector2& Vector2::operator -=(const Vector2 & arg)
+{
+    data[0]-=arg.data[0];
+    data[1]-=arg.data[1];
+    return *this;
+}
+
+IMETHOD Vector2 Vector2::Zero() {
+    return Vector2(0,0);
+}
+
+IMETHOD double Vector2::operator()(int index) const {
+    FRAMES_CHECKI((0<=index)&&(index<=1));
+    return data[index];
+}
+
+IMETHOD double& Vector2::operator () (int index)
+{
+    FRAMES_CHECKI((0<=index)&&(index<=1));
+    return data[index];
+}
+IMETHOD void Vector2::ReverseSign()
+{
+    data[0] = -data[0];
+    data[1] = -data[1];
+}
+
+
+IMETHOD Vector2 operator-(const Vector2 & arg)
+{
+    return Vector2(-arg.data[0],-arg.data[1]);
+}
+
+
+IMETHOD void Vector2::Set3DXY(const Vector& v)
+// projects v in its XY plane, and sets *this to these values
+{
+    data[0]=v(0);
+    data[1]=v(1);
+}
+IMETHOD void Vector2::Set3DYZ(const Vector& v)
+// projects v in its XY plane, and sets *this to these values
+{
+    data[0]=v(1);
+    data[1]=v(2);
+}
+IMETHOD void Vector2::Set3DZX(const Vector& v)
+// projects v in its XY plane, and and sets *this to these values
+{
+    data[0]=v(2);
+    data[1]=v(0);
+}
+
+IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe) 
+// projects v in the XY plane of F_someframe_XY, and sets *this to these values
+// expressed wrt someframe.
+{
+    Vector tmp = F_someframe_XY.Inverse(v_someframe);
+    data[0]=tmp(0);
+    data[1]=tmp(1);
+}
+
+
+
+IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) {
+    c=arg.c;s=arg.s;
+    return *this;
+}
+
+IMETHOD Vector2 Rotation2::operator*(const Vector2& v) const {
+    return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c);
+}
+
+IMETHOD double Rotation2::operator()(int i,int j) const {
+    FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1));
+    if (i==j) return c;
+    if (i==0) 
+        return s;
+    else
+        return -s;
+}
+
+
+IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) {
+    return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
+}
+
+IMETHOD void Rotation2::SetInverse() {
+    s=-s;
+}
+
+IMETHOD Rotation2 Rotation2::Inverse() const {
+    return Rotation2(c,-s);
+}
+
+IMETHOD Vector2 Rotation2::Inverse(const Vector2& v) const {
+    return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c);
+}
+
+IMETHOD Rotation2 Rotation2::Identity() {
+    return Rotation2(1,0);
+}
+     
+IMETHOD void Rotation2::SetIdentity()
+{
+	c = 1;
+	s = 0;
+}
+
+IMETHOD void Rotation2::SetRot(double angle) {
+    c=cos(angle);s=sin(angle);
+}
+
+IMETHOD Rotation2 Rotation2::Rot(double angle) {
+    return Rotation2(cos(angle),sin(angle));
+}
+
+IMETHOD double Rotation2::GetRot() const {
+    return atan2(s,c);
+}
+
+
+IMETHOD Frame2::Frame2() {
+}
+
+IMETHOD Frame2::Frame2(const Rotation2 & R)
+{
+    M=R;
+    p=Vector2::Zero();
+}
+
+IMETHOD Frame2::Frame2(const Vector2 & V)
+{
+    M = Rotation2::Identity();
+    p = V;
+}
+
+IMETHOD Frame2::Frame2(const Rotation2 & R, const Vector2 & V)
+{
+    M = R;
+    p = V;
+}
+
+IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs)
+{
+    return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+
+IMETHOD Vector2 Frame2::operator *(const Vector2 & arg)
+{
+    return M*arg+p;
+}
+
+IMETHOD Vector2 Frame2::Inverse(const Vector2& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+IMETHOD void Frame2::SetIdentity()
+{
+	M.SetIdentity();
+	p = Vector2::Zero();
+}
+
+IMETHOD void Frame2::SetInverse()
+{
+    M.SetInverse();
+    p = M*p;
+    p.ReverseSign();
+}
+
+
+IMETHOD Frame2 Frame2::Inverse() const
+{
+    Frame2 tmp(*this);
+    tmp.SetInverse();
+    return tmp;
+}
+
+IMETHOD Frame2& Frame2::operator =(const Frame2 & arg)
+{ 
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+
+IMETHOD Frame2::Frame2(const Frame2 & arg) :
+    p(arg.p), M(arg.M)
+{}
+
+
+IMETHOD double Frame2::operator()(int i,int j) {
+    FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
+    if (i==2) {
+        if (j==2)
+            return 1;
+        else
+            return 0;
+    } else {
+        if (j==2) 
+            return p(i);
+        else
+            return M(i,j);
+
+    }
+}
+
+IMETHOD double Frame2::operator()(int i,int j) const {
+    FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
+    if (i==2) {
+        if (j==2)
+            return 1;
+        else
+            return 0;
+    } else {
+        if (j==2) 
+            return p(i);
+        else
+            return M(i,j);
+
+    }
+}
+
+// Scalar products.
+
+IMETHOD double dot(const Vector& lhs,const Vector& rhs) {
+    return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
+}
+
+IMETHOD double dot(const Twist& lhs,const Wrench& rhs) {
+    return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
+}
+
+IMETHOD double dot(const Wrench& rhs,const Twist& lhs) {
+    return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
+}
+
+
+
+
+
+// Equality operators
+
+
+
+IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) {
+        return (Equal(a.data[0],b.data[0],eps)&&
+                Equal(a.data[1],b.data[1],eps)&&
+                Equal(a.data[2],b.data[2],eps)   );
+     }
+     
+
+IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) {
+        return (Equal(a.p,b.p,eps)&&
+                Equal(a.M,b.M,eps)   );
+}
+
+IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) {
+        return (Equal(a.force,b.force,eps)&&
+                Equal(a.torque,b.torque,eps)  );
+}
+
+IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+
+IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) {
+        return (Equal(a.data[0],b.data[0],eps)&&
+                Equal(a.data[1],b.data[1],eps)   );
+     }
+     
+IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) {
+    return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) );
+}
+
+IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) {
+        return (Equal(a.p,b.p,eps)&&
+                Equal(a.M,b.M,eps)   );
+}
+
+IMETHOD void SetToZero(Vector& v) {
+    v=Vector::Zero();
+}
+IMETHOD void SetToZero(Twist& v) {
+    SetToZero(v.rot);
+    SetToZero(v.vel);
+}
+IMETHOD void SetToZero(Wrench& v) {
+    SetToZero(v.force);
+    SetToZero(v.torque);
+}
+
+IMETHOD void SetToZero(Vector2& v) {
+    v = Vector2::Zero();
+}
+
+
+////////////////////////////////////////////////////////////////
+// The following defines the operations
+//   diff
+//   addDelta
+//   random
+//   posrandom
+// on all the types defined in this library.
+// (mostly for uniform integration, differentiation and testing).
+// Defined as functions because double is not a class and a method
+// would brake uniformity when defined for a double.
+////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+/**
+ * axis_a_b is a rotation vector, its norm is a rotation angle
+ * axis_a_b rotates the a frame towards the b frame.
+ * This routine returns the rotation matrix R_a_b
+ */
+IMETHOD Rotation Rot(const Vector& axis_a_b) {
+    // The formula is 
+    // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
+    // can be found by multiplying it with an arbitrary vector p
+    // and noting that this vector is rotated.
+	Vector rotvec = axis_a_b;
+	double angle = rotvec.Normalize(1E-10);
+    double ct = ::cos(angle);
+    double st = ::sin(angle);
+    double vt = 1-ct;
+    return Rotation(
+        ct            +  vt*rotvec(0)*rotvec(0), 
+        -rotvec(2)*st +  vt*rotvec(0)*rotvec(1), 
+        rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
+        rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
+        ct            +  vt*rotvec(1)*rotvec(1),
+        -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
+        -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
+        rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
+        ct            +  vt*rotvec(2)*rotvec(2)
+        );
+    }
+
+IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
+	return (b-a)/dt;
+}
+
+/**
+ * \brief diff operator for displacement rotational velocity.
+ *
+ * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
+ * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
+ * have to use diff_displ().
+ *
+ * \TODO represent a displacement twist and displacement rotational velocity with another
+ *       class, instead of Vector and Twist.
+ * \warning do not confuse displacement rotational velocities and velocities
+ * \warning do not confuse displacement twist and twist.
+ *
+IMETHOD Vector diff_displ(const Vector& a,const Vector& b,double dt) {
+	return diff(Rot(a),Rot(b),dt);
+}*/
+
+/**
+ * \brief diff operator for displacement twist.
+ *
+ * The Twist arguments here represent a displacement twist.  i.e. a rotation
+ * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
+ * have to use diff_displ().
+ *
+ * \warning do not confuse displacement rotational velocities and velocities
+ * \warning do not confuse displacement twist and twist.
+ *
+
+IMETHOD Twist diff_displ(const Twist& a,const Twist& b,double dt) {
+	return Twist(diff(a.vel,b.vel,dt),diff(Rot(a.rot),Rot(b.rot),dt));
+}
+*/
+
+IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
+	Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
+	return R_a_b1 * R_b1_b2.GetRot() / dt;
+}
+IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
+	return Twist(
+			diff(F_a_b1.p,F_a_b2.p,dt),
+			diff(F_a_b1.M,F_a_b2.M,dt)
+			);
+}
+IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) {
+	return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt));
+}
+
+IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) {
+	return Wrench(
+			diff(a.force,b.force,dt),
+			diff(a.torque,b.torque,dt)
+			);
+}
+
+
+IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
+	return a+da*dt;
+}
+
+IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
+	return a*Rot(a.Inverse(da)*dt);
+}
+IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) {
+	return Frame(
+			addDelta(a.M,da.rot,dt),
+			addDelta(a.p,da.vel,dt)
+		   );
+}
+IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) {
+	return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt));
+}
+IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) {
+	return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt));
+}
+
+
+/**
+ * \brief addDelta operator for displacement rotational velocity.
+ *
+ * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
+ * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
+ * have to use diff_displ().
+ *
+ * \param a  : displacement rotational velocity
+ * \param da : rotational velocity 
+ * \return   displacement rotational velocity
+ *
+ * \warning do not confuse displacement rotational velocities and velocities
+ * \warning do not confuse displacement twist and twist.
+ *
+IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) {
+    return getRot(addDelta(Rot(a),da,dt)); 
+}*/
+
+/**
+ * \brief addDelta operator for displacement twist.
+ *
+ * The Vector arguments here represent a displacement rotational velocity.  i.e. a rotation
+ * around a fixed axis for a certain angle.  For this representation you cannot use diff() but
+ * have to use diff_displ().
+ *
+ * \param a  : displacement twist 
+ * \param da : twist 
+ * \return   displacement twist 
+ *
+ * \warning do not confuse displacement rotational velocities and velocities
+ * \warning do not confuse displacement twist and twist.
+ *
+IMETHOD Twist addDelta_displ(const Twist& a,const Twist&da,double dt) {
+    return Twist(addDelta(a.vel,da.vel,dt),addDelta_displ(a.rot,da.rot,dt)); 
+}*/
+
+
+IMETHOD void random(Vector& a) {
+	random(a[0]);
+	random(a[1]);
+	random(a[2]);
+}
+IMETHOD void random(Twist& a) {
+	random(a.rot);
+	random(a.vel);
+}
+IMETHOD void random(Wrench& a) {
+   	random(a.torque);
+	random(a.force);
+}
+
+IMETHOD void random(Rotation& R) {
+	double alfa;
+	double beta;
+	double gamma;
+	random(alfa);
+	random(beta);
+	random(gamma);
+	R = Rotation::EulerZYX(alfa,beta,gamma);
+}
+
+IMETHOD void random(Frame& F) {
+	random(F.M);
+	random(F.p);
+}
+
+IMETHOD void posrandom(Vector& a) {
+	posrandom(a[0]);
+	posrandom(a[1]);
+	posrandom(a[2]);
+}
+IMETHOD void posrandom(Twist& a) {
+	posrandom(a.rot);
+	posrandom(a.vel);
+}
+IMETHOD void posrandom(Wrench& a) {
+   	posrandom(a.torque);
+	posrandom(a.force);
+}
+
+IMETHOD void posrandom(Rotation& R) {
+	double alfa;
+	double beta;
+	double gamma;
+	posrandom(alfa);
+	posrandom(beta);
+	posrandom(gamma);
+	R = Rotation::EulerZYX(alfa,beta,gamma);
+}
+
+IMETHOD void posrandom(Frame& F) {
+	random(F.M);
+	random(F.p);
+}
+
+
+
+
+IMETHOD bool operator==(const Frame& a,const Frame& b ) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+        return (a.p == b.p &&
+                a.M == b.M );
+#endif
+}
+
+IMETHOD bool operator!=(const Frame& a,const Frame& b) {
+    return !operator==(a,b);
+}
+
+IMETHOD bool operator==(const Vector& a,const Vector& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+        return (a.data[0]==b.data[0]&&
+                a.data[1]==b.data[1]&&
+                a.data[2]==b.data[2] );
+#endif
+     }
+
+IMETHOD bool operator!=(const Vector& a,const Vector& b) {
+    return !operator==(a,b);
+}
+
+IMETHOD bool operator==(const Twist& a,const Twist& b) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+        return (a.rot==b.rot &&
+                a.vel==b.vel  );
+#endif
+}
+
+IMETHOD bool operator!=(const Twist& a,const Twist& b) {
+    return !operator==(a,b);
+}
+
+IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
+#ifdef KDL_USE_EQUAL
+    return Equal(a,b);
+#else
+    return (a.force==b.force &&
+            a.torque==b.torque );
+#endif
+}
+
+IMETHOD bool operator!=(const Wrench& a,const Wrench& b) {
+    return !operator==(a,b);
+}
+IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
+    return !operator==(a,b);
+}
+
diff --git a/intern/itasc/kdl/frames_io.cpp b/intern/itasc/kdl/frames_io.cpp
new file mode 100644
index 00000000000..0af50bb0e07
--- /dev/null
+++ b/intern/itasc/kdl/frames_io.cpp
@@ -0,0 +1,310 @@
+
+/***************************************************************************
+                        frames_io.h -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ ***************************************************************************
+ *   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                                *
+ *                                                                         *
+ ***************************************************************************/
+
+#include "utilities/error.h"
+#include "utilities/error_stack.h"
+#include "frames.hpp"
+#include "frames_io.hpp"
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <string.h>
+#include <iostream>
+
+namespace KDL {
+
+
+std::ostream& operator << (std::ostream& os,const Vector& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Twist& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
+       << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Wrench& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
+       << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
+       << "]";
+    return os;
+}
+
+
+std::ostream& operator << (std::ostream& os,const Rotation& R) {
+#ifdef KDL_ROTATION_PROPERTIES_RPY
+    double r,p,y;
+    R.GetRPY(r,p,y);
+    os << "[RPY]"<<endl;
+    os << "[";
+    os << std::setw(KDL_FRAME_WIDTH) << r << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << p << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << y << "]";
+#else
+# ifdef KDL_ROTATION_PROPERTIES_EULER
+    double z,y,x;
+    R.GetEulerZYX(z,y,x);
+    os << "[EULERZYX]"<<endl;
+    os << "[";
+    os << std::setw(KDL_FRAME_WIDTH) << z << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << y << ",";
+    os << std::setw(KDL_FRAME_WIDTH) << x << "]";
+# else
+    os << "[";
+    for (int i=0;i<=2;i++) {
+        os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
+                       std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
+                       std::setw(KDL_FRAME_WIDTH) << R(i,2);
+        if (i<2)
+            os << ";"<< std::endl << " ";
+        else
+            os << "]";
+    }
+# endif
+#endif
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os, const Frame& T)
+{
+    os << "[" << T.M << std::endl<< T.p << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os,const Vector2& v) {
+    os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
+       << "]";
+    return os;
+}
+
+// Rotation2 gives back an angle in degrees with the << and >> operators.
+std::ostream& operator << (std::ostream& os,const Rotation2& R) {
+    os << "[" << R.GetRot()*rad2deg << "]";
+    return os;
+}
+
+std::ostream& operator << (std::ostream& os, const Frame2& T)
+{
+    os << T.M << T.p;
+    return os;
+}
+
+std::istream& operator >> (std::istream& is,Vector& v)
+{   IOTrace("Stream input Vector (vector or ZERO)");
+    char storage[10];
+    EatWord(is,"[]",storage,10);
+    if (strlen(storage)==0) {
+        Eat(is,'[');
+        is >> v(0);
+        Eat(is,',');
+        is >> v(1);
+        Eat(is,',');
+        is >> v(2);
+        EatEnd(is,']');
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"ZERO")==0) {
+        v = Vector::Zero();
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Vector_Unexpected_id();
+}
+
+std::istream& operator >> (std::istream& is,Twist& v)
+{   IOTrace("Stream input Twist");
+    Eat(is,'[');
+    is >> v.vel(0);
+    Eat(is,',');
+    is >> v.vel(1);
+    Eat(is,',');
+    is >> v.vel(2);
+    Eat(is,',');
+    is >> v.rot(0);
+    Eat(is,',');
+    is >> v.rot(1);
+    Eat(is,',');
+    is >> v.rot(2);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Wrench& v)
+{   IOTrace("Stream input Wrench");
+    Eat(is,'[');
+    is >> v.force(0);
+    Eat(is,',');
+    is >> v.force(1);
+    Eat(is,',');
+    is >> v.force(2);
+    Eat(is,',');
+    is >> v.torque(0);
+    Eat(is,',');
+    is >> v.torque(1);
+    Eat(is,',');
+    is >> v.torque(2);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Rotation& r)
+{   IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)");
+    char storage[10];
+    EatWord(is,"[]",storage,10);
+    if (strlen(storage)==0) {
+        Eat(is,'[');
+        for (int i=0;i<3;i++) {
+            is >> r(i,0);
+            Eat(is,',') ;
+            is >> r(i,1);
+            Eat(is,',');
+            is >> r(i,2);
+            if (i<2)
+                Eat(is,';');
+            else
+                EatEnd(is,']');
+        }
+        IOTracePop();
+        return is;
+    }
+    Vector v;
+    if (strcmp(storage,"EULERZYX")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::EulerZYX(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"EULERZYZ")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::EulerZYZ(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"RPY")==0) {
+        is >> v;
+        v=v*deg2rad;
+        r = Rotation::RPY(v(0),v(1),v(2));
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"ROT")==0) {
+        is >> v;
+        double angle;
+        Eat(is,'[');
+        is >> angle;
+        EatEnd(is,']');
+        r = Rotation::Rot(v,angle*deg2rad);
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"IDENTITY")==0) {
+        r = Rotation::Identity();
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Rotation_Unexpected_id();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Frame& T)
+{   IOTrace("Stream input Frame (Rotation,Vector) or DH[...]");
+    char storage[10];
+    EatWord(is,"[",storage,10);
+    if (strlen(storage)==0) {
+        Eat(is,'[');
+        is >> T.M;
+        is >> T.p;
+        EatEnd(is,']');
+        IOTracePop();
+        return is;
+    }
+    if (strcmp(storage,"DH")==0) {
+        double a,alpha,d,theta;
+        Eat(is,'[');
+        is >> a;
+        Eat(is,',');
+        is >> alpha;
+        Eat(is,',');
+        is >> d;
+        Eat(is,',');
+        is >> theta;
+        EatEnd(is,']');
+        T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad);
+        IOTracePop();
+        return is;
+    }
+    throw Error_Frame_Frame_Unexpected_id();
+    return is;
+}
+
+std::istream& operator >> (std::istream& is,Vector2& v)
+{   IOTrace("Stream input Vector2");
+    Eat(is,'[');
+    is >> v(0);
+    Eat(is,',');
+    is >> v(1);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+std::istream& operator >> (std::istream& is,Rotation2& r)
+{   IOTrace("Stream input Rotation2");
+    Eat(is,'[');
+    double val;
+    is >> val;
+    r.Rot(val*deg2rad);
+    EatEnd(is,']');
+    IOTracePop();
+    return is;
+}
+std::istream& operator >> (std::istream& is,Frame2& T)
+{   IOTrace("Stream input Frame2");
+    is >> T.M;
+    is >> T.p;
+    IOTracePop();
+    return is;
+}
+
+} // namespace Frame
diff --git a/intern/itasc/kdl/frames_io.hpp b/intern/itasc/kdl/frames_io.hpp
new file mode 100644
index 00000000000..a358d27383f
--- /dev/null
+++ b/intern/itasc/kdl/frames_io.hpp
@@ -0,0 +1,114 @@
+/***************************************************************************
+                        frames_io.h -  description
+                       -------------------------
+    begin                : June 2006
+    copyright            : (C) 2006 Erwin Aertbelien
+    email                : firstname.lastname@mech.kuleuven.ac.be
+
+ History (only major changes)( AUTHOR-Description ) :
+
+ Ruben Smits - Added output for jacobian and jntarray 06/2007
+
+ ***************************************************************************
+ *   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 routines for I/O of Frame and related objects.
+// \verbatim
+//      Spaces, tabs and newlines do not have any importance.
+//      Comments are allowed C-style,C++-style, make/perl/csh -style
+//      Description of the I/O :
+//        Vector  : OUTPUT : e.g. [10,20,30]
+//                  INPUT  :
+//                         1) [10,20,30]
+//                         2) Zero
+//        Twist   : e.g. [1,2,3,4,5,6]
+//           where [1,2,3] is velocity vector
+//           where [4,5,6] is rotational velocity vector
+//        Wrench  : e.g. [1,2,3,4,5,6]
+//           where [1,2,3] represents a force vector
+//           where [4,5,6] represents a torque vector
+//        Rotation : output :
+//                 [1,2,3;
+//                  4,5,6;
+//                  7,8,9] cfr definition of Rotation object.
+//                  input :
+//                    1) like the output
+//                    2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. :
+//                        Eulerzyx[10,20,30]
+//                   (ANGLES are always expressed in DEGREES for I/O)
+//                   (ANGELS are always expressed in RADIANS for internal representation)
+//                    3) Rot [1,2,3] [20]  Rotates around axis [1,2,3] with an angle
+//                   of 20 degrees.
+//                    4) Identity          returns identity rotation matrix.
+//       Frames   : output : [ Rotationmatrix positionvector ]
+//                   e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ]
+//                  Input  :
+//                     1) [ Rotationmatrix positionvector ]
+//                     2) DH [ 10,10,50,30]  Denavit-Hartenberg representation
+//                     ( is in fact not the representation of a Frame, but more
+//                       limited, cfr. documentation of Frame object.)
+//  \endverbatim
+//
+// \warning
+//   You can use iostream.h or iostream header files for file I/O,
+//   if one declares the define WANT_STD_IOSTREAM then the standard C++
+//   iostreams headers are included instead of the compiler-dependent version
+//
+ *
+ ****************************************************************************/
+#ifndef FRAMES_IO_H
+#define FRAMES_IO_H
+
+#include "utilities/utility_io.h"
+#include "frames.hpp"
+#include "jntarray.hpp"
+#include "jacobian.hpp"
+
+namespace KDL {
+
+    //! width to be used when printing variables out with frames_io.h
+    //! global variable, can be changed.
+
+
+    // I/O to C++ stream.
+    std::ostream& operator << (std::ostream& os,const Vector& v);
+    std::ostream& operator << (std::ostream& os,const Rotation& R);
+    std::ostream& operator << (std::ostream& os,const Frame& T);
+    std::ostream& operator << (std::ostream& os,const Twist& T);
+    std::ostream& operator << (std::ostream& os,const Wrench& T);
+    std::ostream& operator << (std::ostream& os,const Vector2& v);
+    std::ostream& operator << (std::ostream& os,const Rotation2& R);
+    std::ostream& operator << (std::ostream& os,const Frame2& T);
+
+
+
+    std::istream& operator >> (std::istream& is,Vector& v);
+    std::istream& operator >> (std::istream& is,Rotation& R);
+    std::istream& operator >> (std::istream& is,Frame& T);
+    std::istream& operator >> (std::istream& os,Twist& T);
+    std::istream& operator >> (std::istream& os,Wrench& T);
+    std::istream& operator >> (std::istream& is,Vector2& v);
+    std::istream& operator >> (std::istream& is,Rotation2& R);
+    std::istream& operator >> (std::istream& is,Frame2& T);
+
+
+} // namespace Frame
+
+#endif
diff --git a/intern/itasc/kdl/framevel.cpp b/intern/itasc/kdl/framevel.cpp
new file mode 100644
index 00000000000..c0a94d64947
--- /dev/null
+++ b/intern/itasc/kdl/framevel.cpp
@@ -0,0 +1,27 @@
+/*****************************************************************************
+ *  \author
+ *  	Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version
+ *		ORO_Geometry V0.2
+ *
+ *	\par History
+ *		- $log$
+ *
+ *	\par Release
+ *		$Id: framevel.cpp 19905 2009-04-23 13:29:54Z ben2610 $
+ *		$Name:  $
+ ****************************************************************************/
+
+
+#include "framevel.hpp"
+
+namespace KDL {
+
+#ifndef KDL_INLINE
+    #include "framevel.inl"
+#endif
+
+
+
+}
diff --git a/intern/itasc/kdl/framevel.hpp b/intern/itasc/kdl/framevel.hpp
new file mode 100644
index 00000000000..21a7844f522
--- /dev/null
+++ b/intern/itasc/kdl/framevel.hpp
@@ -0,0 +1,382 @@
+/*****************************************************************************
+ * \file
+ *      This file contains the definition of classes for a
+ *      Rall Algebra of (subset of) the classes defined in frames,
+ *      i.e. classes that contain a pair (value,derivative) and define operations on that pair
+ *      this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
+ *      Defines VectorVel, RotationVel, FrameVel.  Look at Frames.h for details on how to work
+ *      with Frame objects.
+ *  \author
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $
+ *      $Name:  $
+ ****************************************************************************/
+
+#ifndef KDL_FRAMEVEL_H
+#define KDL_FRAMEVEL_H
+
+#include "utilities/utility.h"
+#include "utilities/rall1d.h"
+#include "utilities/traits.h"
+
+#include "frames.hpp"
+
+
+
+namespace KDL {
+
+typedef Rall1d<double> doubleVel;
+
+IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
+	return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
+}
+
+IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
+	return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
+}
+
+IMETHOD void random(doubleVel& F) {
+	random(F.t);
+	random(F.grad);
+}
+IMETHOD void posrandom(doubleVel& F) {
+	posrandom(F.t);
+	posrandom(F.grad);
+}
+
+}
+
+template <>
+struct Traits<KDL::doubleVel> {
+	typedef double valueType;
+	typedef KDL::doubleVel derivType;
+};
+
+namespace KDL {
+
+class TwistVel;
+class VectorVel;
+class FrameVel;
+class RotationVel;
+
+class VectorVel
+// = TITLE
+//     An VectorVel is a Vector and its first derivative
+// = CLASS TYPE
+//     Concrete
+{
+public:
+    Vector p;       // position vector
+    Vector v;       // velocity vector
+public:
+    VectorVel():p(),v(){}
+    VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
+    explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
+
+    Vector value() const { return p;}
+    Vector deriv() const { return v;}
+
+    IMETHOD VectorVel& operator = (const VectorVel& arg);
+    IMETHOD VectorVel& operator = (const Vector& arg);
+    IMETHOD VectorVel& operator += (const VectorVel& arg);
+    IMETHOD VectorVel& operator -= (const VectorVel& arg);
+    IMETHOD static VectorVel Zero();
+    IMETHOD void ReverseSign();
+    IMETHOD doubleVel Norm() const;
+    IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
+    IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
+    IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
+    IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
+    IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
+
+    IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
+    IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
+    IMETHOD friend void SetToZero(VectorVel& v);
+
+
+    IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
+    IMETHOD friend VectorVel operator - (const VectorVel& r);
+    IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
+    IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
+    IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
+};
+
+
+
+class RotationVel
+// = TITLE
+//     An RotationVel is a Rotation and its first derivative, a rotation vector
+// = CLASS TYPE
+//     Concrete
+{
+public:
+    Rotation R; // Rotation matrix
+    Vector   w; // rotation vector
+public:
+    RotationVel():R(),w() {}
+    explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
+    RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
+
+
+    Rotation value() const { return R;}
+    Vector   deriv() const { return w;}
+
+
+    IMETHOD RotationVel& operator = (const RotationVel& arg);
+    IMETHOD RotationVel& operator = (const Rotation& arg);
+	IMETHOD VectorVel UnitX() const;
+	IMETHOD VectorVel UnitY() const;
+	IMETHOD VectorVel UnitZ() const;
+    IMETHOD static RotationVel Identity();
+    IMETHOD RotationVel Inverse() const;
+    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
+    IMETHOD VectorVel Inverse(const Vector& arg) const;
+    IMETHOD VectorVel operator*(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const Vector& arg) const;
+    IMETHOD void DoRotX(const doubleVel& angle);
+    IMETHOD void DoRotY(const doubleVel& angle);
+    IMETHOD void DoRotZ(const doubleVel& angle);
+    IMETHOD static RotationVel RotX(const doubleVel& angle);
+    IMETHOD static RotationVel RotY(const doubleVel& angle);
+    IMETHOD static RotationVel RotZ(const doubleVel& angle);
+    IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
+    // rotvec has arbitrary norm
+    // rotation around a constant vector !
+    IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
+    // rotvec is normalized.
+    // rotation around a constant vector !
+    IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
+    IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
+    IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
+    IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
+
+    IMETHOD TwistVel Inverse(const TwistVel& arg) const;
+    IMETHOD TwistVel Inverse(const Twist& arg) const;
+    IMETHOD TwistVel operator * (const TwistVel& arg) const;
+    IMETHOD TwistVel operator * (const Twist& arg) const;
+};
+
+
+
+
+class FrameVel
+// = TITLE
+//     An FrameVel is a Frame and its first derivative, a Twist vector
+// = CLASS TYPE
+//     Concrete
+// = CAVEATS
+//
+{
+public:
+    RotationVel M;
+    VectorVel   p;
+public:
+    FrameVel(){}
+
+    explicit FrameVel(const Frame& _T):
+        M(_T.M),p(_T.p) {}
+
+    FrameVel(const Frame& _T,const Twist& _t):
+        M(_T.M,_t.rot),p(_T.p,_t.vel) {}
+
+    FrameVel(const RotationVel& _M,const VectorVel& _p):
+        M(_M),p(_p) {}
+
+
+    Frame value() const { return Frame(M.value(),p.value());}
+    Twist deriv() const { return Twist(p.deriv(),M.deriv());}
+
+
+    IMETHOD FrameVel& operator = (const Frame& arg);
+    IMETHOD FrameVel& operator = (const FrameVel& arg);
+    IMETHOD static FrameVel Identity();
+    IMETHOD FrameVel Inverse() const;
+    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const VectorVel& arg) const;
+    IMETHOD VectorVel operator*(const Vector& arg) const;
+    IMETHOD VectorVel Inverse(const Vector& arg) const;
+    IMETHOD Frame GetFrame() const;
+    IMETHOD Twist GetTwist() const;
+    IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
+    IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
+    IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
+    IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
+    IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
+
+    IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
+    IMETHOD TwistVel  Inverse(const Twist& arg) const;
+    IMETHOD TwistVel operator * (const TwistVel& arg) const;
+    IMETHOD TwistVel operator * (const Twist& arg) const;
+};
+
+
+
+
+
+//very similar to Wrench class.
+class TwistVel
+// = TITLE
+// This class represents a TwistVel. This is a velocity and rotational velocity together
+{
+public:
+    VectorVel vel;
+    VectorVel rot;
+public:
+
+// = Constructors
+    TwistVel():vel(),rot() {};
+    TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
+    TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
+    TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
+
+    Twist value() const {
+        return Twist(vel.value(),rot.value());
+    }
+    Twist deriv() const {
+        return Twist(vel.deriv(),rot.deriv());
+    }
+// = Operators
+     IMETHOD TwistVel& operator-=(const TwistVel& arg);
+     IMETHOD TwistVel& operator+=(const TwistVel& arg);
+
+// = External operators
+     IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
+     IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
+
+     IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
+     IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
+
+     IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
+     IMETHOD friend TwistVel operator-(const TwistVel& arg);
+     IMETHOD friend void SetToZero(TwistVel& v);
+
+
+// = Zero
+     static IMETHOD TwistVel Zero();
+
+// = Reverse Sign
+     IMETHOD void ReverseSign();
+
+// = Change Reference point
+     IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
+     // Changes the reference point of the TwistVel.
+     // The VectorVel v_base_AB is expressed in the same base as the TwistVel
+     // The VectorVel v_base_AB is a VectorVel from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+
+     // = Equality operators
+     // do not use operator == because the definition of Equal(.,.) is slightly
+     // different.  It compares whether the 2 arguments are equal in an eps-interval
+     IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
+     IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
+     IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
+
+// = Conversion to other entities
+     IMETHOD Twist GetTwist() const;
+     IMETHOD Twist GetTwistDot() const;
+// = Friends
+    friend class RotationVel;
+    friend class FrameVel;
+
+};
+
+IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
+	return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
+}
+
+IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
+	return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
+}
+IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
+	return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
+}
+
+IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
+	return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
+}
+
+IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
+	return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
+}
+
+IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
+	return FrameVel(
+			addDelta(a.M,da.rot,dt),
+			addDelta(a.p,da.vel,dt)
+		   );
+}
+
+IMETHOD void random(VectorVel& a) {
+	random(a.p);
+	random(a.v);
+}
+IMETHOD void random(TwistVel& a) {
+	random(a.vel);
+	random(a.rot);
+}
+
+IMETHOD void random(RotationVel& R) {
+	random(R.R);
+	random(R.w);
+}
+
+IMETHOD void random(FrameVel& F) {
+	random(F.M);
+	random(F.p);
+}
+IMETHOD void posrandom(VectorVel& a) {
+	posrandom(a.p);
+	posrandom(a.v);
+}
+IMETHOD void posrandom(TwistVel& a) {
+	posrandom(a.vel);
+	posrandom(a.rot);
+}
+
+IMETHOD void posrandom(RotationVel& R) {
+	posrandom(R.R);
+	posrandom(R.w);
+}
+
+IMETHOD void posrandom(FrameVel& F) {
+	posrandom(F.M);
+	posrandom(F.p);
+}
+
+#ifdef KDL_INLINE
+#include "framevel.inl"
+#endif
+
+} // namespace
+
+#endif
+
+
+
+
diff --git a/intern/itasc/kdl/framevel.inl b/intern/itasc/kdl/framevel.inl
new file mode 100644
index 00000000000..994b3d2028e
--- /dev/null
+++ b/intern/itasc/kdl/framevel.inl
@@ -0,0 +1,534 @@
+/*****************************************************************************
+ * \file  
+ *      provides inline functions of rframes.h
+ *       
+ *  \author 
+ *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
+ *
+ *  \version 
+ *      ORO_Geometry V0.2
+ *
+ *  \par History
+ *      - $log$
+ *
+ *  \par Release
+ *      $Id: framevel.inl 19905 2009-04-23 13:29:54Z ben2610 $
+ *      $Name:  $ 
+ ****************************************************************************/
+
+
+// Methods and operators related to FrameVelVel
+// They all delegate most of the work to RotationVelVel and VectorVelVel
+FrameVel& FrameVel::operator = (const FrameVel& arg) {
+    M=arg.M;
+    p=arg.p;
+    return *this;
+}
+
+FrameVel FrameVel::Identity() {
+    return FrameVel(RotationVel::Identity(),VectorVel::Zero());
+}
+
+
+FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
+{
+    return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
+{
+    return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
+}
+FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
+{
+    return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
+}
+
+VectorVel FrameVel::operator *(const VectorVel & arg) const
+{
+    return M*arg+p;
+}
+VectorVel FrameVel::operator *(const Vector & arg) const
+{
+    return M*arg+p;
+}
+
+VectorVel FrameVel::Inverse(const VectorVel& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+VectorVel FrameVel::Inverse(const Vector& arg) const
+{
+    return M.Inverse(arg-p);
+}
+
+FrameVel FrameVel::Inverse() const
+{
+    return FrameVel(M.Inverse(),-M.Inverse(p));
+}
+
+FrameVel& FrameVel::operator = (const Frame& arg) {
+    M = arg.M;
+    p = arg.p;
+    return *this;
+}
+bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
+    return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
+}
+
+Frame FrameVel::GetFrame() const {
+    return Frame(M.R,p.p);
+}
+
+Twist FrameVel::GetTwist() const {
+    return Twist(p.v,M.w);
+}
+
+
+RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
+    return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
+}
+
+RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
+    return RotationVel( r1*r2.R, r1*r2.w );
+}
+
+RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
+    return RotationVel( r1.R*r2, r1.w );
+}
+
+RotationVel& RotationVel::operator = (const RotationVel& arg) {
+        R=arg.R;
+        w=arg.w;
+        return *this;
+    }
+RotationVel& RotationVel::operator = (const Rotation& arg) {
+    R=arg;
+    w=Vector::Zero();
+    return *this;
+}
+
+VectorVel   RotationVel::UnitX() const {
+	return VectorVel(R.UnitX(),w*R.UnitX()); 
+}
+
+VectorVel   RotationVel::UnitY() const {
+	return VectorVel(R.UnitY(),w*R.UnitY()); 
+}
+
+VectorVel   RotationVel::UnitZ() const {
+	return VectorVel(R.UnitZ(),w*R.UnitZ()); 
+}
+
+
+
+RotationVel RotationVel::Identity() {
+    return RotationVel(Rotation::Identity(),Vector::Zero());
+}
+
+RotationVel RotationVel::Inverse() const {
+    return RotationVel(R.Inverse(),-R.Inverse(w));
+}
+
+VectorVel RotationVel::Inverse(const VectorVel& arg) const {
+    Vector tmp=R.Inverse(arg.p);
+    return VectorVel(tmp,
+                    R.Inverse(arg.v-w*arg.p)
+                    );
+}
+
+VectorVel RotationVel::Inverse(const Vector& arg) const {
+    Vector tmp=R.Inverse(arg);
+    return VectorVel(tmp,
+                    R.Inverse(-w*arg)
+                    );
+}
+
+
+VectorVel RotationVel::operator*(const VectorVel& arg) const {
+    Vector tmp=R*arg.p;
+    return VectorVel(tmp,w*tmp+R*arg.v);
+}
+
+VectorVel RotationVel::operator*(const Vector& arg) const {
+    Vector tmp=R*arg;
+    return VectorVel(tmp,w*tmp);
+}
+
+
+// = Rotations
+// The Rot... static functions give the value of the appropriate rotation matrix back.
+// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
+
+void RotationVel::DoRotX(const doubleVel& angle) {
+    w+=R*Vector(angle.grad,0,0);
+    R.DoRotX(angle.t);
+}
+RotationVel RotationVel::RotX(const doubleVel& angle) {
+    return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
+}
+
+void RotationVel::DoRotY(const doubleVel& angle) {
+    w+=R*Vector(0,angle.grad,0);
+    R.DoRotY(angle.t);
+}
+RotationVel RotationVel::RotY(const doubleVel& angle) {
+    return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
+}
+
+void RotationVel::DoRotZ(const doubleVel& angle) {
+    w+=R*Vector(0,0,angle.grad);
+    R.DoRotZ(angle.t);
+}
+RotationVel RotationVel::RotZ(const doubleVel& angle) {
+    return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
+}
+
+
+RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) 
+// rotvec has arbitrary norm
+// rotation around a constant vector !
+{
+    Vector v(rotvec);
+	v.Normalize();
+    return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
+}
+
+RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) 
+    // rotvec is normalized.
+{
+    return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
+}
+
+
+VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p+r2.p,r1.v+r2.v);
+}
+
+VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p-r2.p,r1.v-r2.v);
+}
+
+VectorVel operator + (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p+r2,r1.v);
+}
+
+VectorVel operator - (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p-r2,r1.v);
+}
+
+VectorVel operator + (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1+r2.p,r2.v);
+}
+
+VectorVel operator - (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1-r2.p,-r2.v);
+}
+
+// unary -
+VectorVel operator - (const VectorVel& r) {
+    return VectorVel(-r.p,-r.v);
+}
+
+void SetToZero(VectorVel& v){
+    SetToZero(v.p);
+    SetToZero(v.v);
+}
+
+// cross prod.
+VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
+}
+
+VectorVel operator * (const VectorVel& r1,const Vector& r2) {
+    return VectorVel(r1.p*r2, r1.v*r2);
+}
+
+VectorVel operator * (const Vector& r1,const VectorVel& r2) {
+    return VectorVel(r1*r2.p, r1*r2.v);
+}
+
+
+
+// scalar mult.
+VectorVel operator * (double r1,const VectorVel& r2) {
+    return VectorVel(r1*r2.p, r1*r2.v);
+}
+
+VectorVel operator * (const VectorVel& r1,double r2) {
+    return VectorVel(r1.p*r2, r1.v*r2);
+}
+
+
+
+VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
+    return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
+}
+
+VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
+    return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
+}
+
+VectorVel operator / (const VectorVel& r1,double r2) {
+    return VectorVel(r1.p/r2, r1.v/r2);
+}
+
+VectorVel operator / (const VectorVel& r2,const doubleVel& r1) {
+    return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
+}
+
+VectorVel operator*(const Rotation& R,const VectorVel& x) {
+    return VectorVel(R*x.p,R*x.v);
+}
+
+VectorVel& VectorVel::operator = (const VectorVel& arg) {
+    p=arg.p;
+    v=arg.v;
+    return *this;
+}
+VectorVel& VectorVel::operator = (const Vector& arg) {
+    p=arg;
+    v=Vector::Zero();
+    return *this;
+}
+VectorVel& VectorVel::operator += (const VectorVel& arg) {
+    p+=arg.p;
+    v+=arg.v;
+    return *this;
+}
+VectorVel& VectorVel::operator -= (const VectorVel& arg) {
+    p-=arg.p;
+    v-=arg.v;
+    return *this;
+}
+
+VectorVel VectorVel::Zero() {
+    return VectorVel(Vector::Zero(),Vector::Zero());
+}
+void VectorVel::ReverseSign() {
+    p.ReverseSign();
+    v.ReverseSign();
+}
+doubleVel VectorVel::Norm() const {
+    double n = p.Norm();
+    return doubleVel(n,dot(p,v)/n);
+}
+
+bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
+    return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
+}
+bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
+    return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
+}
+bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
+    return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
+}
+
+bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
+    return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
+}
+bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
+    return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
+}
+bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
+    return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
+}
+bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const Twist& a,const TwistVel& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+bool Equal(const TwistVel& a,const Twist& b,double eps) {
+        return (Equal(a.rot,b.rot,eps)&&
+                Equal(a.vel,b.vel,eps)  );
+}
+
+
+
+IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
+    return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p));
+}
+IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
+    return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
+}
+IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
+    return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
+}
+
+
+
+
+
+
+
+
+
+
+
+
+TwistVel TwistVel::Zero()
+{
+    return TwistVel(VectorVel::Zero(),VectorVel::Zero());
+}
+
+
+void TwistVel::ReverseSign()
+{   
+    vel.ReverseSign();
+    rot.ReverseSign();
+}
+
+TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB)
+     // Changes the reference point of the TwistVel.
+     // The VectorVel v_base_AB is expressed in the same base as the TwistVel
+     // The VectorVel v_base_AB is a VectorVel from the old point to
+     // the new point.
+     // Complexity : 6M+6A
+{
+    return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
+}
+
+TwistVel& TwistVel::operator-=(const TwistVel& arg)
+{
+    vel-=arg.vel;
+    rot -=arg.rot;
+    return *this;
+}
+
+TwistVel& TwistVel::operator+=(const TwistVel& arg)
+{
+    vel+=arg.vel;
+    rot +=arg.rot;
+    return *this;
+}
+
+
+TwistVel operator*(const TwistVel& lhs,double rhs)
+{
+    return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistVel operator*(double lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistVel operator/(const TwistVel& lhs,double rhs)
+{
+    return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
+{
+    return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
+}
+
+TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
+}
+
+TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs)
+{
+    return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
+}
+
+
+
+// addition of TwistVel's
+TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
+}
+
+TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
+{
+    return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
+}
+
+// unary -
+TwistVel operator-(const TwistVel& arg) 
+{
+    return TwistVel(-arg.vel,-arg.rot);
+}
+
+void SetToZero(TwistVel& v)
+{
+   SetToZero(v.vel);
+   SetToZero(v.rot);
+}
+
+
+
+
+
+TwistVel RotationVel::Inverse(const TwistVel& arg) const
+{
+    return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistVel RotationVel::operator * (const TwistVel& arg) const
+{
+    return TwistVel((*this)*arg.vel,(*this)*arg.rot);
+}
+
+TwistVel RotationVel::Inverse(const Twist& arg) const
+{
+    return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
+}
+
+TwistVel RotationVel::operator * (const Twist& arg) const
+{
+    return TwistVel((*this)*arg.vel,(*this)*arg.rot);
+}
+
+
+TwistVel FrameVel::operator * (const TwistVel& arg) const
+{
+    TwistVel tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistVel FrameVel::operator * (const Twist& arg) const
+{
+    TwistVel tmp;
+    tmp.rot = M*arg.rot;
+    tmp.vel = M*arg.vel+p*tmp.rot;
+    return tmp;
+}
+
+TwistVel FrameVel::Inverse(const TwistVel& arg) const
+{
+    TwistVel tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+TwistVel FrameVel::Inverse(const Twist& arg) const
+{
+    TwistVel tmp;
+    tmp.rot =  M.Inverse(arg.rot);
+    tmp.vel = M.Inverse(arg.vel-p*arg.rot);
+    return tmp;
+}
+
+Twist TwistVel::GetTwist() const {
+    return Twist(vel.p,rot.p);
+}
+
+Twist TwistVel::GetTwistDot() const {
+    return Twist(vel.v,rot.v);
+}
diff --git a/intern/itasc/kdl/inertia.cpp b/intern/itasc/kdl/inertia.cpp
new file mode 100644
index 00000000000..6c7337d0dc4
--- /dev/null
+++ b/intern/itasc/kdl/inertia.cpp
@@ -0,0 +1,48 @@
+// 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
+
+#include "inertia.hpp"
+
+#include <Eigen/Array>
+
+namespace KDL {
+using namespace Eigen;
+
+Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz):
+data(Matrix<double,6,6>::Zero())
+{
+    data(0,0)=Ixx;
+    data(1,1)=Iyy;
+    data(2,2)=Izz;
+    data(2,1)=data(1,2)=Ixy;
+    data(3,1)=data(1,3)=Ixz;
+    data(3,2)=data(2,3)=Iyz;
+
+	data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity();
+}
+
+Inertia::~Inertia()
+{
+}
+
+
+
+}
diff --git a/intern/itasc/kdl/inertia.hpp b/intern/itasc/kdl/inertia.hpp
new file mode 100644
index 00000000000..9f33859671c
--- /dev/null
+++ b/intern/itasc/kdl/inertia.hpp
@@ -0,0 +1,70 @@
+// 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
+
+#ifndef KDLINERTIA_HPP
+#define KDLINERTIA_HPP
+
+#include <Eigen/Array>
+#include "frames.hpp"
+
+namespace KDL {
+
+using namespace Eigen;
+
+/**
+ *	This class offers the inertia-structure of a body
+ *	An inertia is defined in a certain reference point and a certain reference base.
+ *	The reference point does not have to coincide with the origin of the reference frame.
+ */
+class Inertia{
+public:
+
+	/**
+	 * 	This constructor creates a cartesian space inertia matrix,
+	 * 	the arguments are the mass and the inertia moments in the cog.
+	 */
+	Inertia(double m=0,double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
+
+    static inline Inertia Zero(){
+        return Inertia(0,0,0,0,0,0,0);
+    };
+
+    friend class Rotation;
+    friend class Frame;
+
+    /**
+     * F = m*a
+     */
+   // Wrench operator* (const AccelerationTwist& acc);
+
+
+    ~Inertia();
+private:
+    Matrix<double,6,6,RowMajor> data;
+
+};
+
+
+
+
+}
+
+#endif
diff --git a/intern/itasc/kdl/jacobian.cpp b/intern/itasc/kdl/jacobian.cpp
new file mode 100644
index 00000000000..4166a341fe7
--- /dev/null
+++ b/intern/itasc/kdl/jacobian.cpp
@@ -0,0 +1,129 @@
+// 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
+
+#include "jacobian.hpp"
+
+namespace KDL
+{
+    Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks):
+        size(_size),nr_blocks(_nr_blocks)
+    {
+        twists = new Twist[size*nr_blocks];
+    }
+
+    Jacobian::Jacobian(const Jacobian& arg):
+                       size(arg.columns()),
+                       nr_blocks(arg.nr_blocks)
+    {
+        twists = new Twist[size*nr_blocks];
+        for(unsigned int i=0;i<size*nr_blocks;i++)
+            twists[i] = arg.twists[i];
+    }
+
+    Jacobian& Jacobian::operator = (const Jacobian& arg)
+    {
+        assert(size==arg.size);
+        assert(nr_blocks==arg.nr_blocks);
+        for(unsigned int i=0;i<size;i++)
+            twists[i]=arg.twists[i];
+        return *this;
+    }
+
+
+    Jacobian::~Jacobian()
+    {
+        delete [] twists;
+    }
+
+    double Jacobian::operator()(int i,int j)const
+    {
+        assert(i<6*nr_blocks&&j<size);
+        return twists[j+6*(int)(floor((double)i/6))](i%6);
+    }
+
+    double& Jacobian::operator()(int i,int j)
+    {
+        assert(i<6*nr_blocks&&j<size);
+        return twists[j+6*(int)(floor((double)i/6))](i%6);
+    }
+
+    unsigned int Jacobian::rows()const
+    {
+        return 6*nr_blocks;
+    }
+
+    unsigned int Jacobian::columns()const
+    {
+        return size;
+    }
+
+    void SetToZero(Jacobian& jac)
+    {
+        for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
+            SetToZero(jac.twists[i]);
+    }
+
+    void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=src1.twists[i].RefPoint(base_AB);
+    }
+
+    void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=rot*src1.twists[i];
+    }
+
+    void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
+    {
+        assert(src1.size==dest.size);
+        assert(src1.nr_blocks==dest.nr_blocks);
+        for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
+            dest.twists[i]=frame*src1.twists[i];
+    }
+
+    bool Jacobian::operator ==(const Jacobian& arg)
+    {
+        return Equal((*this),arg);
+    }
+    
+    bool Jacobian::operator!=(const Jacobian& arg)
+    {
+        return !Equal((*this),arg);
+    }
+    
+    bool Equal(const Jacobian& a,const Jacobian& b,double eps)
+    {
+        if(a.rows()==b.rows()&&a.columns()==b.columns()){
+            bool rc=true;
+            for(unsigned int i=0;i<a.columns();i++)
+                rc&=Equal(a.twists[i],b.twists[i],eps);
+            return rc;
+        }else
+            return false;
+    }
+    
+}
diff --git a/intern/itasc/kdl/jacobian.hpp b/intern/itasc/kdl/jacobian.hpp
new file mode 100644
index 00000000000..e9057451c9f
--- /dev/null
+++ b/intern/itasc/kdl/jacobian.hpp
@@ -0,0 +1,68 @@
+// 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
+
+#ifndef KDL_JACOBIAN_HPP
+#define KDL_JACOBIAN_HPP
+
+#include "frames.hpp"
+
+namespace KDL
+{
+    //Forward declaration
+    class ChainJntToJacSolver;
+
+    class Jacobian
+    {
+        friend class ChainJntToJacSolver;
+    private:
+        unsigned int size;
+        unsigned int nr_blocks;
+    public:
+        Twist* twists;
+        Jacobian(unsigned int size,unsigned int nr=1);
+        Jacobian(const Jacobian& arg);
+
+        Jacobian& operator=(const Jacobian& arg);
+
+        bool operator ==(const Jacobian& arg);
+        bool operator !=(const Jacobian& arg);
+        
+        friend bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon);
+        
+
+        ~Jacobian();
+
+        double operator()(int i,int j)const;
+        double& operator()(int i,int j);
+        unsigned int rows()const;
+        unsigned int columns()const;
+
+        friend void SetToZero(Jacobian& jac);
+
+        friend void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
+        friend void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
+        friend void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
+
+
+    };
+}
+
+#endif
diff --git a/intern/itasc/kdl/jntarray.cpp b/intern/itasc/kdl/jntarray.cpp
new file mode 100644
index 00000000000..2adb76081f3
--- /dev/null
+++ b/intern/itasc/kdl/jntarray.cpp
@@ -0,0 +1,152 @@
+// 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
+
+#include "jntarray.hpp"
+
+namespace KDL
+{
+    JntArray::JntArray():
+            size(0),
+            data(NULL)
+    {
+    }
+
+    JntArray::JntArray(unsigned int _size):
+        size(_size)
+    {
+        assert(0 < size);
+        data = new double[size];
+        SetToZero(*this);
+    }
+
+
+    JntArray::JntArray(const JntArray& arg):
+        size(arg.size)
+    {
+        data = ((0 < size) ? new double[size] : NULL);
+        for(unsigned int i=0;i<size;i++)
+            data[i]=arg.data[i];
+    }
+
+    JntArray& JntArray::operator = (const JntArray& arg)
+    {
+        assert(size==arg.size);
+        for(unsigned int i=0;i<size;i++)
+            data[i]=arg.data[i];
+        return *this;
+    }
+
+
+    JntArray::~JntArray()
+    {
+        delete [] data;
+    }
+
+    void JntArray::resize(unsigned int newSize)
+    {
+        delete [] data;
+        size = newSize;
+        data = new double[size];
+        SetToZero(*this);
+    }
+
+    double JntArray::operator()(unsigned int i,unsigned int j)const
+    {
+        assert(i<size&&j==0);
+        assert(0 != size);  // found JntArray containing no data
+        return data[i];
+    }
+
+    double& JntArray::operator()(unsigned int i,unsigned int j)
+    {
+        assert(i<size&&j==0);
+        assert(0 != size);  // found JntArray containing no data
+        return data[i];
+    }
+
+    unsigned int JntArray::rows()const
+    {
+        return size;
+    }
+
+    unsigned int JntArray::columns()const
+    {
+        return 0;
+    }
+
+    void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
+    {
+        assert(src1.size==src2.size&&src1.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src1.data[i]+src2.data[i];
+    }
+
+    void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
+    {
+        assert(src1.size==src2.size&&src1.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src1.data[i]-src2.data[i];
+    }
+
+    void Multiply(const JntArray& src,const double& factor,JntArray& dest)
+    {
+        assert(src.size==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=factor*src.data[i];
+    }
+
+    void Divide(const JntArray& src,const double& factor,JntArray& dest)
+    {
+        assert(src.rows()==dest.size);
+        for(unsigned int i=0;i<dest.size;i++)
+            dest.data[i]=src.data[i]/factor;
+    }
+
+    void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
+    {
+        assert(jac.columns()==src.size);
+        SetToZero(dest);
+        for(unsigned int i=0;i<6;i++)
+            for(unsigned int j=0;j<src.size;j++)
+                dest(i)+=jac(i,j)*src.data[j];
+    }
+
+    void SetToZero(JntArray& array)
+    {
+        for(unsigned int i=0;i<array.size;i++)
+            array.data[i]=0;
+    }
+
+    bool Equal(const JntArray& src1, const JntArray& src2,double eps)
+    {
+        assert(src1.size==src2.size);
+        bool ret = true;
+        for(unsigned int i=0;i<src1.size;i++)
+            ret = ret && Equal(src1.data[i],src2.data[i],eps);
+        return ret;
+    }
+
+    bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
+    //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
+
+}
+
+
diff --git a/intern/itasc/kdl/jntarray.hpp b/intern/itasc/kdl/jntarray.hpp
new file mode 100644
index 00000000000..19ffa4343e3
--- /dev/null
+++ b/intern/itasc/kdl/jntarray.hpp
@@ -0,0 +1,217 @@
+// 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
+
+#ifndef KDL_JNTARRAY_HPP
+#define KDL_JNTARRAY_HPP
+
+#include "frames.hpp"
+#include "jacobian.hpp"
+
+namespace KDL
+{
+    /**
+     * @brief This class represents an fixed size array containing
+     * joint values of a KDL::Chain.
+     *
+     * \warning An object constructed with the default constructor provides
+     * a valid, but inert, object. Many of the member functions will do
+     * the correct thing and have no affect on this object, but some 
+     * member functions can _NOT_ deal with an inert/empty object. These 
+     * functions will assert() and exit the program instead. The intended use 
+     * case for the default constructor (in an RTT/OCL setting) is outlined in
+     * code below - the default constructor plus the resize() function allow
+     * use of JntArray objects whose size is set within a configureHook() call
+     * (typically based on a size determined from a property).
+	 
+\code
+class MyTask : public RTT::TaskContext
+{
+   JntArray		j;
+   MyTask() 
+   {}			// invokes j's default constructor
+
+   bool configureHook()
+   {
+       unsigned int size = some_property.rvalue();
+	   j.resize(size)
+	   ...
+   }
+
+   void updateHook()
+   {
+   ** use j here
+   }
+};
+/endcode
+
+     */	
+
+    class JntArray
+    {
+    private:
+        unsigned int size;
+        double* data;
+    public:
+        /** Construct with _no_ data array
+         * @post NULL == data
+         * @post 0 == rows()
+         * @warning use of an object constructed like this, without
+         * a resize() first, may result in program exit! See class
+         * documentation.
+         */
+        JntArray();
+        /**
+         * Constructor of the joint array
+         *
+         * @param size size of the array, this cannot be changed
+         * afterwards.
+         * @pre 0 < size
+         * @post NULL != data
+         * @post 0 < rows()
+         * @post all elements in data have 0 value
+         */
+        JntArray(unsigned int size);
+        /** Copy constructor 
+         * @note Will correctly copy an empty object
+         */
+        JntArray(const JntArray& arg);
+        ~JntArray();
+        /** Resize the array 
+         * @warning This causes a dynamic allocation (and potentially 	
+         * also a dynamic deallocation). This _will_ negatively affect
+         * real-time performance! 
+         *
+         * @post newSize == rows()
+         * @post NULL != data
+         * @post all elements in data have 0 value
+         */
+        void resize(unsigned int newSize);
+		
+        JntArray& operator = ( const JntArray& arg);
+        /**
+         * get_item operator for the joint array, if a second value is
+         * given it should be zero, since a JntArray resembles a column.
+         *
+         *
+         * @return the joint value at position i, starting from 0
+         * @pre 0 != size (ie non-default constructor or resize() called)
+         */
+        double operator()(unsigned int i,unsigned int j=0)const;
+        /**
+         * set_item operator, again if a second value is given it
+         *should be zero.
+         *
+         * @return reference to the joint value at position i,starting
+         *from zero.
+         * @pre 0 != size (ie non-default constructor or resize() called)
+         */
+        double& operator()(unsigned int i,unsigned int j=0);
+        /**
+         * Returns the number of rows (size) of the array
+         *
+         */
+        unsigned int rows()const;
+        /**
+         * Returns the number of columns of the array, always 1.
+         */
+        unsigned int columns()const;
+
+        /**
+         * Function to add two joint arrays, all the arguments must
+         * have the same size: A + B = C. This function is
+         * aliasing-safe, A or B can be the same array as C.
+         *
+         * @param src1 A
+         * @param src2 B
+         * @param dest C
+         */
+        friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
+        /**
+         * Function to subtract two joint arrays, all the arguments must
+         * have the same size: A - B = C. This function is
+         * aliasing-safe, A or B can be the same array as C.
+         *
+         * @param src1 A
+         * @param src2 B
+         * @param dest C
+         */
+        friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
+        /**
+         * Function to multiply all the array values with a scalar
+         * factor: A*b=C. This function is aliasing-safe, A can be the
+         * same array as C.
+         *
+         * @param src A
+         * @param factor b
+         * @param dest C
+         */
+        friend void Multiply(const JntArray& src,const double& factor,JntArray& dest);
+        /**
+         * Function to divide all the array values with a scalar
+         * factor: A/b=C. This function is aliasing-safe, A can be the
+         * same array as C.
+         *
+         * @param src A
+         * @param factor b
+         * @param dest C
+         */
+        friend void Divide(const JntArray& src,const double& factor,JntArray& dest);
+        /**
+         * Function to multiply a KDL::Jacobian with a KDL::JntArray
+         * to get a KDL::Twist, it should not be used to calculate the
+         * forward velocity kinematics, the solver classes are built
+         * for this purpose.
+         * J*q = t
+         *
+         * @param jac J
+         * @param src q
+         * @param dest t
+         * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
+         */
+        friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
+        /**
+         * Function to set all the values of the array to 0
+         *
+         * @param array
+         */
+        friend void SetToZero(JntArray& array);
+        /**
+         * Function to check if two arrays are the same with a
+         *precision of eps
+         *
+         * @param src1
+         * @param src2
+         * @param eps default: epsilon
+         * @return true if each element of src1 is within eps of the same
+		 * element in src2, or if both src1 and src2 have no data (ie 0==rows())
+         */
+        friend bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon);
+
+        friend bool operator==(const JntArray& src1,const JntArray& src2);
+        //friend bool operator!=(const JntArray& src1,const JntArray& src2);
+        };
+
+    bool operator==(const JntArray& src1,const JntArray& src2);
+    //bool operator!=(const JntArray& src1,const JntArray& src2);
+
+}
+
+#endif
diff --git a/intern/itasc/kdl/jntarrayacc.cpp b/intern/itasc/kdl/jntarrayacc.cpp
new file mode 100644
index 00000000000..3c9c67d9ef9
--- /dev/null
+++ b/intern/itasc/kdl/jntarrayacc.cpp
@@ -0,0 +1,170 @@
+// 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
+
+#include "jntarrayacc.hpp"
+
+namespace KDL
+{
+    JntArrayAcc::JntArrayAcc(unsigned int size):
+        q(size),qdot(size),qdotdot(size)
+    {
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
+        q(qin),qdot(qdotin),qdotdot(qdotdotin)
+    {
+        assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
+        q(qin),qdot(qdotin),qdotdot(q.rows())
+    {
+        assert(q.rows()==qdot.rows());
+    }
+    JntArrayAcc::JntArrayAcc(const JntArray& qin):
+        q(qin),qdot(q.rows()),qdotdot(q.rows())
+    {
+    }
+
+    JntArray JntArrayAcc::value()const
+    {
+        return q;
+    }
+
+    JntArray JntArrayAcc::deriv()const
+    {
+        return qdot;
+    }
+    JntArray JntArrayAcc::dderiv()const
+    {
+        return qdotdot;
+    }
+
+    void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+        Add(src1.qdotdot,src2.qdotdot,dest.qdotdot);
+    }
+    void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+        dest.qdotdot=src1.qdotdot;
+    }
+    void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
+    {
+        Add(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+        dest.qdotdot=src1.qdotdot;
+    }
+
+    void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+        Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot);
+    }
+    void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+        dest.qdotdot=src1.qdotdot;
+    }
+    void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
+    {
+        Subtract(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+        dest.qdotdot=src1.qdotdot;
+    }
+
+    void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,factor,dest.q);
+        Multiply(src.qdot,factor,dest.qdot);
+        Multiply(src.qdotdot,factor,dest.qdotdot);
+    }
+    void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.qdot,factor.grad*2,dest.qdot);
+        Multiply(src.qdotdot,factor.t,dest.qdotdot);
+        Add(dest.qdot,dest.qdotdot,dest.qdotdot);
+        Multiply(src.q,factor.grad,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+    void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,factor.dd,dest.q);
+        Multiply(src.qdot,factor.d*2,dest.qdot);
+        Multiply(src.qdotdot,factor.t,dest.qdotdot);
+        Add(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.d,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+
+    void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
+    {
+        Divide(src.q,factor,dest.q);
+        Divide(src.qdot,factor,dest.qdot);
+        Divide(src.qdotdot,factor,dest.qdotdot);
+    }
+    void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q);
+        Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot);
+        Divide(src.qdotdot,factor.t,dest.qdotdot);
+        Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+    void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
+    {
+        Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q);
+        Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot);
+        Divide(src.qdotdot,factor.t,dest.qdotdot);
+        Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
+        Add(dest.qdotdot,dest.q,dest.qdotdot);
+        Multiply(src.q,factor.d/(factor.t*factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+
+    void SetToZero(JntArrayAcc& array)
+    {
+        SetToZero(array.q);
+        SetToZero(array.qdot);
+        SetToZero(array.qdotdot);
+    }
+
+    bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps)
+    {
+        return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps));
+    }
+}
+
+
diff --git a/intern/itasc/kdl/jntarrayacc.hpp b/intern/itasc/kdl/jntarrayacc.hpp
new file mode 100644
index 00000000000..275aa58f21e
--- /dev/null
+++ b/intern/itasc/kdl/jntarrayacc.hpp
@@ -0,0 +1,66 @@
+// 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
+
+#ifndef KDL_JNTARRAYACC_HPP
+#define KDL_JNTARRAYACC_HPP
+
+#include "utilities/utility.h"
+#include "jntarray.hpp"
+#include "jntarrayvel.hpp"
+#include "frameacc.hpp"
+
+namespace KDL
+{
+    class JntArrayAcc
+    {
+    public:
+        JntArray q;
+        JntArray qdot;
+        JntArray qdotdot;
+    public:
+        JntArrayAcc(unsigned int size);
+        JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot);
+        JntArrayAcc(const JntArray& q,const JntArray& qdot);
+        JntArrayAcc(const JntArray& q);
+
+        JntArray value()const;
+        JntArray deriv()const;
+        JntArray dderiv()const;
+
+        friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
+        friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
+        friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
+        friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
+        friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
+        friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
+        friend void SetToZero(JntArrayAcc& array);
+        friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon);
+
+    };
+}
+
+#endif
diff --git a/intern/itasc/kdl/jntarrayvel.cpp b/intern/itasc/kdl/jntarrayvel.cpp
new file mode 100644
index 00000000000..df5c7fb0fb3
--- /dev/null
+++ b/intern/itasc/kdl/jntarrayvel.cpp
@@ -0,0 +1,111 @@
+// 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
+
+
+#include "jntarrayacc.hpp"
+
+namespace KDL
+{
+    JntArrayVel::JntArrayVel(unsigned int size):
+        q(size),qdot(size)
+    {
+    }
+    JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
+        q(qin),qdot(qdotin)
+    {
+        assert(q.rows()==qdot.rows());
+    }
+    JntArrayVel::JntArrayVel(const JntArray& qin):
+        q(qin),qdot(q.rows())
+    {
+    }
+
+    JntArray JntArrayVel::value()const
+    {
+        return q;
+    }
+
+    JntArray JntArrayVel::deriv()const
+    {
+        return qdot;
+    }
+
+    void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
+    {
+        Add(src1.q,src2.q,dest.q);
+        Add(src1.qdot,src2.qdot,dest.qdot);
+    }
+    void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
+    {
+        Add(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+    }
+
+    void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
+    {
+        Subtract(src1.q,src2.q,dest.q);
+        Subtract(src1.qdot,src2.qdot,dest.qdot);
+    }
+    void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
+    {
+        Subtract(src1.q,src2,dest.q);
+        dest.qdot=src1.qdot;
+    }
+
+    void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,factor,dest.q);
+        Multiply(src.qdot,factor,dest.qdot);
+    }
+    void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,factor.grad,dest.q);
+        Multiply(src.qdot,factor.t,dest.qdot);
+        Add(dest.qdot,dest.q,dest.qdot);
+        Multiply(src.q,factor.t,dest.q);
+    }
+
+    void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
+    {
+        Divide(src.q,factor,dest.q);
+        Divide(src.qdot,factor,dest.qdot);
+    }
+    void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
+    {
+        Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q);
+        Divide(src.qdot,factor.t,dest.qdot);
+        Subtract(dest.qdot,dest.q,dest.qdot);
+        Divide(src.q,factor.t,dest.q);
+    }
+
+    void SetToZero(JntArrayVel& array)
+    {
+        SetToZero(array.q);
+        SetToZero(array.qdot);
+    }
+
+    bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
+    {
+        return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
+    }
+}
+
+
diff --git a/intern/itasc/kdl/jntarrayvel.hpp b/intern/itasc/kdl/jntarrayvel.hpp
new file mode 100644
index 00000000000..faa82076ebb
--- /dev/null
+++ b/intern/itasc/kdl/jntarrayvel.hpp
@@ -0,0 +1,59 @@
+// 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
+
+#ifndef KDL_JNTARRAYVEL_HPP
+#define KDL_JNTARRAYVEL_HPP
+
+#include "utilities/utility.h"
+#include "jntarray.hpp"
+#include "framevel.hpp"
+
+namespace KDL
+{
+
+    class JntArrayVel
+    {
+    public:
+        JntArray q;
+        JntArray qdot;
+    public:
+        JntArrayVel(unsigned int size);
+        JntArrayVel(const JntArray& q,const JntArray& qdot);
+        JntArrayVel(const JntArray& q);
+
+        JntArray value()const;
+        JntArray deriv()const;
+
+        friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
+        friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
+        friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
+        friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
+        friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
+        friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
+        friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
+        friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
+        friend void SetToZero(JntArrayVel& array);
+        friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon);
+
+    };
+}
+
+#endif
diff --git a/intern/itasc/kdl/joint.cpp b/intern/itasc/kdl/joint.cpp
new file mode 100644
index 00000000000..dc5f17e5bf7
--- /dev/null
+++ b/intern/itasc/kdl/joint.cpp
@@ -0,0 +1,153 @@
+// 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
+
+#include "joint.hpp"
+
+namespace KDL {
+
+    Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
+                 const double& _inertia, const double& _damping, const double& _stiffness):
+        type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
+    {
+		// for sphere and swing, offset is not used, assume no offset
+    }
+
+    Joint::Joint(const Joint& in):
+	type(in.type),scale(in.scale),offset(in.offset),
+        inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
+    {
+    }
+
+    Joint& Joint::operator=(const Joint& in)
+    {
+        type=in.type;
+        scale=in.scale;
+        offset=in.offset;
+        inertia=in.inertia;
+        damping=in.damping;
+        stiffness=in.stiffness;
+		return *this;
+    }
+
+
+    Joint::~Joint()
+    {
+    }
+
+    Frame Joint::pose(const double& q)const
+    {
+
+        switch(type){
+        case RotX:
+            return Frame(Rotation::RotX(scale*q+offset));
+            break;
+        case RotY:
+            return  Frame(Rotation::RotY(scale*q+offset));
+            break;
+        case RotZ:
+            return  Frame(Rotation::RotZ(scale*q+offset));
+            break;
+        case TransX:
+            return  Frame(Vector(scale*q+offset,0.0,0.0));
+            break;
+        case TransY:
+            return Frame(Vector(0.0,scale*q+offset,0.0));
+            break;
+        case TransZ:
+            return Frame(Vector(0.0,0.0,scale*q+offset));
+            break;
+		case Sphere:
+			// the joint angles represent a rotation vector expressed in the base frame of the joint
+			// (= the frame you get when there is no offset nor rotation)
+			return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2])));
+			break;
+		case Swing:
+			// the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
+			// (= the frame you get when there is no offset nor rotation)
+			return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1])));
+			break;
+        default:
+            return Frame::Identity();
+            break;
+        }
+    }
+
+    Twist Joint::twist(const double& qdot, int dof)const
+    {
+        switch(type){
+        case RotX:
+            return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+            break;
+        case RotY:
+            return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
+            break;
+        case RotZ:
+            return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+            break;
+        case TransX:
+            return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
+            break;
+        case TransY:
+            return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
+            break;
+        case TransZ:
+            return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
+            break;
+		case Swing:
+			switch (dof) {
+			case 0:
+				return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+			case 1:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+			}
+            return Twist::Zero();
+		case Sphere:
+			switch (dof) {
+			case 0:
+				return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+			case 1:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
+			case 2:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+			}
+            return Twist::Zero();
+        default:
+            return Twist::Zero();
+            break;
+        }
+	}
+
+	unsigned int Joint::getNDof() const
+	{
+		switch (type) {
+		case Sphere:
+			return 3;
+		case Swing:
+			return 2;
+		case None:
+			return 0;
+		default:
+			return 1;
+		}
+	}
+
+} // end of namespace KDL
+
diff --git a/intern/itasc/kdl/joint.hpp b/intern/itasc/kdl/joint.hpp
new file mode 100644
index 00000000000..a1291509f0f
--- /dev/null
+++ b/intern/itasc/kdl/joint.hpp
@@ -0,0 +1,138 @@
+// 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
+
+#ifndef KDL_JOINT_HPP
+#define KDL_JOINT_HPP
+
+#include "frames.hpp"
+#include <string>
+
+namespace KDL {
+
+    /**
+	  * \brief This class encapsulates a simple joint, that is with one
+	  * parameterized degree of freedom and with scalar dynamic properties.
+     *
+     * A simple joint is described by the following properties :
+     *      - scale: ratio between motion input and motion output
+     *      - offset: between the "physical" and the "logical" zero position.
+     *      - type: revolute or translational, along one of the basic frame axes
+	  *      - inertia, stiffness and damping: scalars representing the physical
+	  *      effects along/about the joint axis only.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Joint {
+    public:
+        typedef enum { RotX,RotY,RotZ,TransX,TransY,TransZ,Sphere,Swing,None} JointType;
+        /**
+         * Constructor of a joint.
+         *
+         * @param type type of the joint, default: Joint::None
+         * @param scale scale between joint input and actual geometric
+         * movement, default: 1
+         * @param offset offset between joint input and actual
+         * geometric input, default: 0
+         * @param inertia 1D inertia along the joint axis, default: 0
+         * @param damping 1D damping along the joint axis, default: 0
+         * @param stiffness 1D stiffness along the joint axis,
+         * default: 0
+         */
+        Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
+              const double& inertia=0,const double& damping=0,const double& stiffness=0);
+        Joint(const Joint& in);
+
+        Joint& operator=(const Joint& arg);
+
+        /**
+         * Request the 6D-pose between the beginning and the end of
+         * the joint at joint position q
+         *
+         * @param q the 1D joint position
+         *
+         * @return the resulting 6D-pose
+         */
+        Frame pose(const double& q)const;
+        /**
+         * Request the resulting 6D-velocity with a joint velocity qdot
+         *
+         * @param qdot the 1D joint velocity
+         *
+         * @return the resulting 6D-velocity
+         */
+        Twist twist(const double& qdot, int dof=0)const;
+
+        /**
+         * Request the type of the joint.
+         *
+         * @return const reference to the type
+         */
+        const JointType& getType() const
+        {
+            return type;
+        };
+
+        /**
+         * Request the stringified type of the joint.
+         *
+         * @return const string
+         */
+        const std::string getTypeName() const
+        {
+            switch (type) {
+            case RotX:
+                return "RotX";
+            case RotY:
+                return "RotY";
+            case RotZ:
+                return "RotZ";
+            case TransX:
+                return "TransX";
+            case TransY:
+                return "TransY";
+            case TransZ:
+                return "TransZ";
+			case Sphere:
+				return "Sphere";
+			case Swing:
+				return "Swing";
+            case None:
+                return "None";
+            default:
+                return "None";
+            }
+        };
+		unsigned int getNDof() const;
+
+        virtual ~Joint();
+
+    private:
+        Joint::JointType type;
+        double scale;
+        double offset;
+        double inertia;
+        double damping;
+        double stiffness;
+    };
+
+} // end of namespace KDL
+
+#endif
diff --git a/intern/itasc/kdl/kinfam_io.cpp b/intern/itasc/kdl/kinfam_io.cpp
new file mode 100644
index 00000000000..900e2e101a9
--- /dev/null
+++ b/intern/itasc/kdl/kinfam_io.cpp
@@ -0,0 +1,101 @@
+// 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
+
+#include "kinfam_io.hpp"
+#include "frames_io.hpp"
+
+namespace KDL {
+std::ostream& operator <<(std::ostream& os, const Joint& joint) {
+	return os << joint.getTypeName();
+}
+
+std::istream& operator >>(std::istream& is, Joint& joint) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Segment& segment) {
+	os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Segment& segment) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Chain& chain) {
+	os << "[";
+	for (unsigned int i = 0; i < chain.getNrOfSegments(); i++)
+		os << chain.getSegment(i) << "\n";
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Chain& chain) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Tree& tree) {
+	SegmentMap::const_iterator root = tree.getSegment("root");
+	return os << root;
+}
+
+std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) {
+	//os<<root->first<<": "<<root->second.segment<<"\n";
+	os << root->first<<"(q_nr: "<<root->second.q_nr<<")"<<"\n \t";
+	for (unsigned int i = 0; i < root->second.children.size(); i++) {
+		os <<(root->second.children[i])<<"\t";
+	}
+	return os << "\n";
+}
+
+std::istream& operator >>(std::istream& is, Tree& tree) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const JntArray& array) {
+	os << "[";
+	for (unsigned int i = 0; i < array.rows(); i++)
+		os << std::setw(KDL_FRAME_WIDTH) << array(i);
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, JntArray& array) {
+	return is;
+}
+
+std::ostream& operator <<(std::ostream& os, const Jacobian& jac) {
+	os << "[";
+	for (unsigned int i = 0; i < jac.rows(); i++) {
+		for (unsigned int j = 0; j < jac.columns(); j++)
+			os << std::setw(KDL_FRAME_WIDTH) << jac(i, j);
+		os << std::endl;
+	}
+	os << "]";
+	return os;
+}
+
+std::istream& operator >>(std::istream& is, Jacobian& jac) {
+	return is;
+}
+
+}
+
diff --git a/intern/itasc/kdl/kinfam_io.hpp b/intern/itasc/kdl/kinfam_io.hpp
new file mode 100644
index 00000000000..a8dbfd1c5dc
--- /dev/null
+++ b/intern/itasc/kdl/kinfam_io.hpp
@@ -0,0 +1,70 @@
+// 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
+
+#ifndef KDL_KINFAM_IO_HPP
+#define KDL_KINFAM_IO_HPP
+
+#include <iostream>
+#include <fstream>
+
+#include "joint.hpp"
+#include "segment.hpp"
+#include "chain.hpp"
+#include "jntarray.hpp"
+#include "jacobian.hpp"
+#include "tree.hpp"
+
+namespace KDL {
+std::ostream& operator <<(std::ostream& os, const Joint& joint);
+std::istream& operator >>(std::istream& is, Joint& joint);
+std::ostream& operator <<(std::ostream& os, const Segment& segment);
+std::istream& operator >>(std::istream& is, Segment& segment);
+std::ostream& operator <<(std::ostream& os, const Chain& chain);
+std::istream& operator >>(std::istream& is, Chain& chain);
+
+std::ostream& operator <<(std::ostream& os, const Tree& tree);
+std::istream& operator >>(std::istream& is, Tree& tree);
+
+std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it);
+
+std::ostream& operator <<(std::ostream& os, const JntArray& array);
+std::istream& operator >>(std::istream& is, JntArray& array);
+std::ostream& operator <<(std::ostream& os, const Jacobian& jac);
+std::istream& operator >>(std::istream& is, Jacobian& jac);
+
+template<typename T>
+std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) {
+	os << "[";
+	for (unsigned int i = 0; i < vec.size(); i++)
+		os << vec[i] << " ";
+	os << "]";
+	return os;
+}
+;
+
+template<typename T>
+std::istream& operator >>(std::istream& is, std::vector<T>& vec) {
+	return is;
+}
+;
+}
+#endif
+
diff --git a/intern/itasc/kdl/segment.cpp b/intern/itasc/kdl/segment.cpp
new file mode 100644
index 00000000000..02f71d5e9f1
--- /dev/null
+++ b/intern/itasc/kdl/segment.cpp
@@ -0,0 +1,68 @@
+// 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
+
+#include "segment.hpp"
+
+namespace KDL {
+
+    Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M):
+        joint(_joint),M(_M),
+        f_tip(_f_tip)
+    {
+    }
+
+    Segment::Segment(const Segment& in):
+        joint(in.joint),M(in.M),
+        f_tip(in.f_tip)
+    {
+    }
+
+    Segment& Segment::operator=(const Segment& arg)
+    {
+        joint=arg.joint;
+        M=arg.M;
+        f_tip=arg.f_tip;
+        return *this;
+    }
+
+    Segment::~Segment()
+    {
+    }
+
+    Frame Segment::pose(const double& q)const
+    {
+        return joint.pose(q)*f_tip;
+    }
+
+    Twist Segment::twist(const double& q, const double& qdot, int dof)const
+    {
+        return joint.twist(qdot, dof).RefPoint(pose(q).p);
+    }
+
+    Twist Segment::twist(const Vector& p, const double& qdot, int dof)const
+    {
+        return joint.twist(qdot, dof).RefPoint(p);
+    }
+
+	Twist Segment::twist(const Frame& f, const double& qdot, int dof)const
+    {
+        return (f.M*joint.twist(qdot, dof)).RefPoint(f.p);
+    }
+}//end of namespace KDL
+
diff --git a/intern/itasc/kdl/segment.hpp b/intern/itasc/kdl/segment.hpp
new file mode 100644
index 00000000000..7c82ab418fa
--- /dev/null
+++ b/intern/itasc/kdl/segment.hpp
@@ -0,0 +1,149 @@
+// 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
+
+
+#ifndef KDL_SEGMENT_HPP
+#define KDL_SEGMENT_HPP
+
+#include "frames.hpp"
+#include "inertia.hpp"
+#include "joint.hpp"
+#include <vector>
+
+namespace KDL {
+
+    /**
+	  * \brief This class encapsulates a simple segment, that is a "rigid
+	  * body" (i.e., a frame and an inertia) with a joint and with
+	  * "handles", root and tip to connect to other segments.
+     *
+     * A simple segment is described by the following properties :
+     *      - Joint
+     *      - inertia: of the rigid body part of the Segment
+     *      - Offset from the end of the joint to the tip of the segment:
+     *        the joint is located at the root of the segment.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Segment {
+        friend class Chain;
+    private:
+        Joint joint;
+        Inertia M;
+        Frame f_tip;
+
+    public:
+        /**
+         * Constructor of the segment
+         *
+         * @param joint joint of the segment, default:
+         * Joint(Joint::None)
+         * @param f_tip frame from the end of the joint to the tip of
+         * the segment, default: Frame::Identity()
+         * @param M rigid body inertia of the segment, default: Inertia::Zero()
+         */
+        Segment(const Joint& joint=Joint(), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero());
+        Segment(const Segment& in);
+        Segment& operator=(const Segment& arg);
+
+        virtual ~Segment();
+
+        /**
+         * Request the pose of the segment, given the joint position q.
+         *
+         * @param q 1D position of the joint
+         *
+         * @return pose from the root to the tip of the segment
+         */
+        Frame pose(const double& q)const;
+        /**
+         * Request the 6D-velocity of the tip of the segment, given
+         * the joint position q and the joint velocity qdot.
+         *
+         * @param q ND position of the joint
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity of the tip of the segment, expressed
+         *in the base-frame of the segment(root) and with the tip of
+         *the segment as reference point.
+         */
+        Twist twist(const double& q,const double& qdot, int dof=0)const;
+
+        /**
+         * Request the 6D-velocity at a given point p, relative to base frame of the segment
+         * givven the joint velocity qdot.
+         *
+         * @param p reference point
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity at a given point p, expressed
+         * in the base-frame of the segment(root) 
+         */
+	    Twist twist(const Vector& p, const double& qdot, int dof=0)const;
+
+        /**
+         * Request the 6D-velocity at a given frame origin, relative to base frame of the segment
+         * assuming the frame rotation is the rotation of the joint.
+         *
+         * @param f joint pose frame + reference point
+         * @param qdot ND velocity of the joint
+         *
+         * @return 6D-velocity at frame reference point, expressed
+         * in the base-frame of the segment(root) 
+         */
+		Twist twist(const Frame& f, const double& qdot, int dof)const;
+
+        /**
+         * Request the joint of the segment
+         *
+         *
+         * @return const reference to the joint of the segment
+         */
+        const Joint& getJoint()const
+        {
+            return joint;
+        }
+        /**
+         * Request the inertia of the segment
+         *
+         *
+         * @return const reference to the inertia of the segment
+         */
+        const Inertia& getInertia()const
+        {
+            return M;
+        }
+
+        /**
+         * Request the pose from the joint end to the tip of the
+         *segment.
+         *
+         * @return const reference to the joint end - segment tip pose.
+         */
+        const Frame& getFrameToTip()const
+        {
+            return f_tip;
+        }
+
+    };
+}//end of namespace KDL
+
+#endif
diff --git a/intern/itasc/kdl/tree.cpp b/intern/itasc/kdl/tree.cpp
new file mode 100644
index 00000000000..f117e54959b
--- /dev/null
+++ b/intern/itasc/kdl/tree.cpp
@@ -0,0 +1,117 @@
+// 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
+
+#include "tree.hpp"
+#include <sstream>
+namespace KDL {
+using namespace std;
+
+Tree::Tree() :
+    nrOfJoints(0), nrOfSegments(0) {
+    segments.insert(make_pair("root", TreeElement::Root()));
+}
+
+Tree::Tree(const Tree& in) {
+    segments.clear();
+    nrOfSegments = 0;
+    nrOfJoints = 0;
+
+    segments.insert(make_pair("root", TreeElement::Root()));
+    this->addTree(in, "", "root");
+
+}
+
+Tree& Tree::operator=(const Tree& in) {
+    segments.clear();
+    nrOfSegments = 0;
+    nrOfJoints = 0;
+
+    segments.insert(make_pair("root", TreeElement::Root()));
+    this->addTree(in, "", "root");
+    return *this;
+}
+
+bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
+        const std::string& hook_name) {
+    SegmentMap::iterator parent = segments.find(hook_name);
+    //check if parent exists
+    if (parent == segments.end())
+        return false;
+    pair<SegmentMap::iterator, bool> retval;
+    //insert new element
+    retval = segments.insert(make_pair(segment_name, TreeElement(segment,
+            parent, nrOfJoints)));
+    //check if insertion succeeded
+    if (!retval.second)
+        return false;
+    //add iterator to new element in parents children list
+    parent->second.children.push_back(retval.first);
+    //increase number of segments
+    nrOfSegments++;
+    //increase number of joints
+	nrOfJoints += segment.getJoint().getNDof();
+    return true;
+}
+
+bool Tree::addChain(const Chain& chain, const std::string& chain_name,
+        const std::string& hook_name) {
+    string parent_name = hook_name;
+    for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
+        ostringstream segment_name;
+        segment_name << chain_name << "Segment" << i;
+        if (this->addSegment(chain.getSegment(i), segment_name.str(),
+                parent_name))
+            parent_name = segment_name.str();
+        else
+            return false;
+    }
+    return true;
+}
+
+bool Tree::addTree(const Tree& tree, const std::string& tree_name,
+        const std::string& hook_name) {
+    return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name);
+}
+
+bool Tree::addTreeRecursive(SegmentMap::const_iterator root,
+        const std::string& tree_name, const std::string& hook_name) {
+    //get iterator for root-segment
+    SegmentMap::const_iterator child;
+    //try to add all of root's children
+    for (unsigned int i = 0; i < root->second.children.size(); i++) {
+        child = root->second.children[i];
+        //Try to add the child
+        if (this->addSegment(child->second.segment, tree_name + child->first,
+                hook_name)) {
+            //if child is added, add all the child's children
+            if (!(this->addTreeRecursive(child, tree_name, tree_name
+                    + child->first)))
+                //if it didn't work, return false
+                return false;
+        } else
+            //If the child could not be added, return false
+            return false;
+    }
+    return true;
+}
+
+}
+
diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp
new file mode 100644
index 00000000000..bdd3aa94572
--- /dev/null
+++ b/intern/itasc/kdl/tree.hpp
@@ -0,0 +1,167 @@
+// 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
+
+#ifndef KDL_TREE_HPP
+#define KDL_TREE_HPP
+
+#include "segment.hpp"
+#include "chain.hpp"
+
+#include <string>
+#include <map>
+
+namespace KDL
+{
+    //Forward declaration
+    class TreeElement;
+    typedef std::map<std::string,TreeElement> SegmentMap;
+
+    class TreeElement
+    {
+    private:
+        TreeElement():q_nr(0)
+        {};
+    public:
+        Segment segment;
+        unsigned int q_nr;
+        SegmentMap::const_iterator  parent;
+        std::vector<SegmentMap::const_iterator > children;
+        TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in)
+        {
+			q_nr=q_nr_in;
+            segment=segment_in;
+            parent=parent_in;
+        };
+        static TreeElement Root()
+        {
+            return TreeElement();
+        };
+    };
+
+    /**
+     * \brief  This class encapsulates a <strong>tree</strong>
+     * kinematic interconnection structure. It is build out of segments.
+     *
+     * @ingroup KinematicFamily
+     */
+    class Tree
+    {
+    private:
+        SegmentMap segments;
+        unsigned int nrOfJoints;
+        unsigned int nrOfSegments;
+
+        bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name);
+
+    public:
+        /**
+         * The constructor of a tree, a new tree is always empty
+         */
+        Tree();
+        Tree(const Tree& in);
+        Tree& operator= (const Tree& arg);
+
+        /**
+         * Adds a new segment to the end of the segment with
+         * hook_name as segment_name
+         *
+         * @param segment new segment to add
+         * @param segment_name name of the new segment
+         * @param hook_name name of the segment to connect this
+         * segment with.
+         *
+         * @return false if hook_name could not be found.
+         */
+        bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name);
+
+        /**
+         * Adds a complete chain to the end of the segment with
+         * hook_name as segment_name. Segment i of
+         * the chain will get chain_name+".Segment"+i as segment_name.
+         *
+         * @param chain Chain to add
+         * @param chain_name name of the chain
+         * @param hook_name name of the segment to connect the chain with.
+         *
+         * @return false if hook_name could not be found.
+         */
+        bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name);
+
+        /**
+         * Adds a complete tree to the end of the segment with
+         * hookname as segment_name. The segments of the tree will get
+         * tree_name+segment_name as segment_name.
+         *
+         * @param tree Tree to add
+         * @param tree_name name of the tree
+         * @param hook_name name of the segment to connect the tree with
+         *
+         * @return false if hook_name could not be found
+         */
+        bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name);
+
+        /**
+         * Request the total number of joints in the tree.\n
+         * <strong> Important:</strong> It is not the same as the
+         * total number of segments since a segment does not need to have
+         * a joint.
+         *
+         * @return total nr of joints
+         */
+        unsigned int getNrOfJoints()const
+        {
+            return nrOfJoints;
+        };
+
+        /**
+         * Request the total number of segments in the tree.
+         * @return total number of segments
+         */
+        unsigned int getNrOfSegments()const {return nrOfSegments;};
+
+        /**
+         * Request the segment of the tree with name segment_name.
+         *
+         * @param segment_name the name of the requested segment
+         *
+         * @return constant iterator pointing to the requested segment
+         */
+        SegmentMap::const_iterator getSegment(const std::string& segment_name)const
+        {
+            return segments.find(segment_name);
+        };
+
+
+
+        const SegmentMap& getSegments()const
+        {
+            return segments;
+        }
+
+        virtual ~Tree(){};
+    };
+}
+#endif
+
+
+
+
+
diff --git a/intern/itasc/kdl/treefksolver.hpp b/intern/itasc/kdl/treefksolver.hpp
new file mode 100644
index 00000000000..22d5400ab0a
--- /dev/null
+++ b/intern/itasc/kdl/treefksolver.hpp
@@ -0,0 +1,110 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008  Julia Jesse
+
+// 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
+
+#ifndef KDL_TREE_FKSOLVER_HPP
+#define KDL_TREE_FKSOLVER_HPP
+
+#include <string>
+
+#include "tree.hpp"
+//#include "framevel.hpp"
+//#include "frameacc.hpp"
+#include "jntarray.hpp"
+//#include "jntarrayvel.hpp"
+//#include "jntarrayacc.hpp"
+
+namespace KDL {
+
+    /**
+	  * \brief This <strong>abstract</strong> class encapsulates a
+	  * solver for the forward position kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+
+    //Forward definition
+    class TreeFkSolverPos {
+    public:
+        /**
+         * Calculate forward position kinematics for a KDL::Tree,
+         * from joint coordinates to cartesian pose.
+         *
+         * @param q_in input joint coordinates
+         * @param p_out reference to output cartesian pose
+         *
+         * @return if < 0 something went wrong
+         */
+        virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0;
+        virtual ~TreeFkSolverPos(){};
+    };
+
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward velocity kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+//    class TreeFkSolverVel {
+//    public:
+        /**
+         * Calculate forward position and velocity kinematics, from
+         * joint coordinates to cartesian coordinates.
+         *
+         * @param q_in input joint coordinates (position and velocity)
+         * @param out output cartesian coordinates (position and velocity)
+         *
+         * @return if < 0 something went wrong
+         */
+//        virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
+
+//        virtual ~TreeFkSolverVel(){};
+//    };
+    
+    /**
+     * \brief This <strong>abstract</strong> class encapsulates a solver
+     * for the forward acceleration kinematics for a KDL::Tree.
+     *
+     * @ingroup KinematicFamily
+     */
+
+//    class TreeFkSolverAcc {
+//   public:
+        /**
+         * Calculate forward position, velocity and accelaration
+         * kinematics, from joint coordinates to cartesian coordinates
+         *
+         * @param q_in input joint coordinates (position, velocity and
+         * acceleration
+         @param out output cartesian coordinates (position, velocity
+         * and acceleration
+         *
+         * @return if < 0 something went wrong
+         */
+//    virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
+
+//        virtual ~TreeFkSolverAcc()=0;
+//    };
+
+
+}//end of namespace KDL
+
+#endif
diff --git a/intern/itasc/kdl/treefksolverpos_recursive.cpp b/intern/itasc/kdl/treefksolverpos_recursive.cpp
new file mode 100644
index 00000000000..8c54906dcf4
--- /dev/null
+++ b/intern/itasc/kdl/treefksolverpos_recursive.cpp
@@ -0,0 +1,71 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008 Julia Jesse
+
+// 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
+
+#include "treefksolverpos_recursive.hpp"
+#include <iostream>
+
+namespace KDL {
+
+    TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree):
+        tree(_tree)
+    {
+    }
+
+    int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
+    {      
+		SegmentMap::const_iterator it = tree.getSegment(segmentName); 
+		SegmentMap::const_iterator baseit = tree.getSegment(baseName); 
+        
+        if(q_in.rows() != tree.getNrOfJoints())
+    	    	return -1;
+        else if(it == tree.getSegments().end()) //if the segment name is not found
+         	return -2;
+        else if(baseit == tree.getSegments().end()) //if the base segment name is not found
+         	return -3;
+        else{
+        	const TreeElement& currentElement = it->second;
+			p_out = recursiveFk(q_in, it, baseit);	
+        	return 0;        	
+        }
+    }
+
+	Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
+	{
+		//gets the frame for the current element (segment)
+		const TreeElement& currentElement = it->second;
+		
+		if(it == baseit){
+			return KDL::Frame::Identity();
+		}
+		else{
+			Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
+			SegmentMap::const_iterator parentIt = currentElement.parent;
+			return recursiveFk(q_in, parentIt, baseit) * currentFrame;
+		}
+	}
+
+    TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive()
+    {
+    }
+
+
+}
diff --git a/intern/itasc/kdl/treefksolverpos_recursive.hpp b/intern/itasc/kdl/treefksolverpos_recursive.hpp
new file mode 100644
index 00000000000..c22fe4af75b
--- /dev/null
+++ b/intern/itasc/kdl/treefksolverpos_recursive.hpp
@@ -0,0 +1,53 @@
+// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2008 Julia Jesse
+
+// 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
+
+#ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP
+#define KDLTREEFKSOLVERPOS_RECURSIVE_HPP
+
+#include "treefksolver.hpp"
+
+namespace KDL {
+
+    /**
+     * Implementation of a recursive forward position kinematics
+     * algorithm to calculate the position transformation from joint
+     * space to Cartesian space of a general kinematic tree (KDL::Tree).
+     *
+     * @ingroup KinematicFamily
+     */
+    class TreeFkSolverPos_recursive : public TreeFkSolverPos
+    {
+    public:
+        TreeFkSolverPos_recursive(const Tree& tree);
+        ~TreeFkSolverPos_recursive();
+
+		virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName);
+
+    private:
+        const Tree tree;
+        
+        Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit);
+    };
+
+}
+
+#endif
diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp
new file mode 100644
index 00000000000..194f18eb959
--- /dev/null
+++ b/intern/itasc/kdl/treejnttojacsolver.cpp
@@ -0,0 +1,78 @@
+/*
+ * TreeJntToJacSolver.cpp
+ *
+ *  Created on: Nov 27, 2008
+ *      Author: rubensmits
+ */
+
+#include "treejnttojacsolver.hpp"
+#include <iostream>
+
+namespace KDL {
+
+TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) :
+    tree(tree_in) {
+}
+
+TreeJntToJacSolver::~TreeJntToJacSolver() {
+}
+
+int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
+        const std::string& segmentname) {
+    //First we check all the sizes:
+    if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
+            != tree.getNrOfJoints())
+        return -1;
+
+    //Lets search the tree-element
+    SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
+
+    //If segmentname is not inside the tree, back out:
+    if (it == tree.getSegments().end())
+        return -2;
+
+    //Let's make the jacobian zero:
+    SetToZero(jac);
+
+    SegmentMap::const_iterator root = tree.getSegments().find("root");
+
+    Frame T_total = Frame::Identity();
+	Frame T_local, T_joint;
+	Twist t_local;
+    //Lets recursively iterate until we are in the root segment
+    while (it != root) {
+        //get the corresponding q_nr for this TreeElement:
+        unsigned int q_nr = it->second.q_nr;
+
+        //get the pose of the joint.
+		T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
+		// combine with the tip to have the tip pose
+		T_local = T_joint*it->second.segment.getFrameToTip();
+        //calculate new T_end:
+        T_total = T_local * T_total;
+
+        //get the twist of the segment:
+		int ndof = it->second.segment.getJoint().getNDof();
+		for (int dof=0; dof<ndof; dof++) {
+			// combine joint rotation with tip position to get a reference frame for the joint
+			T_joint.p = T_local.p;
+			// in which the twist can be computed (needed for NDof joint)
+            t_local = it->second.segment.twist(T_joint, 1.0, dof);
+            //transform the endpoint of the local twist to the global endpoint:
+            t_local = t_local.RefPoint(T_total.p - T_local.p);
+            //transform the base of the twist to the endpoint
+            t_local = T_total.M.Inverse(t_local);
+            //store the twist in the jacobian:
+            jac.twists[q_nr+dof] = t_local;
+        }
+        //goto the parent
+        it = it->second.parent;
+    }//endwhile
+    //Change the base of the complete jacobian from the endpoint to the base
+    changeBase(jac, T_total.M, jac);
+
+    return 0;
+
+}//end JntToJac
+}//end namespace
+
diff --git a/intern/itasc/kdl/treejnttojacsolver.hpp b/intern/itasc/kdl/treejnttojacsolver.hpp
new file mode 100644
index 00000000000..40977dcd577
--- /dev/null
+++ b/intern/itasc/kdl/treejnttojacsolver.hpp
@@ -0,0 +1,38 @@
+/*
+ * TreeJntToJacSolver.hpp
+ *
+ *  Created on: Nov 27, 2008
+ *      Author: rubensmits
+ */
+
+#ifndef TREEJNTTOJACSOLVER_HPP_
+#define TREEJNTTOJACSOLVER_HPP_
+
+#include "tree.hpp"
+#include "jacobian.hpp"
+#include "jntarray.hpp"
+
+namespace KDL {
+
+class TreeJntToJacSolver {
+public:
+    TreeJntToJacSolver(const Tree& tree);
+
+    virtual ~TreeJntToJacSolver();
+
+    /*
+     * Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root.
+     * The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment
+     */
+
+    int JntToJac(const JntArray& q_in, Jacobian& jac,
+            const std::string& segmentname);
+
+private:
+    KDL::Tree tree;
+
+};
+
+}//End of namespace
+
+#endif /* TREEJNTTOJACSOLVER_H_ */
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
diff --git a/intern/itasc/make/msvc_9_0/itasc.vcproj b/intern/itasc/make/msvc_9_0/itasc.vcproj
new file mode 100644
index 00000000000..f4a81079da0
--- /dev/null
+++ b/intern/itasc/make/msvc_9_0/itasc.vcproj
@@ -0,0 +1,539 @@
+<?xml version="1.0" encoding="Windows-1252"?>

+<VisualStudioProject

+	ProjectType="Visual C++"

+	Version="9,00"

+	Name="INT_itasc"

+	ProjectGUID="{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}"

+	RootNamespace="itasc"

+	Keyword="Win32Proj"

+	TargetFrameworkVersion="196613"

+	>

+	<Platforms>

+		<Platform

+			Name="Win32"

+		/>

+	</Platforms>

+	<ToolFiles>

+	</ToolFiles>

+	<Configurations>

+		<Configuration

+			Name="Blender Debug|Win32"

+			OutputDirectory="..\..\..\..\..\build\msvc_9\intern\itasc\debug"

+			IntermediateDirectory="..\..\..\..\..\build\msvc_9\intern\itasc\debug"

+			ConfigurationType="4"

+			InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"

+			UseOfMFC="0"

+			ATLMinimizesCRunTimeLibraryUsage="false"

+			CharacterSet="2"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				Optimization="0"

+				AdditionalIncludeDirectories="..\..\..\..\extern\Eigen2"

+				PreprocessorDefinitions="WIN32;_DEBUG;_LIB"

+				MinimalRebuild="false"

+				BasicRuntimeChecks="3"

+				RuntimeLibrary="1"

+				EnableEnhancedInstructionSet="0"

+				FloatingPointModel="0"

+				UsePrecompiledHeader="0"

+				PrecompiledHeaderFile="..\..\..\..\..\build\msvc_9\intern\itasc\debug\itasc.pch"

+				AssemblerListingLocation="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"

+				ObjectFile="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"

+				ProgramDataBaseFileName="..\..\..\..\..\build\msvc_9\intern\itasc\debug\"

+				WarningLevel="3"

+				DebugInformationFormat="3"

+				CompileAs="0"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLibrarianTool"

+				OutputFile="..\..\..\..\..\build\msvc_9\libs\intern\debug\libitasc.lib"

+				SuppressStartupBanner="true"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+		<Configuration

+			Name="Blender Release|Win32"

+			OutputDirectory="..\..\..\..\..\build\msvc_9\intern\itasc"

+			IntermediateDirectory="..\..\..\..\..\build\msvc_9\intern\itasc"

+			ConfigurationType="4"

+			InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"

+			UseOfMFC="0"

+			ATLMinimizesCRunTimeLibraryUsage="false"

+			CharacterSet="2"

+			WholeProgramOptimization="1"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				Optimization="2"

+				EnableIntrinsicFunctions="true"

+				AdditionalIncludeDirectories="..\..\..\..\extern\Eigen2"

+				PreprocessorDefinitions="WIN32;NDEBUG;_LIB"

+				RuntimeLibrary="0"

+				EnableFunctionLevelLinking="true"

+				EnableEnhancedInstructionSet="0"

+				FloatingPointModel="2"

+				UsePrecompiledHeader="0"

+				AssemblerListingLocation="$(IntDir)\"

+				WarningLevel="3"

+				CompileAs="0"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLibrarianTool"

+				OutputFile="..\..\..\..\..\build\msvc_9\libs\intern\libitasc.lib"

+				SuppressStartupBanner="true"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+	</Configurations>

+	<References>

+	</References>

+	<Files>

+		<Filter

+			Name="kdl"

+			>

+			<Filter

+				Name="Header Files"

+				>

+				<File

+					RelativePath="..\..\kdl\chain.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\chainfksolver.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\chainfksolverpos_recursive.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\chainjnttojacsolver.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frameacc.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frameacc.inl"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frames.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frames.inl"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frames_io.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\framevel.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\framevel.inl"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\inertia.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jacobian.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarray.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarrayacc.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarrayvel.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\joint.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\kinfam_io.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\segment.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\tree.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\treefksolver.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\treefksolverpos_recursive.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\treejnttojacsolver.hpp"

+					>

+				</File>

+				<Filter

+					Name="Utilities"

+					>

+					<File

+						RelativePath="..\..\kdl\utilities\error.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\error_stack.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\kdl-config.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\rall1d.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\rall2d.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\svd_eigen_HH.hpp"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\traits.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\utility.h"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\utility_io.h"

+						>

+					</File>

+				</Filter>

+			</Filter>

+			<Filter

+				Name="Source Files"

+				>

+				<File

+					RelativePath="..\..\kdl\chain.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\chainfksolverpos_recursive.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\chainjnttojacsolver.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frameacc.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frames.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\frames_io.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\framevel.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\inertia.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jacobian.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarray.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarrayacc.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\jntarrayvel.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\joint.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\kinfam_io.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\segment.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\tree.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\treefksolverpos_recursive.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\kdl\treejnttojacsolver.cpp"

+					>

+				</File>

+				<Filter

+					Name="Utilities"

+					>

+					<File

+						RelativePath="..\..\kdl\utilities\error_stack.cpp"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\utility.cpp"

+						>

+					</File>

+					<File

+						RelativePath="..\..\kdl\utilities\utility_io.cpp"

+						>

+					</File>

+				</Filter>

+			</Filter>

+		</Filter>

+		<Filter

+			Name="itasc"

+			>

+			<Filter

+				Name="Header Files"

+				Filter="h;hpp;hxx;hm;inl;inc;xsd"

+				UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"

+				>

+				<File

+					RelativePath="..\..\Armature.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Cache.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\ConstraintSet.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\ControlledObject.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\CopyPose.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Distance.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\eigen_types.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\FixedObject.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\MovingFrame.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Object.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Scene.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Solver.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\UncontrolledObject.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WDLSSolver.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WorldObject.hpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WSDLSSolver.hpp"

+					>

+				</File>

+			</Filter>

+			<Filter

+				Name="Source Files"

+				Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx"

+				UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"

+				>

+				<File

+					RelativePath="..\..\Armature.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Cache.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\ConstraintSet.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\ControlledObject.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\CopyPose.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Distance.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\eigen_types.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\FixedObject.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\MovingFrame.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\Scene.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\UncontrolledObject.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WDLSSolver.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WorldObject.cpp"

+					>

+				</File>

+				<File

+					RelativePath="..\..\WSDLSSolver.cpp"

+					>

+				</File>

+			</Filter>

+		</Filter>

+	</Files>

+	<Globals>

+	</Globals>

+</VisualStudioProject>

diff --git a/intern/itasc/ublas_types.hpp b/intern/itasc/ublas_types.hpp
new file mode 100644
index 00000000000..bf9bdcc26f2
--- /dev/null
+++ b/intern/itasc/ublas_types.hpp
@@ -0,0 +1,82 @@
+/*
+ * ublas_types.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef UBLAS_TYPES_HPP_
+#define UBLAS_TYPES_HPP_
+
+#include <boost/numeric/ublas/matrix.hpp>
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/matrix_proxy.hpp>
+#include <boost/numeric/ublas/vector_proxy.hpp>
+#include "kdl/frames.hpp"
+#include "kdl/tree.hpp"
+#include "kdl/chain.hpp"
+#include "kdl/jacobian.hpp"
+#include "kdl/jntarray.hpp"
+
+
+namespace iTaSC{
+
+namespace ublas = boost::numeric::ublas;
+using KDL::Twist;
+using KDL::Frame;
+using KDL::Joint;
+using KDL::Inertia;
+using KDL::SegmentMap;
+using KDL::Tree;
+using KDL::JntArray;
+using KDL::Jacobian;
+using KDL::Segment;
+using KDL::Rotation;
+using KDL::Vector;
+using KDL::Chain;
+
+#define u_scalar double
+#define u_vector ublas::vector<u_scalar>
+#define u_zero_vector ublas::zero_vector<u_scalar>
+#define u_matrix ublas::matrix<u_scalar>
+#define u_matrix6 ublas::matrix<u_scalar,6,6>
+#define u_identity_matrix ublas::identity_matrix<u_scalar>
+#define u_scalar_vector ublas::scalar_vector<u_scalar>
+#define u_zero_matrix ublas::zero_matrix<u_scalar>
+#define u_vector6 ublas::bounded_vector<u_scalar,6>
+
+inline static int changeBase(const u_matrix& J_in, const Frame& T, u_matrix& J_out) {
+
+    if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
+        return -1;
+    for (unsigned int j = 0; j < J_in.size2(); ++j) {
+        ublas::matrix_column<const u_matrix > Jj_in = column(J_in,j);
+        ublas::matrix_column<u_matrix > Jj_out = column(J_out,j);
+		Twist arg;
+        for(unsigned int i=0;i<6;++i)
+            arg(i)=Jj_in(i);
+		Twist tmp(T*arg);
+        for(unsigned int i=0;i<6;++i)
+                    Jj_out(i)=tmp(i);
+    }
+    return 0;
+}
+inline static int changeBase(const ublas::matrix_range<u_matrix >& J_in, const Frame& T, ublas::matrix_range<u_matrix >& J_out) {
+
+    if (J_out.size1() != 6 || J_in.size1() != 6 || J_in.size2() != J_out.size2())
+        return -1;
+    for (unsigned int j = 0; j < J_in.size2(); ++j) {
+        ublas::matrix_column<const ublas::matrix_range<u_matrix > > Jj_in = column(J_in,j);
+        ublas::matrix_column<ublas::matrix_range<u_matrix > > Jj_out = column(J_out,j);
+		Twist arg;
+        for(unsigned int i=0;i<6;++i)
+            arg(i)=Jj_in(i);
+		Twist tmp(T*arg);
+        for(unsigned int i=0;i<6;++i)
+                    Jj_out(i)=tmp(i);
+    }
+    return 0;
+}
+
+}
+#endif /* UBLAS_TYPES_HPP_ */
diff --git a/intern/memutil/make/msvc_9_0/memutil.vcproj b/intern/memutil/make/msvc_9_0/memutil.vcproj
index 6f642fb16bc..0b8251f0d7e 100644
--- a/intern/memutil/make/msvc_9_0/memutil.vcproj
+++ b/intern/memutil/make/msvc_9_0/memutil.vcproj
@@ -119,6 +119,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\..\..\intern;..\..\..\memutil"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/moto/make/msvc_9_0/moto.vcproj b/intern/moto/make/msvc_9_0/moto.vcproj
index b33bb165a75..34c5705e2f2 100644
--- a/intern/moto/make/msvc_9_0/moto.vcproj
+++ b/intern/moto/make/msvc_9_0/moto.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\include\"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/smoke/intern/WTURBULENCE.cpp b/intern/smoke/intern/WTURBULENCE.cpp
index a1b2aaf30f2..bcfc61856af 100644
--- a/intern/smoke/intern/WTURBULENCE.cpp
+++ b/intern/smoke/intern/WTURBULENCE.cpp
@@ -986,4 +986,3 @@ void WTURBULENCE::stepTurbulenceFull(float dtOrg, float* xvel, float* yvel, floa
   
   _totalStepsBig++;
 }
-
diff --git a/intern/smoke/make/msvc_9_0/smoke.vcproj b/intern/smoke/make/msvc_9_0/smoke.vcproj
index aa3779031f0..38a761d5d82 100644
--- a/intern/smoke/make/msvc_9_0/smoke.vcproj
+++ b/intern/smoke/make/msvc_9_0/smoke.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="2"

 				AdditionalIncludeDirectories="..\..\intern;..\..\..\..\..\lib\windows\zlib\include;..\..\..\..\..\lib\windows\png\include;..\..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\..\build\msvc_9\intern\guardedalloc\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/intern/string/make/msvc_9_0/string.vcproj b/intern/string/make/msvc_9_0/string.vcproj
index 16df974ff9c..512d67623b6 100644
--- a/intern/string/make/msvc_9_0/string.vcproj
+++ b/intern/string/make/msvc_9_0/string.vcproj
@@ -118,6 +118,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\.."

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/blender/BLO_readblenfile/BLO_readblenfile.vcproj b/projectfiles_vc9/blender/BLO_readblenfile/BLO_readblenfile.vcproj
index 07afe8c5b30..4d3b086cc7a 100644
--- a/projectfiles_vc9/blender/BLO_readblenfile/BLO_readblenfile.vcproj
+++ b/projectfiles_vc9/blender/BLO_readblenfile/BLO_readblenfile.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="BLO_readblenfile"

 	ProjectGUID="{DB6BE55D-B6D9-494D-856A-8764FF7BA91D}"

+	RootNamespace="BLO_readblenfile"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -264,6 +265,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\zlib\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\inflate;..\..\..\source\blender\deflate;..\..\..\source\blender\blenloader;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenkernel;..\..\..\source\blender\readblenfile;..\..\..\source\blender\readstreamglue;..\..\..\source\kernel\gen_messaging"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/blender/BPY_python/BPY_python.vcproj b/projectfiles_vc9/blender/BPY_python/BPY_python.vcproj
index 7f1eff21e6f..6137696fc59 100644
--- a/projectfiles_vc9/blender/BPY_python/BPY_python.vcproj
+++ b/projectfiles_vc9/blender/BPY_python/BPY_python.vcproj
@@ -119,6 +119,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\img;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesrna;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenloader;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\source\blender\editors\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/blender/avi/BL_avi.vcproj b/projectfiles_vc9/blender/avi/BL_avi.vcproj
index f9700ab85c2..e45cb1944a2 100644
--- a/projectfiles_vc9/blender/avi/BL_avi.vcproj
+++ b/projectfiles_vc9/blender/avi/BL_avi.vcproj
@@ -192,6 +192,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\jpeg\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\avi;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\makesdna;..\..\..\source\blender\imbuf\intern\openexr"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,DWORDS_LITTLEENDIAN"

diff --git a/projectfiles_vc9/blender/blender.sln b/projectfiles_vc9/blender/blender.sln
index 1bd132ea223..d7c16229049 100644
--- a/projectfiles_vc9/blender/blender.sln
+++ b/projectfiles_vc9/blender/blender.sln
@@ -2,6 +2,7 @@ Microsoft Visual Studio Solution File, Format Version 10.00
 # Visual Studio 2008

 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "blender", "blender.vcproj", "{F78B7FC9-DE32-465E-9F26-BB0B6B7A2EAF}"

 	ProjectSection(ProjectDependencies) = postProject

+		{02110D03-59DB-4571-8787-72B3C03B2F2D} = {02110D03-59DB-4571-8787-72B3C03B2F2D}

 		{E5F2F004-C704-4DCC-A08F-6EB1E38EAB9F} = {E5F2F004-C704-4DCC-A08F-6EB1E38EAB9F}

 		{6E24BF09-9653-4166-A871-F65CC9E98A9B} = {6E24BF09-9653-4166-A871-F65CC9E98A9B}

 		{A90C4918-4B21-4277-93BD-AF65F30951D9} = {A90C4918-4B21-4277-93BD-AF65F30951D9}

@@ -17,6 +18,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "blender", "blender.vcproj",
 		{FFD3C64A-30E2-4BC7-BC8F-51818C320400} = {FFD3C64A-30E2-4BC7-BC8F-51818C320400}

 		{31628053-825D-4C06-8A21-D13883489718} = {31628053-825D-4C06-8A21-D13883489718}

 		{EADC3C5A-6C51-4F03-8038-1553E7D7F740} = {EADC3C5A-6C51-4F03-8038-1553E7D7F740}

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC} = {59567A5B-F63A-4A5C-B33A-0A45C300F4DC}

 		{B093415D-C0F6-4E76-8F5A-6BC1917BCE9E} = {B093415D-C0F6-4E76-8F5A-6BC1917BCE9E}

 		{DB6BE55D-B6D9-494D-856A-8764FF7BA91D} = {DB6BE55D-B6D9-494D-856A-8764FF7BA91D}

 		{0A73055E-4DED-40CD-9F72-9093ED3EEC7E} = {0A73055E-4DED-40CD-9F72-9093ED3EEC7E}

@@ -43,6 +45,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "blender", "blender.vcproj",
 		{E90C7BC2-CF30-4A60-A8F2-0050D592E358} = {E90C7BC2-CF30-4A60-A8F2-0050D592E358}

 		{8B8D4FC3-3234-4E54-8376-5AB83D00D164} = {8B8D4FC3-3234-4E54-8376-5AB83D00D164}

 		{4B6AFCC5-968C-424A-8F20-76E41B3BEF74} = {4B6AFCC5-968C-424A-8F20-76E41B3BEF74}

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3} = {9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}

 		{87032FD2-9BA0-6B43-BE33-8902BA8F9172} = {87032FD2-9BA0-6B43-BE33-8902BA8F9172}

 		{EB75F4D6-2970-4A3A-8D99-2BAD7201C0E9} = {EB75F4D6-2970-4A3A-8D99-2BAD7201C0E9}

 		{5A2EA6DC-1A53-4E87-9166-52870CE3B4EA} = {5A2EA6DC-1A53-4E87-9166-52870CE3B4EA}

@@ -136,6 +139,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "blenderplayer", "..\gameeng
 		{FFD3C64A-30E2-4BC7-BC8F-51818C320400} = {FFD3C64A-30E2-4BC7-BC8F-51818C320400}

 		{31628053-825D-4C06-8A21-D13883489718} = {31628053-825D-4C06-8A21-D13883489718}

 		{EADC3C5A-6C51-4F03-8038-1553E7D7F740} = {EADC3C5A-6C51-4F03-8038-1553E7D7F740}

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC} = {59567A5B-F63A-4A5C-B33A-0A45C300F4DC}

 		{DB6BE55D-B6D9-494D-856A-8764FF7BA91D} = {DB6BE55D-B6D9-494D-856A-8764FF7BA91D}

 		{0A73055E-4DED-40CD-9F72-9093ED3EEC7E} = {0A73055E-4DED-40CD-9F72-9093ED3EEC7E}

 		{09222F5E-1625-4FF3-A89A-384D16875EE5} = {09222F5E-1625-4FF3-A89A-384D16875EE5}

@@ -160,6 +164,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "blenderplayer", "..\gameeng
 		{E90C7BC2-CF30-4A60-A8F2-0050D592E358} = {E90C7BC2-CF30-4A60-A8F2-0050D592E358}

 		{8B8D4FC3-3234-4E54-8376-5AB83D00D164} = {8B8D4FC3-3234-4E54-8376-5AB83D00D164}

 		{4B6AFCC5-968C-424A-8F20-76E41B3BEF74} = {4B6AFCC5-968C-424A-8F20-76E41B3BEF74}

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3} = {9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}

 		{87032FD2-9BA0-6B43-BE33-8902BA8F9172} = {87032FD2-9BA0-6B43-BE33-8902BA8F9172}

 		{5A2EA6DC-1A53-4E87-9166-52870CE3B4EA} = {5A2EA6DC-1A53-4E87-9166-52870CE3B4EA}

 		{E86B7BDE-C33C-4E55-9433-E74C141D7538} = {E86B7BDE-C33C-4E55-9433-E74C141D7538}

@@ -225,6 +230,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "EXT_build_install_all", "..
 	ProjectSection(ProjectDependencies) = postProject

 		{79D0B232-208C-F208-DA71-79B4AC088602} = {79D0B232-208C-F208-DA71-79B4AC088602}

 		{FFD3C64A-30E2-4BC7-BC8F-51818C320400} = {FFD3C64A-30E2-4BC7-BC8F-51818C320400}

+		{EADC3C5A-6C51-4F03-8038-1553E7D7F740} = {EADC3C5A-6C51-4F03-8038-1553E7D7F740}

+		{BAC615B0-F1AF-418B-8D23-A10FD8870D6A} = {BAC615B0-F1AF-418B-8D23-A10FD8870D6A}

 		{8BFA4082-773B-D100-BC24-659083BA023F} = {8BFA4082-773B-D100-BC24-659083BA023F}

 		{BAC615B0-F1AF-418B-8D23-A10FD8870D6A} = {BAC615B0-F1AF-418B-8D23-A10FD8870D6A}

 	EndProjectSection

@@ -268,6 +275,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "INT_build_install_all", "..
 	ProjectSection(ProjectDependencies) = postProject

 		{A90C4918-4B21-4277-93BD-AF65F30951D9} = {A90C4918-4B21-4277-93BD-AF65F30951D9}

 		{C66F722C-46BE-40C9-ABAE-2EAC7A697EB8} = {C66F722C-46BE-40C9-ABAE-2EAC7A697EB8}

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC} = {59567A5B-F63A-4A5C-B33A-0A45C300F4DC}

 		{B093415D-C0F6-4E76-8F5A-6BC1917BCE9E} = {B093415D-C0F6-4E76-8F5A-6BC1917BCE9E}

 		{76D90B92-ECC7-409C-9F98-A8814B90F3C0} = {76D90B92-ECC7-409C-9F98-A8814B90F3C0}

 		{542A9FA1-B7FF-441C-AE15-054DB31D3488} = {542A9FA1-B7FF-441C-AE15-054DB31D3488}

@@ -292,6 +300,8 @@ EndProject
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RNA_makesrna", "makesrna\RNA_makesrna.vcproj", "{8C2BCCF8-4D9E-46D3-BABF-C1545A332CE6}"

 	ProjectSection(ProjectDependencies) = postProject

 		{7495FE37-933A-4AC1-BB2A-B3FDB4DE4284} = {7495FE37-933A-4AC1-BB2A-B3FDB4DE4284}

+		{31628053-825D-4C06-8A21-D13883489718} = {31628053-825D-4C06-8A21-D13883489718}

+		{1CC733F1-6AB5-4904-8F63-C08C46B79DD9} = {1CC733F1-6AB5-4904-8F63-C08C46B79DD9}

 	EndProjectSection

 EndProject

 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "RNA_rna", "makesrna\RNA_rna.vcproj", "{DFE7F3E3-E62A-4677-B666-DF0DDF70C359}"

@@ -306,6 +316,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DNA_dna", "makesdna\DNA_dna
 EndProject

 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BLF_blenfont", "blenfont\BLF_blenfont.vcproj", "{D1A9312F-4557-4982-A0F4-4D08508235F4}"

 EndProject

+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "INT_itasc", "..\..\intern\itasc\make\msvc_9_0\itasc.vcproj", "{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}"

+EndProject

+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BIK_ikplugin", "ikplugin\BIK_ikplugin.vcproj", "{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}"

+EndProject

 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "INT_smoke", "..\..\intern\smoke\make\msvc_9_0\smoke.vcproj", "{E8904FB3-F8F7-BC21-87A6-029A57B901F4}"

 EndProject

 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "INT_audaspace", "..\..\intern\audaspace\make\msvc_9_0\audaspace.vcproj", "{87032FD2-9BA0-6B43-BE33-8902BA8F9172}"

@@ -557,7 +571,6 @@ Global
 		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.BlenderPlayer Debug|Win32.ActiveCfg = 3D Plugin Debug|Win32

 		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.BlenderPlayer Release|Win32.ActiveCfg = 3D Plugin Release|Win32

 		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.Debug|Win32.ActiveCfg = 3D Plugin Debug|Win32

-		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.Debug|Win32.Build.0 = 3D Plugin Debug|Win32

 		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.Release|Win32.ActiveCfg = 3D Plugin Release|Win32

 		{DF25E6F2-780C-438B-8AAD-D10CF8B3820A}.Release|Win32.Build.0 = 3D Plugin Release|Win32

 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.3D Plugin Debug|Win32.ActiveCfg = 3D Plugin Debug|Win32

@@ -575,7 +588,6 @@ Global
 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.BlenderPlayer Release|Win32.ActiveCfg = BlenderPlayer Release|Win32

 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.BlenderPlayer Release|Win32.Build.0 = BlenderPlayer Release|Win32

 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.Debug|Win32.ActiveCfg = BlenderPlayer Debug|Win32

-		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.Debug|Win32.Build.0 = BlenderPlayer Debug|Win32

 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.Release|Win32.ActiveCfg = BlenderPlayer Release|Win32

 		{D8ABD6A5-1B36-4D62-934E-B5C6801130B0}.Release|Win32.Build.0 = BlenderPlayer Release|Win32

 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.3D Plugin Debug|Win32.ActiveCfg = BlenderPlayer Release|Win32

@@ -593,7 +605,6 @@ Global
 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.BlenderPlayer Release|Win32.ActiveCfg = BlenderPlayer Release|Win32

 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.BlenderPlayer Release|Win32.Build.0 = BlenderPlayer Release|Win32

 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.Debug|Win32.ActiveCfg = BlenderPlayer Debug|Win32

-		{3D310C60-6771-48E4-BCCA-D2718CDED898}.Debug|Win32.Build.0 = BlenderPlayer Debug|Win32

 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.Release|Win32.ActiveCfg = BlenderPlayer Release|Win32

 		{3D310C60-6771-48E4-BCCA-D2718CDED898}.Release|Win32.Build.0 = BlenderPlayer Release|Win32

 		{8154A59A-CAED-403D-AB94-BC4E7C032666}.3D Plugin Debug|Win32.ActiveCfg = Blender Release|Win32

@@ -1039,6 +1050,7 @@ Global
 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.3DPlugin Release|Win32.ActiveCfg = 3D Plugin Debug|Win32

 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.3DPlugin Release|Win32.Build.0 = 3D Plugin Debug|Win32

 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.Blender Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.Blender Debug|Win32.Build.0 = Blender Debug|Win32

 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.Blender Release|Win32.ActiveCfg = Blender Release|Win32

 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.BlenderPlayer Debug|Win32.ActiveCfg = Blender Debug|Win32

 		{9C71A793-C177-4CAB-8EC5-923D500B39F8}.BlenderPlayer Release|Win32.ActiveCfg = Blender Release|Win32

@@ -1289,6 +1301,7 @@ Global
 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.3DPlugin Release|Win32.ActiveCfg = 3DPlugin Release|Win32

 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.3DPlugin Release|Win32.Build.0 = 3DPlugin Release|Win32

 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.Blender Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{02110D03-59DB-4571-8787-72B3C03B2F2D}.Blender Debug|Win32.Build.0 = Blender Debug|Win32

 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.Blender Release|Win32.ActiveCfg = Blender Release|Win32

 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.BlenderPlayer Debug|Win32.ActiveCfg = Blender Debug|Win32

 		{02110D03-59DB-4571-8787-72B3C03B2F2D}.BlenderPlayer Release|Win32.ActiveCfg = Blender Release|Win32

@@ -1416,6 +1429,42 @@ Global
 		{D1A9312F-4557-4982-A0F4-4D08508235F4}.Debug|Win32.Build.0 = Blender Debug|Win32

 		{D1A9312F-4557-4982-A0F4-4D08508235F4}.Release|Win32.ActiveCfg = Blender Release|Win32

 		{D1A9312F-4557-4982-A0F4-4D08508235F4}.Release|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3D Plugin Debug|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3D Plugin Debug|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3D Plugin Release|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3D Plugin Release|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3DPlugin Debug|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.3DPlugin Release|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Blender Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Blender Debug|Win32.Build.0 = Blender Debug|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Blender Release|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Blender Release|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.BlenderPlayer Debug|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.BlenderPlayer Debug|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.BlenderPlayer Release|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.BlenderPlayer Release|Win32.Build.0 = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Debug|Win32.Build.0 = Blender Debug|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Release|Win32.ActiveCfg = Blender Release|Win32

+		{59567A5B-F63A-4A5C-B33A-0A45C300F4DC}.Release|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3D Plugin Debug|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3D Plugin Debug|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3D Plugin Release|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3D Plugin Release|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3DPlugin Debug|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.3DPlugin Release|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Blender Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Blender Debug|Win32.Build.0 = Blender Debug|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Blender Release|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Blender Release|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.BlenderPlayer Debug|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.BlenderPlayer Debug|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.BlenderPlayer Release|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.BlenderPlayer Release|Win32.Build.0 = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Debug|Win32.ActiveCfg = Blender Debug|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Debug|Win32.Build.0 = Blender Debug|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Release|Win32.ActiveCfg = Blender Release|Win32

+		{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}.Release|Win32.Build.0 = Blender Release|Win32

 		{E8904FB3-F8F7-BC21-87A6-029A57B901F4}.3D Plugin Debug|Win32.ActiveCfg = 3DPlugin Debug|Win32

 		{E8904FB3-F8F7-BC21-87A6-029A57B901F4}.3D Plugin Debug|Win32.Build.0 = 3DPlugin Debug|Win32

 		{E8904FB3-F8F7-BC21-87A6-029A57B901F4}.3D Plugin Release|Win32.ActiveCfg = 3DPlugin Debug|Win32

diff --git a/projectfiles_vc9/blender/blender.vcproj b/projectfiles_vc9/blender/blender.vcproj
index e6a5c483f7a..d6f29788f9d 100644
--- a/projectfiles_vc9/blender/blender.vcproj
+++ b/projectfiles_vc9/blender/blender.vcproj
@@ -43,6 +43,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\source\blender;..\..\source\blender\imbuf;..\..\source\blender\misc;..\..\source\blender\blenlib;..\..\source\blender\editors\include;..\..\source\blender\python;..\..\source\blender\windowmanager;..\..\source\blender\renderui;..\..\source\blender\makesdna;..\..\source\blender\makesrna;..\..\source\blender\blenkernel;..\..\source\blender\blenloader;..\..\source\blender\renderconverter;..\..\source\blender\render\extern\include;..\..\source\blender\radiosity\extern\include;..\..\source\kernel\gen_system;..\..\source\kernel\gen_messaging;..\..\..\build\msvc_9\extern\glew\include;..\..\source\blender\gpu"

 				PreprocessorDefinitions="NDEBUG;WIN32;_CONSOLE;WITH_QUICKTIME;GAMEBLENDER=1;USE_SUMO_SOLID;FTGL_LIBRARY_STATIC"

diff --git a/projectfiles_vc9/blender/blenfont/BLF_blenfont.vcproj b/projectfiles_vc9/blender/blenfont/BLF_blenfont.vcproj
index 7b92276d4b8..07cddc10fc5 100644
--- a/projectfiles_vc9/blender/blenfont/BLF_blenfont.vcproj
+++ b/projectfiles_vc9/blender/blenfont/BLF_blenfont.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\intern\guardedalloc;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenfont;..\..\..\source\blender\ftfont;..\..\..\..\lib\windows\freetype\include;..\..\..\source\blender\editors\include;..\..\..\..\build\msvc_9\extern\glew\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB;WITH_BF_INTERNATIONAL"

diff --git a/projectfiles_vc9/blender/blenkernel/BKE_blenkernel.vcproj b/projectfiles_vc9/blender/blenkernel/BKE_blenkernel.vcproj
index 87f10346726..3f17da1ddab 100644
--- a/projectfiles_vc9/blender/blenkernel/BKE_blenkernel.vcproj
+++ b/projectfiles_vc9/blender/blenkernel/BKE_blenkernel.vcproj
@@ -43,7 +43,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="_DEBUG,WIN32,_LIB"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="3"

@@ -118,7 +118,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="_DEBUG;WIN32;_LIB;WITH_OPENEXR;WITH_DDS;WITH_BULLET;WITH_FFMPEG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -192,8 +192,9 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;UNWRAPPER;WITH_OPENEXR;WITH_DDS;WITH_BULLET=1;WITH_FFMPEG"

 				StringPooling="true"

 				RuntimeLibrary="0"

@@ -268,7 +269,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

 				StringPooling="true"

 				RuntimeLibrary="2"

@@ -343,7 +344,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="_DEBUG;WIN32;_LIB;WITH_FFMPEG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -418,7 +419,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\extern\lzo\minilzo;..\..\..\extern\lzma;..\..\..\source\blender\blenfont;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;UNWRAPPER;WITH_FFMPEG"

 				StringPooling="true"

 				RuntimeLibrary="0"

diff --git a/projectfiles_vc9/blender/blenlib/BLI_blenlib.vcproj b/projectfiles_vc9/blender/blenlib/BLI_blenlib.vcproj
index 6d111ea4b4f..b883d8bf2b2 100644
--- a/projectfiles_vc9/blender/blenlib/BLI_blenlib.vcproj
+++ b/projectfiles_vc9/blender/blenlib/BLI_blenlib.vcproj
@@ -194,6 +194,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\freetype\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenkernel"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB"

diff --git a/projectfiles_vc9/blender/blenpluginapi/blenpluginapi/blenpluginapi.vcproj b/projectfiles_vc9/blender/blenpluginapi/blenpluginapi/blenpluginapi.vcproj
index 02ea370e34a..26493699f5d 100644
--- a/projectfiles_vc9/blender/blenpluginapi/blenpluginapi/blenpluginapi.vcproj
+++ b/projectfiles_vc9/blender/blenpluginapi/blenpluginapi/blenpluginapi.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="blenpluginapi"

 	ProjectGUID="{BB6AA598-B336-4F8B-9DF9-8CAE7BE71C23}"

+	RootNamespace="blenpluginapi"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -115,6 +116,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\source\blender;..\..\..\..\source\blender\blenlib;..\..\..\..\source\blender\imbuf;..\..\..\..\source\blender\makesdna;..\..\..\..\source\blender\blenpluginapi"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/blender/editors/ED_editors.vcproj b/projectfiles_vc9/blender/editors/ED_editors.vcproj
index e81e1a2e361..3d7a12435a3 100644
--- a/projectfiles_vc9/blender/editors/ED_editors.vcproj
+++ b/projectfiles_vc9/blender/editors/ED_editors.vcproj
@@ -42,8 +42,9 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\QTDevWin\CIncludes;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\python\include\python2.6;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\ghost\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\blenkey\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\source\blender;..\..\..\source\blender\img;..\..\..\source\blender\verify;..\..\..\source\blender\ftfont;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\editors\include;..\..\..\source\blender\renderui;..\..\..\source\blender\blenloader;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenfont;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\windowmanager;..\..\..\source\blender\blenpluginapi;..\..\..\source\blender\renderconverter;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\network;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\QTDevWin\CIncludes;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\python\include\python2.6;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\ghost\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\blenkey\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\source\blender;..\..\..\source\blender\img;..\..\..\source\blender\verify;..\..\..\source\blender\ftfont;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\editors\include;..\..\..\source\blender\renderui;..\..\..\source\blender\blenloader;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenfont;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\windowmanager;..\..\..\source\blender\blenpluginapi;..\..\..\source\blender\renderconverter;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\network;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;_CONSOLE;GAMEBLENDER=1;WITH_QUICKTIME;INTERNATIONAL;WITH_FREETYPE2;WITH_INTERNATIONAL;WITH_OPENEXR;WITH_DDS;WITH_BULLET=1;WITH_FFMPEG"

 				StringPooling="true"

 				RuntimeLibrary="0"

@@ -118,7 +119,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\lib\windows\QTDevWin\CIncludes;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\python\include\python2.6;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\ghost\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\blenkey\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\source\blender;..\..\..\source\blender\img;..\..\..\source\blender\verify;..\..\..\source\blender\ftfont;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\editors\include;..\..\..\source\blender\renderui;..\..\..\source\blender\blenloader;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenfont;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\windowmanager;..\..\..\source\blender\blenpluginapi;..\..\..\source\blender\renderconverter;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\network;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\lib\windows\QTDevWin\CIncludes;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\python\include\python2.6;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\ghost\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\blenkey\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\source\blender;..\..\..\source\blender\img;..\..\..\source\blender\verify;..\..\..\source\blender\ftfont;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\editors\include;..\..\..\source\blender\renderui;..\..\..\source\blender\blenloader;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenfont;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\nodes;..\..\..\source\blender\windowmanager;..\..\..\source\blender\blenpluginapi;..\..\..\source\blender\renderconverter;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\network;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\lib\windows\pthreads\include;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="_DEBUG;WIN32;_LIB;_CONSOLE;GAMEBLENDER=1;WITH_QUICKTIME;INTERNATIONAL;WITH_BF_INTERNATIONAL;WITH_FREETYPE2;WITH_OPENEXR;WITH_DDS;WITH_BULLET = 1;WITH_FFMPEG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

diff --git a/projectfiles_vc9/blender/gpu/BL_gpu.vcproj b/projectfiles_vc9/blender/gpu/BL_gpu.vcproj
index 1daf345f638..db96ab440b9 100644
--- a/projectfiles_vc9/blender/gpu/BL_gpu.vcproj
+++ b/projectfiles_vc9/blender/gpu/BL_gpu.vcproj
@@ -192,6 +192,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\gpu"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,DWORDS_LITTLEENDIAN"

diff --git a/projectfiles_vc9/blender/ikplugin/BIK_ikplugin.vcproj b/projectfiles_vc9/blender/ikplugin/BIK_ikplugin.vcproj
new file mode 100644
index 00000000000..48693942798
--- /dev/null
+++ b/projectfiles_vc9/blender/ikplugin/BIK_ikplugin.vcproj
@@ -0,0 +1,214 @@
+<?xml version="1.0" encoding="Windows-1252"?>

+<VisualStudioProject

+	ProjectType="Visual C++"

+	Version="9,00"

+	Name="BIK_ikplugin"

+	ProjectGUID="{9951A8C9-84FE-4CFE-9E18-9D01CB8E09F3}"

+	RootNamespace="BRE_ikplugin"

+	TargetFrameworkVersion="131072"

+	>

+	<Platforms>

+		<Platform

+			Name="Win32"

+		/>

+	</Platforms>

+	<ToolFiles>

+	</ToolFiles>

+	<Configurations>

+		<Configuration

+			Name="Blender Debug|Win32"

+			OutputDirectory="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug"

+			IntermediateDirectory="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug"

+			ConfigurationType="4"

+			InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"

+			UseOfMFC="0"

+			ATLMinimizesCRunTimeLibraryUsage="false"

+			CharacterSet="2"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				Optimization="0"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\ikplugin;..\..\..\intern\itasc;..\..\..\extern\Eigen2"

+				PreprocessorDefinitions="WIN32,_DEBUG,_LIB"

+				BasicRuntimeChecks="3"

+				RuntimeLibrary="1"

+				DefaultCharIsUnsigned="true"

+				UsePrecompiledHeader="0"

+				PrecompiledHeaderFile="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug\BRE_yafray.pch"

+				AssemblerListingLocation="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug\"

+				ObjectFile="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug\"

+				ProgramDataBaseFileName="..\..\..\..\build\msvc_9\source\blender\ikplugin\debug\"

+				WarningLevel="2"

+				SuppressStartupBanner="true"

+				DebugInformationFormat="3"

+				CompileAs="0"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+				PreprocessorDefinitions="_DEBUG"

+				Culture="1043"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLibrarianTool"

+				OutputFile="..\..\..\..\build\msvc_9\libs\debug\BRE_ikplugin.lib"

+				SuppressStartupBanner="true"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+		<Configuration

+			Name="Blender Release|Win32"

+			OutputDirectory="..\..\..\..\build\msvc_9\source\blender\ikplugin"

+			IntermediateDirectory="..\..\..\..\build\msvc_9\source\blender\ikplugin"

+			ConfigurationType="4"

+			InheritedPropertySheets="$(VCInstallDir)VCProjectDefaults\UpgradeFromVC71.vsprops"

+			UseOfMFC="0"

+			ATLMinimizesCRunTimeLibraryUsage="false"

+			CharacterSet="2"

+			>

+			<Tool

+				Name="VCPreBuildEventTool"

+			/>

+			<Tool

+				Name="VCCustomBuildTool"

+			/>

+			<Tool

+				Name="VCXMLDataGeneratorTool"

+			/>

+			<Tool

+				Name="VCWebServiceProxyGeneratorTool"

+			/>

+			<Tool

+				Name="VCMIDLTool"

+			/>

+			<Tool

+				Name="VCCLCompilerTool"

+				Optimization="2"

+				InlineFunctionExpansion="1"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\ikplugin;..\..\..\intern\itasc;..\..\..\extern\Eigen2"

+				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

+				StringPooling="true"

+				RuntimeLibrary="0"

+				EnableFunctionLevelLinking="true"

+				DefaultCharIsUnsigned="true"

+				UsePrecompiledHeader="0"

+				PrecompiledHeaderFile="..\..\..\..\build\msvc_9\source\blender\yafray\BIK_ikplugin.pch"

+				AssemblerListingLocation="..\..\..\..\build\msvc_9\source\blender\ikplugin\"

+				ObjectFile="..\..\..\..\build\msvc_9\source\blender\ikplugin\"

+				ProgramDataBaseFileName="..\..\..\..\build\msvc_9\source\blender\ikplugin\"

+				WarningLevel="2"

+				SuppressStartupBanner="true"

+				CompileAs="0"

+			/>

+			<Tool

+				Name="VCManagedResourceCompilerTool"

+			/>

+			<Tool

+				Name="VCResourceCompilerTool"

+				PreprocessorDefinitions="NDEBUG"

+				Culture="1043"

+			/>

+			<Tool

+				Name="VCPreLinkEventTool"

+			/>

+			<Tool

+				Name="VCLibrarianTool"

+				OutputFile="..\..\..\..\build\msvc_9\libs\BIK_ikplugin.lib"

+				SuppressStartupBanner="true"

+			/>

+			<Tool

+				Name="VCALinkTool"

+			/>

+			<Tool

+				Name="VCXDCMakeTool"

+			/>

+			<Tool

+				Name="VCBscMakeTool"

+			/>

+			<Tool

+				Name="VCFxCopTool"

+			/>

+			<Tool

+				Name="VCPostBuildEventTool"

+			/>

+		</Configuration>

+	</Configurations>

+	<References>

+	</References>

+	<Files>

+		<Filter

+			Name="Source Files"

+			Filter="cpp;c;cxx;rc;def;r;odl;idl;hpj;bat"

+			>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\ikplugin_api.c"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\iksolver_plugin.c"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\itasc_plugin.cpp"

+				>

+			</File>

+		</Filter>

+		<Filter

+			Name="Header Files"

+			Filter="h;hpp;hxx;hm;inl"

+			>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\ikplugin_api.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\iksolver_plugin.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\ikplugin\intern\itasc_plugin.h"

+				>

+			</File>

+		</Filter>

+		<File

+			RelativePath="..\..\..\source\blender\ikplugin\BIK_api.h"

+			>

+		</File>

+	</Files>

+	<Globals>

+	</Globals>

+</VisualStudioProject>

diff --git a/projectfiles_vc9/blender/imbuf/BL_imbuf.vcproj b/projectfiles_vc9/blender/imbuf/BL_imbuf.vcproj
index 0e25b26831b..b0ba1133393 100644
--- a/projectfiles_vc9/blender/imbuf/BL_imbuf.vcproj
+++ b/projectfiles_vc9/blender/imbuf/BL_imbuf.vcproj
@@ -266,6 +266,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\jpeg\include;..\..\..\..\lib\windows\zlib\include;..\..\..\..\lib\windows\png\include;..\..\..\..\lib\windows\tiff\include;..\..\..\..\lib\windows\openexr\include;..\..\..\..\lib\windows\openexr\include\Iex;..\..\..\..\lib\windows\openexr\include\Imath;..\..\..\..\lib\windows\openexr\include\IlmImf;..\..\..\..\lib\windows\QTDevWin\CIncludes;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\build\msvc_9\intern\memutil\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenloader;..\..\..\source\blender\makesdna;..\..\..\source\blender\imbuf\intern;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\imbuf\intern\openexr;..\..\..\source\blender\imbuf\intern\dds"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;WITH_QUICKTIME;WITH_OPENEXR;WITH_DDS;WITH_FFMPEG"

diff --git a/projectfiles_vc9/blender/loader/BLO_loader.vcproj b/projectfiles_vc9/blender/loader/BLO_loader.vcproj
index e8b155875d0..0ddb02afad1 100644
--- a/projectfiles_vc9/blender/loader/BLO_loader.vcproj
+++ b/projectfiles_vc9/blender/loader/BLO_loader.vcproj
@@ -267,6 +267,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\zlib\include;..\..\..\source\blender\inflate;..\..\..\source\blender\deflate;..\..\..\source\blender\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\makesrna;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenkernel;..\..\..\source\blender\blenloader;..\..\..\source\blender\streamglue;..\..\..\source\blender\readblenfile;..\..\..\source\blender\writeblenfile;..\..\..\source\blender\readstreamglue;..\..\..\source\blender\writestreamglue;..\..\..\source\blender\render\extern\include;..\..\..\source\kernel\gen_messaging;..\..\..\..\build\msvc_9\extern\verse\include"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB"

diff --git a/projectfiles_vc9/blender/makesdna/DNA_makesdna.vcproj b/projectfiles_vc9/blender/makesdna/DNA_makesdna.vcproj
index 77fbe72dede..631c5dc2693 100644
--- a/projectfiles_vc9/blender/makesdna/DNA_makesdna.vcproj
+++ b/projectfiles_vc9/blender/makesdna/DNA_makesdna.vcproj
@@ -137,6 +137,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender;..\..\..\source\blender\blenlib;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenkernel;..\..\..\..\lib\windows\pthreads\include"

 				PreprocessorDefinitions="NDEBUG,WIN32,_CONSOLE"

diff --git a/projectfiles_vc9/blender/makesrna/RNA_makesrna.vcproj b/projectfiles_vc9/blender/makesrna/RNA_makesrna.vcproj
index 227b4856143..f52dd193150 100644
--- a/projectfiles_vc9/blender/makesrna/RNA_makesrna.vcproj
+++ b/projectfiles_vc9/blender/makesrna/RNA_makesrna.vcproj
@@ -74,11 +74,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib odbc32.lib odbccp32.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="2"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern\mtdll\debug;..\..\..\..\build\msvc_9\libs\mtdll\debug"

+				AdditionalLibraryDirectories=""

 				IgnoreDefaultLibraryNames=" libc.lib, libcmt.lib, msvcrt.lib, libcd.lib, libcmtd.lib"

 				GenerateDebugInformation="true"

 				ProgramDatabaseFile="..\..\..\..\build\msvc_9\libs\debug\DNA_makesrna.pdb"

@@ -139,6 +138,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender;..\..\..\source\blender\blenlib;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\editors\include;..\..\..\source\blender\blenkernel;..\..\..\..\lib\windows\pthreads\include;..\..\..\source\blender\render\extern\include;..\..\..\source\blender\windowmanager;..\..\..\source\blender\imbuf"

 				PreprocessorDefinitions="NDEBUG,WIN32,_CONSOLE"

@@ -169,11 +169,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="1"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern;..\..\..\..\build\msvc_9\libs"

+				AdditionalLibraryDirectories=""

 				IgnoreDefaultLibraryNames="libc.lib"

 				ProgramDatabaseFile="..\..\..\..\build\msvc_9\libs\DNA_makesrna.pdb"

 				SubSystem="1"

@@ -263,11 +262,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib odbc32.lib odbccp32.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="1"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern\mtdll;..\..\..\..\build\msvc_9\libs\mtdll"

+				AdditionalLibraryDirectories=""

 				IgnoreDefaultLibraryNames=" libc.lib, libcmt.lib, libcd.lib, libcmtd.lib, msvcrtd.lib"

 				ProgramDatabaseFile="..\..\..\..\build\msvc_9\libs\DNA_makesrna.pdb"

 				SubSystem="1"

@@ -358,11 +356,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386&#x0D;&#x0A;"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib DNA_dna.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="2"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern\debug;..\..\..\..\build\msvc_9\libs\debug"

+				AdditionalLibraryDirectories=""

 				IgnoreAllDefaultLibraries="false"

 				IgnoreDefaultLibraryNames="libc.lib, libcd.lib"

 				GenerateDebugInformation="true"

@@ -455,11 +452,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386&#x0D;&#x0A;"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib odbc32.lib odbccp32.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="2"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern\debug;..\..\..\..\build\msvc_9\libs\debug"

+				AdditionalLibraryDirectories=""

 				IgnoreAllDefaultLibraries="false"

 				IgnoreDefaultLibraryNames="libc.lib, libcd.lib"

 				GenerateDebugInformation="true"

@@ -551,11 +547,10 @@
 			<Tool

 				Name="VCLinkerTool"

 				AdditionalOptions="/MACHINE:I386"

-				AdditionalDependencies="libguardedalloc.lib BLI_blenlib.lib odbc32.lib odbccp32.lib"

 				OutputFile="..\..\..\source\blender\makesrna\intern\RNA_makesrna.exe"

 				LinkIncremental="1"

 				SuppressStartupBanner="true"

-				AdditionalLibraryDirectories="..\..\..\..\build\msvc_9\libs\intern;..\..\..\..\build\msvc_9\libs"

+				AdditionalLibraryDirectories=""

 				IgnoreDefaultLibraryNames="libc.lib"

 				ProgramDatabaseFile="..\..\..\..\build\msvc_9\libs\DNA_makesrna.pdb"

 				SubSystem="1"

@@ -871,6 +866,22 @@
 			Name="Header Files"

 			Filter="h;hpp;hxx;hm;inl"

 			>

+			<File

+				RelativePath="..\..\..\source\blender\makesrna\RNA_access.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\makesrna\RNA_define.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\makesrna\RNA_enum_types.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\blender\makesrna\RNA_types.h"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="Resource Files"

diff --git a/projectfiles_vc9/blender/makesrna/RNA_rna.vcproj b/projectfiles_vc9/blender/makesrna/RNA_rna.vcproj
index 84359390f41..b52cf929fc3 100644
--- a/projectfiles_vc9/blender/makesrna/RNA_rna.vcproj
+++ b/projectfiles_vc9/blender/makesrna/RNA_rna.vcproj
@@ -43,7 +43,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\windowmanager;..\..\..\source\blender\editors\include;..\..\..\source\blender\render\extern\include"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\ikplugin;..\..\..\source\blender\windowmanager;..\..\..\source\blender\editors\include;..\..\..\source\blender\render\extern\include"

 				PreprocessorDefinitions="WIN32;_DEBUG;_LIB;_CRT_SECURE_NO_WARNINGS"

 				MinimalRebuild="true"

 				BasicRuntimeChecks="3"

@@ -114,7 +114,7 @@
 				Name="VCCLCompilerTool"

 				Optimization="2"

 				EnableIntrinsicFunctions="true"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\windowmanager;..\..\..\source\blender\editors\include;..\..\..\source\blender\render\extern\include"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\ikplugin;..\..\..\source\blender\windowmanager;..\..\..\source\blender\editors\include;..\..\..\source\blender\render\extern\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB;_CRT_SECURE_NO_WARNINGS"

 				MinimalRebuild="true"

 				RuntimeLibrary="0"

diff --git a/projectfiles_vc9/blender/nodes/nodes.vcproj b/projectfiles_vc9/blender/nodes/nodes.vcproj
index ca997306f04..4b6f90b6cd6 100644
--- a/projectfiles_vc9/blender/nodes/nodes.vcproj
+++ b/projectfiles_vc9/blender/nodes/nodes.vcproj
@@ -252,6 +252,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\zlib\include;..\..\..\..\build\msvc_9\intern\bsp\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\decimation\include;..\..\..\..\build\msvc_9\intern\elbeem\include;..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\source\blender;..\..\..\source\blender\avi;..\..\..\source\blender\imbuf;..\..\..\source\blender\editors\include;..\..\..\source\blender\python;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\nodes;..\..\..\source\blender\blenloader;..\..\..\source\kernel\gen_system;..\..\..\source\blender\renderconverter;..\..\..\source\blender\render\extern\include;..\..\..\source\gameengine\SoundSystem;..\..\..\..\build\msvc_9\extern\verse\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\blender\gpu;..\..\..\source\blender\makesrna"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;WITH_OPENEXR"

 				BasicRuntimeChecks="0"

diff --git a/projectfiles_vc9/blender/render/BRE_render.vcproj b/projectfiles_vc9/blender/render/BRE_render.vcproj
index 4e354c6dde3..6349226363d 100644
--- a/projectfiles_vc9/blender/render/BRE_render.vcproj
+++ b/projectfiles_vc9/blender/render/BRE_render.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="BRE_render"

 	ProjectGUID="{106AE171-0083-41D6-A949-20DB0E8DC251}"

+	RootNamespace="BRE_render"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -41,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\sdl\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\smoke\include;..\..\..\source\blender;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\yafray;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\python;..\..\..\source\blender\blenkernel;..\..\..\source\blender\quicktime;..\..\..\source\blender\blenloader;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\radiosity\extern\include;..\..\..\source\blender\render\intern\include;..\..\..\source\blender\render\extern\include;..\..\..\source\kernel;..\..\..\source\kernel\gen_messaging"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;WITH_QUICKTIME;WITH_OPENEXR"

diff --git a/projectfiles_vc9/gameengine/blenderhook/KX_blenderhook.vcproj b/projectfiles_vc9/gameengine/blenderhook/KX_blenderhook.vcproj
index 4d3d74ca949..ab959de31c7 100644
--- a/projectfiles_vc9/gameengine/blenderhook/KX_blenderhook.vcproj
+++ b/projectfiles_vc9/gameengine/blenderhook/KX_blenderhook.vcproj
@@ -118,6 +118,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\misc;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\blender\blenloader;..\..\..\source\blender\blenkernel;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\ketsji;..\..\..\source\gameengine\network;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\Physics\Sumo;..\..\..\source\gameengine\Physics\common;..\..\..\source\gameengine\network\loopbacknetwork;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\Physics\Sumo\Fuzzics\include;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\source\blender\blenfont;..\..\..\intern\audaspace\intern"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;USE_SUMO_SOLID;WITH_GLEXT;WITH_FFMPEG"

diff --git a/projectfiles_vc9/gameengine/converter/KX_converter.vcproj b/projectfiles_vc9/gameengine/converter/KX_converter.vcproj
index d179fe03dc7..48b7f81b2aa 100644
--- a/projectfiles_vc9/gameengine/converter/KX_converter.vcproj
+++ b/projectfiles_vc9/gameengine/converter/KX_converter.vcproj
@@ -43,7 +43,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="WIN32,_LIB,_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="3"

@@ -117,8 +117,9 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,USE_SUMO_SOLID"

 				StringPooling="true"

 				RuntimeLibrary="0"

@@ -194,7 +195,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="WIN32,_LIB,_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -269,7 +270,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,USE_SUMO_SOLID"

 				StringPooling="true"

 				RuntimeLibrary="2"

@@ -345,7 +346,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="WIN32,_LIB,_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -420,7 +421,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender;..\..\..\source\blender\imbuf;..\..\..\source\blender\blenlib;..\..\..\source\blender\include;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\makesrna;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\ode;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\gameengine\soundsystem\snd_blenderwavecache;..\..\..\source\sumo\include;..\..\..\source\sumo\Fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\gpu;..\..\..\source\blender\windowmanager;..\..\..\intern\audaspace\intern;..\..\..\source\blender\ikplugin"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,USE_SUMO_SOLID"

 				StringPooling="true"

 				RuntimeLibrary="0"

@@ -481,6 +482,22 @@
 				>

 			</File>

 			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureActuator.cpp"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureChannel.cpp"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureConstraint.cpp"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureObject.cpp"

+				>

+			</File>

+			<File

 				RelativePath="..\..\..\source\gameengine\Converter\BL_BlenderDataConversion.cpp"

 				>

 			</File>

@@ -554,7 +571,15 @@
 				>

 			</File>

 			<File

-				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureObject.cpp"

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureActuator.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureChannel.h"

+				>

+			</File>

+			<File

+				RelativePath="..\..\..\source\gameengine\Converter\BL_ArmatureConstraint.h"

 				>

 			</File>

 			<File

diff --git a/projectfiles_vc9/gameengine/expression/EXP_expressions.vcproj b/projectfiles_vc9/gameengine/expression/EXP_expressions.vcproj
index 9aeb9584b35..1ee2fe7eb52 100644
--- a/projectfiles_vc9/gameengine/expression/EXP_expressions.vcproj
+++ b/projectfiles_vc9/gameengine/expression/EXP_expressions.vcproj
@@ -43,7 +43,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="WIN32;_LIB;EXP_PYTHON_EMBEDDING;_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -118,7 +118,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="WIN32,_LIB,EXP_PYTHON_EMBEDDING,_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="3"

@@ -193,7 +193,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,EXP_PYTHON_EMBEDDING"

 				StringPooling="true"

 				RuntimeLibrary="2"

@@ -267,8 +267,9 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,EXP_PYTHON_EMBEDDING"

 				StringPooling="true"

 				RuntimeLibrary="0"

@@ -343,7 +344,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				Optimization="0"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="WIN32;_LIB;EXP_PYTHON_EMBEDDING;_DEBUG"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="1"

@@ -418,7 +419,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				InlineFunctionExpansion="1"

-				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

+				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\SceneGraph;..\..\..\source\blender\blenloader"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,EXP_PYTHON_EMBEDDING"

 				StringPooling="true"

 				RuntimeLibrary="0"

diff --git a/projectfiles_vc9/gameengine/gamelogic/SCA_GameLogic.vcproj b/projectfiles_vc9/gameengine/gamelogic/SCA_GameLogic.vcproj
index 61e589213c0..72150cb7752 100644
--- a/projectfiles_vc9/gameengine/gamelogic/SCA_GameLogic.vcproj
+++ b/projectfiles_vc9/gameengine/gamelogic/SCA_GameLogic.vcproj
@@ -117,6 +117,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\lib\windows\sdl\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\Rasterizer;..\..\..\source\blender\makesdna;..\..\..\source\gameengine\Scenegraph"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB,EXP_PYTHON_EMBEDDING"

@@ -498,6 +499,10 @@
 				>

 			</File>

 			<File

+				RelativePath="..\..\..\source\gameengine\GameLogic\SCA_BasicEventManager.cpp"

+				>

+			</File>

+			<File

 				RelativePath="..\..\..\source\gameengine\GameLogic\SCA_DelaySensor.cpp"

 				>

 			</File>

@@ -659,6 +664,10 @@
 				>

 			</File>

 			<File

+				RelativePath="..\..\..\source\gameengine\GameLogic\SCA_BasicEventManager.h"

+				>

+			</File>

+			<File

 				RelativePath="..\..\..\source\gameengine\GameLogic\SCA_DelaySensor.h"

 				>

 			</File>

diff --git a/projectfiles_vc9/gameengine/gameplayer/axctl/GP_axctl.vcproj b/projectfiles_vc9/gameengine/gameplayer/axctl/GP_axctl.vcproj
index ed2dad65374..5bcfe1aabd1 100644
--- a/projectfiles_vc9/gameengine/gameplayer/axctl/GP_axctl.vcproj
+++ b/projectfiles_vc9/gameengine/gameplayer/axctl/GP_axctl.vcproj
@@ -161,6 +161,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\build\msvc_9\intern\openal\include;..\..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\..\build\msvc_9\intern\SoundSystem\include;..\..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\source\blender\imbuf;..\..\..\..\source\blender\blenlib;..\..\..\..\source\blender\include;..\..\..\..\source\blender\blenkernel;..\..\..\..\source\blender\makesdna;..\..\..\..\source\blender\blenloader;..\..\..\..\source\blender\render\extern\include;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\kernel\gen_messaging;..\..\..\..\source\gameengine\ketsji;..\..\..\..\source\gameengine\network;..\..\..\..\source\gameengine\rasterizer;..\..\..\..\source\gameengine\converter;..\..\..\..\source\gameengine\gamelogic;..\..\..\..\source\gameengine\expressions;..\..\..\..\source\gameengine\scenegraph;..\..\..\..\source\gameengine\soundsystem;..\..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\..\source\gameengine\gameplayer\common;..\..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\source\gameengine\network\loopbacknetwork;..\..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\..\source\gameengine\GamePlayer\common;..\..\..\..\source\gameengine\GamePlayer\common\windows;..\..\..\..\source\sumo\include;..\..\..\..\source\sumo\fuzzics\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_WINDOWS;_USRDLL"

diff --git a/projectfiles_vc9/gameengine/gameplayer/common/GP_common.vcproj b/projectfiles_vc9/gameengine/gameplayer/common/GP_common.vcproj
index 87d9683a25e..ad8f3338594 100644
--- a/projectfiles_vc9/gameengine/gameplayer/common/GP_common.vcproj
+++ b/projectfiles_vc9/gameengine/gameplayer/common/GP_common.vcproj
@@ -265,6 +265,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\lib\windows\zlib\include;..\..\..\..\..\lib\windows\png\include;..\..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\..\build\msvc_9\intern\openal\include;..\..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\..\build\msvc_9\intern\SoundSystem\include;..\..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\source\blender\imbuf;..\..\..\..\source\blender\blenlib;..\..\..\..\source\blender\blenkernel;..\..\..\..\source\blender\blenloader;..\..\..\..\source\blender\makesdna;..\..\..\..\source\blender\render\extern\include;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\gameengine\ketsji;..\..\..\..\source\gameengine\network;..\..\..\..\source\gameengine\rasterizer;..\..\..\..\source\gameengine\Converter;..\..\..\..\source\gameengine\gamelogic;..\..\..\..\source\gameengine\scenegraph;..\..\..\..\source\gameengine\expressions;..\..\..\..\source\gameengine\soundsystem;..\..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\..\source\gameengine\GamePlayer\common;..\..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\source\gameengine\soundsystem\snd_dummy;..\..\..\..\source\gameengine\network\loopbacknetwork;..\..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\..\source\gameengine\Physics\Sumo;..\..\..\..\source\gameengine\Physics\common;..\..\..\..\source\gameengine\Physics\Sumo\Fuzzics\include;..\..\..\..\source\sumo\include;..\..\..\..\source\sumo\fuzzics\include;..\..\..\..\source\blender\gpu"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/gameplayer/ghost/GP_ghost.vcproj b/projectfiles_vc9/gameengine/gameplayer/ghost/GP_ghost.vcproj
index e29a65264e9..4d6b7a4cd8a 100644
--- a/projectfiles_vc9/gameengine/gameplayer/ghost/GP_ghost.vcproj
+++ b/projectfiles_vc9/gameengine/gameplayer/ghost/GP_ghost.vcproj
@@ -136,6 +136,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\ghost\include;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\build\msvc_9\intern\openal\include;..\..\..\..\..\build\msvc_9\intern\opennl\include;..\..\..\..\..\build\msvc_9\intern\bmfont\include;..\..\..\..\..\build\msvc_9\intern\iksolver\include;..\..\..\..\..\build\msvc_9\intern\SoundSystem\include;..\..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\source\blender\include;..\..\..\..\source\blender\imbuf;..\..\..\..\source\blender\blenlib;..\..\..\..\source\blender\blenkernel;..\..\..\..\source\blender\makesdna;..\..\..\..\source\blender\blenloader;..\..\..\..\source\blender\readblenfile;..\..\..\..\source\blender\render\extern\include;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\kernel\gen_messaging;..\..\..\..\source\gameengine\ketsji;..\..\..\..\source\gameengine\Physics;..\..\..\..\source\gameengine\network;..\..\..\..\source\gameengine\rasterizer;..\..\..\..\source\gameengine\converter;..\..\..\..\source\gameengine\gamelogic;..\..\..\..\source\gameengine\expressions;..\..\..\..\source\gameengine\scenegraph;..\..\..\..\source\gameengine\Physics\Ode;..\..\..\..\source\gameengine\soundsystem;..\..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\..\source\gameengine\gameplayer\common;..\..\..\..\source\gameengine\soundsystem\snd_openal;..\..\..\..\source\gameengine\network\loopbacknetwork;..\..\..\..\source\gameengine\rasterizer\ras_openglrasterizer;..\..\..\..\source\gameengine\gameplayer\common\windows;..\..\..\..\source\sumo\include;..\..\..\..\source\sumo\fuzzics\include;..\..\..\..\source\blender\gpu;..\..\..\..\..\build\msvc_9\intern\guardedalloc\include"

 				PreprocessorDefinitions="WIN32,NDEBUG,_CONSOLE;WITH_FFMPEG"

diff --git a/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj b/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj
index f709e0b7d29..b552b87e820 100644
--- a/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj
+++ b/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj
@@ -269,6 +269,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\sdl\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\python\generic;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenkernel;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\physics\sumo\include;..\..\..\source\gameengine\physics\common\dummy;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\sumo\include;..\..\..\source\sumo\fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\python\api2_2x;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;USE_SUMO_SOLID;WITH_GLEXT"

@@ -718,6 +719,10 @@
 				Name="SensorsImp"

 				>

 				<File

+					RelativePath="..\..\..\source\gameengine\Ketsji\KX_ArmatureSensor.cpp"

+					>

+				</File>

+				<File

 					RelativePath="..\..\..\source\gameengine\Ketsji\KX_MouseFocusSensor.cpp"

 					>

 				</File>

@@ -1027,6 +1032,10 @@
 				Name="Sensors"

 				>

 				<File

+					RelativePath="..\..\..\source\gameengine\Ketsji\KX_ArmatureSensor.h"

+					>

+				</File>

+				<File

 					RelativePath="..\..\..\source\gameengine\Ketsji\KX_MouseFocusSensor.h"

 					>

 				</File>

diff --git a/projectfiles_vc9/gameengine/ketsji/network/KX_network.vcproj b/projectfiles_vc9/gameengine/ketsji/network/KX_network.vcproj
index 36c8218b803..92cde144652 100644
--- a/projectfiles_vc9/gameengine/ketsji/network/KX_network.vcproj
+++ b/projectfiles_vc9/gameengine/ketsji/network/KX_network.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\gameengine\ketsji;..\..\..\..\source\gameengine\Network;..\..\..\..\source\gameengine\expressions;..\..\..\..\source\gameengine\GameLogic;..\..\..\..\source\gameengine\Scenegraph"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/network/loopbacknetwork/NG_loopbacknetwork.vcproj b/projectfiles_vc9/gameengine/network/loopbacknetwork/NG_loopbacknetwork.vcproj
index bbb8cc5ec25..ca5e0ddaf94 100644
--- a/projectfiles_vc9/gameengine/network/loopbacknetwork/NG_loopbacknetwork.vcproj
+++ b/projectfiles_vc9/gameengine/network/loopbacknetwork/NG_loopbacknetwork.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="NG_loopbacknetwork"

 	ProjectGUID="{6B801390-5F95-4F07-81A7-97FBA046AACC}"

+	RootNamespace="NG_loopbacknetwork"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -116,6 +117,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\gameengine\Network"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/network/network/NG_network.vcproj b/projectfiles_vc9/gameengine/network/network/NG_network.vcproj
index a9e0ce1acd2..b7a8ab77e26 100644
--- a/projectfiles_vc9/gameengine/network/network/NG_network.vcproj
+++ b/projectfiles_vc9/gameengine/network/network/NG_network.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="NG_network"

 	ProjectGUID="{0A73055E-4DED-40CD-9F72-9093ED3EEC7E}"

+	RootNamespace="NG_network"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -191,6 +192,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\source\kernel\gen_system"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Bullet/PHY_Bullet.vcproj b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Bullet/PHY_Bullet.vcproj
index d4e29e0774a..128b902754c 100644
--- a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Bullet/PHY_Bullet.vcproj
+++ b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Bullet/PHY_Bullet.vcproj
@@ -176,6 +176,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				AdditionalIncludeDirectories="..\..\..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\..\..\build\msvc_9\intern\SoundSystem\include;..\..\..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\..\source\gameengine\Physics\common;..\..\..\..\..\source\gameengine\Physics\Bullet;..\..\..\..\..\source\gameengine\Rasterizer;..\..\..\..\..\source\gameengine\Ketsji;..\..\..\..\..\source\gameengine\Expressions;..\..\..\..\..\source\gameengine\GameLogic;..\..\..\..\..\source\gameengine\SceneGraph;..\..\..\..\..\source\kernel\gen_system;..\..\..\..\..\source\blender\makesdna;..\..\..\..\..\source\blender\blenkernel;..\..\..\..\..\source\blender\blenlib"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB"

 				RuntimeLibrary="0"

diff --git a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Dummy/PHY_Dummy.vcproj b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Dummy/PHY_Dummy.vcproj
index 5c0159d5aa9..bd39d14895f 100644
--- a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Dummy/PHY_Dummy.vcproj
+++ b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Dummy/PHY_Dummy.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="PHY_Dummy"

 	ProjectGUID="{3648FB9A-C36F-43AB-AED0-1F1361E67FC7}"

+	RootNamespace="PHY_Dummy"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -264,6 +265,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\source\gameengine\physics\common"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Ode/PHY_Ode.vcproj b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Ode/PHY_Ode.vcproj
index e9773ecf683..d5b3f657f11 100644
--- a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Ode/PHY_Ode.vcproj
+++ b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Ode/PHY_Ode.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="PHY_Ode"

 	ProjectGUID="{EC405272-28E3-4840-AAC2-53D6DE4E163D}"

+	RootNamespace="PHY_Ode"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -264,6 +265,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\..\lib\windows\ode\include;..\..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\source\gameengine\physics\common"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Physics.vcproj b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Physics.vcproj
index 5441c5bd4f1..7158da47f3b 100644
--- a/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Physics.vcproj
+++ b/projectfiles_vc9/gameengine/physics/PHY_Physics/PHY_Physics.vcproj
@@ -116,6 +116,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\moto\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB;USE_SUMO_SOLID"

diff --git a/projectfiles_vc9/gameengine/rasterizer/RAS_rasterizer.vcproj b/projectfiles_vc9/gameengine/rasterizer/RAS_rasterizer.vcproj
index 43c8d4100bf..f82b46413c9 100644
--- a/projectfiles_vc9/gameengine/rasterizer/RAS_rasterizer.vcproj
+++ b/projectfiles_vc9/gameengine/rasterizer/RAS_rasterizer.vcproj
@@ -192,6 +192,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\Expressions;..\..\..\source\gameengine\SceneGraph;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\source\blender\makesdna"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;WITH_GLEXT"

diff --git a/projectfiles_vc9/gameengine/rasterizer/openglrasterizer/RAS_openglrasterizer.vcproj b/projectfiles_vc9/gameengine/rasterizer/openglrasterizer/RAS_openglrasterizer.vcproj
index 09b87f41ddf..4d55bd0da37 100644
--- a/projectfiles_vc9/gameengine/rasterizer/openglrasterizer/RAS_openglrasterizer.vcproj
+++ b/projectfiles_vc9/gameengine/rasterizer/openglrasterizer/RAS_openglrasterizer.vcproj
@@ -267,6 +267,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\source\kernel\gen_system;..\..\..\..\source\gameengine\Rasterizer;..\..\..\..\source\gameengine\SceneGraph;..\..\..\..\source\blender\gpu;..\..\..\..\source\gameengine\Ketsji;..\..\..\..\source\blender\makesdna;..\..\..\..\source\blender\blenkernel;..\..\..\..\source\blender\blenlib"

 				PreprocessorDefinitions="NDEBUG;WIN32;_LIB;WITH_GLEXT"

diff --git a/projectfiles_vc9/gameengine/scenegraph/SG_SceneGraph.vcproj b/projectfiles_vc9/gameengine/scenegraph/SG_SceneGraph.vcproj
index 95e61cc4af8..043a934758c 100644
--- a/projectfiles_vc9/gameengine/scenegraph/SG_SceneGraph.vcproj
+++ b/projectfiles_vc9/gameengine/scenegraph/SG_SceneGraph.vcproj
@@ -42,6 +42,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\moto\include"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/projectfiles_vc9/gameengine/videotexture/TEX_Video.vcproj b/projectfiles_vc9/gameengine/videotexture/TEX_Video.vcproj
index 94e09303a1b..4eb37259785 100644
--- a/projectfiles_vc9/gameengine/videotexture/TEX_Video.vcproj
+++ b/projectfiles_vc9/gameengine/videotexture/TEX_Video.vcproj
@@ -112,6 +112,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\ffmpeg\include;..\..\..\..\lib\windows\ffmpeg\include\msvc;..\..\..\..\lib\windows\pthreads\include;..\..\..\source\gameengine\Ketsji;..\..\..\source\gameengine\Expressions;..\..\..\source\gameengine\GameLogic;..\..\..\source\gameengine\SceneGraph;..\..\..\source\gameengine\Rasterizer;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\BlenderRoutines;..\..\..\source\blender\editors\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\blenkernel;..\..\..\source\blender\makesdna;..\..\..\source\blender\imbuf;..\..\..\source\blender\python;..\..\..\source\blender\gpu;..\..\..\source\kernel\gen_system;..\..\..\intern\string;..\..\..\intern\moto\include;..\..\..\intern\guardedalloc;..\..\..\intern\SoundSystem;..\..\..\extern\glew\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include"

 				PreprocessorDefinitions="WIN32;NDEBUG;_LIB;WITH_FFMPEG;__STDC_CONSTANT_MACROS"

diff --git a/projectfiles_vc9/kernel/gen_messaging/gen_messaging.vcproj b/projectfiles_vc9/kernel/gen_messaging/gen_messaging.vcproj
index ac958a10f54..8dfe63b67a4 100644
--- a/projectfiles_vc9/kernel/gen_messaging/gen_messaging.vcproj
+++ b/projectfiles_vc9/kernel/gen_messaging/gen_messaging.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="gen_messaging"

 	ProjectGUID="{727F90AC-ABE6-40BF-8937-C2F2F1D13DEA}"

+	RootNamespace="gen_messaging"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -189,6 +190,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\source\kernel\gen_messaging"

 				PreprocessorDefinitions="WIN32,NDEBUG,_LIB"

diff --git a/projectfiles_vc9/kernel/system/SYS_system.vcproj b/projectfiles_vc9/kernel/system/SYS_system.vcproj
index c667a6050e8..36ec0001d8b 100644
--- a/projectfiles_vc9/kernel/system/SYS_system.vcproj
+++ b/projectfiles_vc9/kernel/system/SYS_system.vcproj
@@ -4,6 +4,7 @@
 	Version="9,00"

 	Name="SYS_system"

 	ProjectGUID="{BAAE3F2B-BCF8-4E84-B8BA-CFB2D64945FE}"

+	RootNamespace="SYS_system"

 	TargetFrameworkVersion="131072"

 	>

 	<Platforms>

@@ -266,6 +267,7 @@
 			/>

 			<Tool

 				Name="VCCLCompilerTool"

+				Optimization="2"

 				InlineFunctionExpansion="1"

 				AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include"

 				PreprocessorDefinitions="NDEBUG,WIN32,_LIB"

diff --git a/release/ui/buttons_data_bone.py b/release/ui/buttons_data_bone.py
index 510b41de8a7..dae969a7e5f 100644
--- a/release/ui/buttons_data_bone.py
+++ b/release/ui/buttons_data_bone.py
@@ -174,6 +174,9 @@ class BONE_PT_inverse_kinematics(BoneButtonsPanel):
 		bone = context.bone
 		pchan = ob.pose.pose_channels[context.bone.name]
 
+		row = layout.row()
+		row.itemR(ob.pose, "ik_solver")
+
 		split = layout.split(percentage=0.25)
 		split.itemR(pchan, "ik_dof_x", text="X")
 		row = split.row()
@@ -218,11 +221,27 @@ class BONE_PT_inverse_kinematics(BoneButtonsPanel):
 		row.itemR(pchan, "ik_min_z", text="")
 		row.itemR(pchan, "ik_max_z", text="")
 		row.active = pchan.ik_dof_z and pchan.ik_limit_z
-
 		split = layout.split()
 		split.itemR(pchan, "ik_stretch", text="Stretch", slider=True)
 		split.itemL()
 
+		if ob.pose.ik_solver == "ITASC":
+			layout.itemL(text="Joint constraint:")
+			split = layout.split(percentage=0.3)
+			row = split.row()
+			row.itemR(pchan, "ik_rot_control", text="Rotation")
+			row = split.row()
+			row.itemR(pchan, "ik_rot_weight", text="Weight", slider=True)
+			row.active = pchan.ik_rot_control
+			# not supported yet
+			#split = layout.split(percentage=0.3)
+			#row = split.row()
+			#row.itemR(pchan, "ik_lin_control", text="Size")
+			#row = split.row()
+			#row.itemR(pchan, "ik_lin_weight", text="Weight", slider=True)
+			#row.active = pchan.ik_lin_control
+			
+
 class BONE_PT_deform(BoneButtonsPanel):
 	__label__ = "Deform"
 	__default_closed__ = True
@@ -271,9 +290,64 @@ class BONE_PT_deform(BoneButtonsPanel):
 		col.itemL(text="Offset:")
 		col.itemR(bone, "cyclic_offset")
 
+class BONE_PT_iksolver_itasc(BoneButtonsPanel):
+	__idname__ = "BONE_PT_iksolver_itasc"
+	__label__ = "iTaSC parameters"
+	__default_closed__ = True
+	
+	def poll(self, context):
+		ob = context.object
+		bone = context.bone
+
+		if ob and context.bone:
+			pchan = ob.pose.pose_channels[context.bone.name]
+			return pchan.has_ik and ob.pose.ik_solver == "ITASC" and ob.pose.ik_param
+		
+		return False
+
+	def draw(self, context):
+		layout = self.layout
+		ob = context.object
+		itasc = ob.pose.ik_param
+
+		layout.row().itemR(itasc, "simulation")
+		if itasc.simulation:
+			split = layout.split()
+			row = split.row()
+			row.itemR(itasc, "reiteration")
+			row = split.row()
+			if itasc.reiteration:
+				itasc.initial_reiteration = True
+			row.itemR(itasc, "initial_reiteration")
+			row.active = not itasc.reiteration
+		
+		flow = layout.column_flow()
+		flow.itemR(itasc, "precision")
+		flow.itemR(itasc, "num_iter")
+		flow.active = not itasc.simulation or itasc.initial_reiteration or itasc.reiteration
+
+		if itasc.simulation:		
+			layout.itemR(itasc, "auto_step")
+			row = layout.row()
+			if itasc.auto_step:
+				row.itemR(itasc, "min_step")
+				row.itemR(itasc, "max_step")
+			else:
+				row.itemR(itasc, "num_step")
+			
+		layout.itemR(itasc, "solver")
+		if itasc.simulation:
+			layout.itemR(itasc, "feedback")
+			layout.itemR(itasc, "max_velocity")
+		if itasc.solver == "DLS":
+			row = layout.row()
+			row.itemR(itasc, "dampmax")
+			row.itemR(itasc, "dampeps")
+
 bpy.types.register(BONE_PT_context_bone)
 bpy.types.register(BONE_PT_transform)
 bpy.types.register(BONE_PT_transform_locks)
 bpy.types.register(BONE_PT_bone)
 bpy.types.register(BONE_PT_deform)
 bpy.types.register(BONE_PT_inverse_kinematics)
+bpy.types.register(BONE_PT_iksolver_itasc)
diff --git a/release/ui/buttons_object_constraint.py b/release/ui/buttons_object_constraint.py
index 8671f9e25a8..ca5c86fddd7 100644
--- a/release/ui/buttons_object_constraint.py
+++ b/release/ui/buttons_object_constraint.py
@@ -6,14 +6,14 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 	__region_type__ = 'WINDOW'
 	__context__ = "constraint"
 
-	def draw_constraint(self, con):
+	def draw_constraint(self, context, con):
 		layout = self.layout
 		
 		box = layout.template_constraint(con)
 
 		if box:
 			# match enum type to our functions, avoids a lookup table.
-			getattr(self, con.type)(box, con)
+			getattr(self, con.type)(context, box, con)
 	
 			# show/key buttons here are most likely obsolete now, with
 			# keyframing functionality being part of every button
@@ -48,8 +48,29 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 					row.itemR(con, "head_tail", text="")
 			elif con.target.type in ('MESH', 'LATTICE'):
 				layout.item_pointerR(con, "subtarget", con.target, "vertex_groups", text="Vertex Group")
+
+	def ik_template(self, layout, con):
+		layout.itemR(con, "pole_target")
+	
+		if con.pole_target and con.pole_target.type == 'ARMATURE':
+			layout.item_pointerR(con, "pole_subtarget", con.pole_target.data, "bones", text="Bone")
+		
+		if con.pole_target:
+			row = layout.row()
+			row.itemL()
+			row.itemR(con, "pole_angle")
+		
+		split = layout.split()
+		col = split.column()
+		col.itemR(con, "tail")
+		col.itemR(con, "stretch")
+
+		col = split.column()
+		col.itemR(con, "iterations")
+		col.itemR(con, "chain_length")
+		
 	
-	def CHILD_OF(self, layout, con):
+	def CHILD_OF(self, context, layout, con):
 		self.target_template(layout, con)
 
 		split = layout.split()
@@ -76,7 +97,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemO("constraint.childof_set_inverse")
 		row.itemO("constraint.childof_clear_inverse")
 		
-	def TRACK_TO(self, layout, con):
+	def TRACK_TO(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row()
@@ -90,35 +111,40 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		
 		self.space_template(layout, con)
 		
-	def IK(self, layout, con):
+	def IK(self, context, layout, con):
+		if context.object.pose.ik_solver == "ITASC":
+			layout.itemR(con, "ik_type")
+			getattr(self, "IK_"+con.ik_type)(context, layout, con)
+		else:
+			self.IK_COPY_POSE(context, layout, con)
+
+	def IK_COPY_POSE(self, context, layout, con):
 		self.target_template(layout, con)
-		
-		layout.itemR(con, "pole_target")
-	
-		if con.pole_target and con.pole_target.type == 'ARMATURE':
-			layout.item_pointerR(con, "pole_subtarget", con.pole_target.data, "bones", text="Bone")
-		
+		self.ik_template(layout, con)
+
 		split = layout.split()
-	
 		col = split.column()
-		col.itemR(con, "iterations")
-		col.itemR(con, "chain_length")
-		sub = col.column()
-		sub.active = con.pole_target
-		sub.itemR(con, "pole_angle")
+		col.itemL()
+		col.itemR(con, "targetless")
+		col.itemR(con, "rotation")
+
+		col = split.column()
 		col.itemL(text="Weight:")
 		col.itemR(con, "weight", text="Position", slider=True)
 		sub = col.column()
 		sub.active = con.rotation
 		sub.itemR(con, "orient_weight", text="Rotation", slider=True)
 		
-		col = split.column()
-		col.itemR(con, "tail")
-		col.itemR(con, "rotation")
-		col.itemR(con, "targetless")
-		col.itemR(con, "stretch")
-		
-	def FOLLOW_PATH(self, layout, con):
+	def IK_DISTANCE(self, context, layout, con):
+		self.target_template(layout, con)
+		self.ik_template(layout, con)
+
+		layout.itemR(con, "limit_mode")
+		row = layout.row()
+		row.itemR(con, "weight", text="Weight", slider=True)
+		row.itemR(con, "distance", text="Distance", slider=True)
+
+	def FOLLOW_PATH(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		split = layout.split()
@@ -142,7 +168,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemR(con, "up", text="Up")
 		row.itemL()
 		
-	def LIMIT_ROTATION(self, layout, con):
+	def LIMIT_ROTATION(self, context, layout, con):
 		
 		split = layout.split()
 		
@@ -175,7 +201,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Convert:")
 		row.itemR(con, "owner_space", text="")
 		
-	def LIMIT_LOCATION(self, layout, con):
+	def LIMIT_LOCATION(self, context, layout, con):
 		split = layout.split()
 		
 		col = split.column()
@@ -216,7 +242,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Convert:")
 		row.itemR(con, "owner_space", text="")
 		
-	def LIMIT_SCALE(self, layout, con):
+	def LIMIT_SCALE(self, context, layout, con):
 		split = layout.split()
 
 		col = split.column()
@@ -257,7 +283,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Convert:")
 		row.itemR(con, "owner_space", text="")
 	
-	def COPY_ROTATION(self, layout, con):
+	def COPY_ROTATION(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		split = layout.split()
@@ -284,7 +310,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		
 		self.space_template(layout, con)
 		
-	def COPY_LOCATION(self, layout, con):
+	def COPY_LOCATION(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		split = layout.split()
@@ -311,7 +337,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 			
 		self.space_template(layout, con)
 		
-	def COPY_SCALE(self, layout, con):
+	def COPY_SCALE(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row(align=True)
@@ -323,9 +349,9 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		
 		self.space_template(layout, con)
 		
-	#def SCRIPT(self, layout, con):
+	#def SCRIPT(self, context, layout, con):
 	
-	def ACTION(self, layout, con):
+	def ACTION(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		layout.itemR(con, "action")
@@ -345,7 +371,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Convert:")
 		row.itemR(con, "owner_space", text="")
 	
-	def LOCKED_TRACK(self, layout, con):
+	def LOCKED_TRACK(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row()
@@ -356,7 +382,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Lock:")
 		row.itemR(con, "locked", expand=True)
 		
-	def LIMIT_DISTANCE(self, layout, con):
+	def LIMIT_DISTANCE(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		col = layout.column(align=True);
@@ -367,7 +393,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Clamp Region:")
 		row.itemR(con, "limit_mode", text="")
 		
-	def STRETCH_TO(self, layout, con):
+	def STRETCH_TO(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row()
@@ -383,7 +409,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Plane:")
 		row.itemR(con, "keep_axis", expand=True)
 		
-	def FLOOR(self, layout, con):
+	def FLOOR(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row()
@@ -396,7 +422,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row.itemL(text="Min/Max:")
 		row.itemR(con, "floor_location", expand=True)
 		
-	def RIGID_BODY_JOINT(self, layout, con):
+	def RIGID_BODY_JOINT(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		layout.itemR(con, "pivot_type")
@@ -422,7 +448,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		
 		#Missing: Limit arrays (not wrapped in RNA yet) 
 	
-	def CLAMP_TO(self, layout, con):
+	def CLAMP_TO(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		row = layout.row()
@@ -432,7 +458,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		row = layout.row()
 		row.itemR(con, "cyclic")
 		
-	def TRANSFORM(self, layout, con):
+	def TRANSFORM(self, context, layout, con):
 		self.target_template(layout, con)
 		
 		layout.itemR(con, "extrapolate_motion", text="Extrapolate")
@@ -481,7 +507,7 @@ class ConstraintButtonsPanel(bpy.types.Panel):
 		
 		self.space_template(layout, con)
 		
-	def SHRINKWRAP (self, layout, con):
+	def SHRINKWRAP (self, context, layout, con):
 		self.target_template(layout, con)
 		
 		layout.itemR(con, "distance")
@@ -509,7 +535,7 @@ class OBJECT_PT_constraints(ConstraintButtonsPanel):
 		row.itemL();
 
 		for con in ob.constraints:
-			self.draw_constraint(con)
+			self.draw_constraint(context, con)
 
 class BONE_PT_constraints(ConstraintButtonsPanel):
 	__label__ = "Constraints"
@@ -530,7 +556,7 @@ class BONE_PT_constraints(ConstraintButtonsPanel):
 		row.itemL();
 
 		for con in pchan.constraints:
-			self.draw_constraint(con)
+			self.draw_constraint(context, con)
 
 bpy.types.register(OBJECT_PT_constraints)
 bpy.types.register(BONE_PT_constraints)
diff --git a/source/blender/CMakeLists.txt b/source/blender/CMakeLists.txt
index 703c5acd8a2..99297714fd2 100644
--- a/source/blender/CMakeLists.txt
+++ b/source/blender/CMakeLists.txt
@@ -40,6 +40,7 @@ ADD_SUBDIRECTORY(makesrna)
 ADD_SUBDIRECTORY(readblenfile)
 ADD_SUBDIRECTORY(render)
 ADD_SUBDIRECTORY(blenfont)
+ADD_SUBDIRECTORY(ikplugin)
 
 IF(WITH_OPENEXR)
 	ADD_SUBDIRECTORY(imbuf/intern/openexr)
diff --git a/source/blender/Makefile b/source/blender/Makefile
index 31636f838c3..6bc874c3c93 100644
--- a/source/blender/Makefile
+++ b/source/blender/Makefile
@@ -34,7 +34,7 @@ DIRS = windowmanager editors blenloader readblenfile
 DIRS += avi imbuf render blenlib blenkernel blenpluginapi
 DIRS += makesdna makesrna
 DIRS += python nodes gpu
-DIRS += blenfont
+DIRS += blenfont ikplugin
 
 ifeq ($(WITH_QUICKTIME), true)
 	DIRS += quicktime
diff --git a/source/blender/SConscript b/source/blender/SConscript
index a064850c170..af2c81a3b45 100644
--- a/source/blender/SConscript
+++ b/source/blender/SConscript
@@ -16,6 +16,7 @@ SConscript(['avi/SConscript',
             'readblenfile/SConscript',
             'render/SConscript',
             'nodes/SConscript',
+            'ikplugin/SConscript',
             'windowmanager/SConscript',
 	    'blenfont/SConscript'])
 
diff --git a/source/blender/blenkernel/BKE_action.h b/source/blender/blenkernel/BKE_action.h
index f079cc08281..17b56864d1e 100644
--- a/source/blender/blenkernel/BKE_action.h
+++ b/source/blender/blenkernel/BKE_action.h
@@ -41,6 +41,7 @@ struct bAction;
 struct bActionGroup;
 struct FCurve;
 struct bPose;
+struct bItasc;
 struct bPoseChannel;
 struct Object;
 struct Scene;
@@ -154,11 +155,21 @@ struct bPoseChannel *get_active_posechannel(struct Object *ob);
  */
 struct bPoseChannel *verify_pose_channel(struct bPose* pose, const char* name);
 
-
+/* Copy the data from the action-pose (src) into the pose */
+void extract_pose_from_pose(struct bPose *pose, const struct bPose *src);
 
 /* sets constraint flags */
 void update_pose_constraint_flags(struct bPose *pose);
 
+/* return the name of structure pointed by pose->ikparam */
+const char *get_ikparam_name(struct bPose *pose);
+
+/* allocate and initialize pose->ikparam according to pose->iksolver */
+void init_pose_ikparam(struct bPose *pose);
+
+/* initialize a bItasc structure with default value */
+void init_pose_itasc(struct bItasc *itasc);
+
 /* clears BONE_UNKEYED flags for frame changing */
 // XXX to be depreceated for a more general solution in animsys...
 void framechange_poses_clear_unkeyed(void);
@@ -181,16 +192,6 @@ void copy_pose_result(struct bPose *to, struct bPose *from);
 /* clear all transforms */
 void rest_pose(struct bPose *pose);
 
-/* Game Engine ------------------------- */
-
-/* exported for game engine */
-void game_blend_poses(struct bPose *dst, struct bPose *src, float srcweight/*, short mode*/); /* was blend_poses */
-void extract_pose_from_pose(struct bPose *pose, const struct bPose *src);
-
-/* functions used by the game engine */
-void game_copy_pose(struct bPose **dst, struct bPose *src);
-void game_free_pose(struct bPose *pose);
-
 #ifdef __cplusplus
 };
 #endif
diff --git a/source/blender/blenkernel/BKE_armature.h b/source/blender/blenkernel/BKE_armature.h
index 1cbb2331782..8dbd2721fb9 100644
--- a/source/blender/blenkernel/BKE_armature.h
+++ b/source/blender/blenkernel/BKE_armature.h
@@ -89,6 +89,7 @@ void where_is_armature (struct bArmature *arm);
 void where_is_armature_bone(struct Bone *bone, struct Bone *prevbone);
 void armature_rebuild_pose(struct Object *ob, struct bArmature *arm);
 void where_is_pose (struct Scene *scene, struct Object *ob);
+void where_is_pose_bone(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime);
 
 /* get_objectspace_bone_matrix has to be removed still */
 void get_objectspace_bone_matrix (struct Bone* bone, float M_accumulatedMatrix[][4], int root, int posed);
@@ -102,11 +103,6 @@ void armature_mat_pose_to_bone(struct bPoseChannel *pchan, float inmat[][4], flo
 void armature_loc_pose_to_bone(struct bPoseChannel *pchan, float *inloc, float *outloc);
 void armature_mat_pose_to_delta(float delta_mat[][4], float pose_mat[][4], float arm_mat[][4]);
 
-/*	Animation functions */
-struct PoseTree *ik_tree_to_posetree(struct Object *ob, struct Bone *bone);
-void solve_posetree(PoseTree *tree);
-void free_posetree(PoseTree *tree);
-
 /* B-Bone support */
 typedef struct Mat4 {
 	float mat[4][4];
diff --git a/source/blender/blenkernel/BKE_constraint.h b/source/blender/blenkernel/BKE_constraint.h
index a0061173438..126816f5a95 100644
--- a/source/blender/blenkernel/BKE_constraint.h
+++ b/source/blender/blenkernel/BKE_constraint.h
@@ -38,6 +38,9 @@ struct Scene;
 struct bPoseChannel;
 
 /* ---------------------------------------------------------------------------- */
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 /* special struct for use in constraint evaluation */
 typedef struct bConstraintOb {
@@ -131,6 +134,9 @@ void constraint_mat_convertspace(struct Object *ob, struct bPoseChannel *pchan,
 void get_constraint_target_matrix(struct bConstraint *con, int n, short ownertype, void *ownerdata, float mat[][4], float ctime);
 void solve_constraints(struct ListBase *conlist, struct bConstraintOb *cob, float ctime);
 
+#ifdef __cplusplus
+}
+#endif
 
 #endif
 
diff --git a/source/blender/blenkernel/CMakeLists.txt b/source/blender/blenkernel/CMakeLists.txt
index 68aed2b0184..3473950ab3a 100644
--- a/source/blender/blenkernel/CMakeLists.txt
+++ b/source/blender/blenkernel/CMakeLists.txt
@@ -30,7 +30,7 @@ SET(INC
 	. ../../../intern/guardedalloc ../../../intern/memutil ../editors/include ../blenlib ../makesdna
 	../render/extern/include ../../../intern/decimation/extern
 	../imbuf ../avi ../../../intern/elbeem/extern ../../../intern/opennl/extern
-	../../../intern/iksolver/extern ../blenloader
+	../../../intern/iksolver/extern ../blenloader ../ikplugin
 	../nodes ../../../extern/glew/include ../gpu ../makesrna ../../../intern/smoke/extern
 	../../../intern/bsp/extern ../blenfont
 	../../../intern/audaspace/intern
diff --git a/source/blender/blenkernel/SConscript b/source/blender/blenkernel/SConscript
index 1f42390504d..944667e2963 100644
--- a/source/blender/blenkernel/SConscript
+++ b/source/blender/blenkernel/SConscript
@@ -5,7 +5,7 @@ sources = env.Glob('intern/*.c')
 
 incs = '. #/intern/guardedalloc #/intern/memutil ../editors/include ../blenlib ../blenfont ../makesdna'
 incs += ' ../render/extern/include #/intern/decimation/extern ../makesrna'
-incs += ' ../imbuf ../avi #/intern/elbeem/extern ../nodes'
+incs += ' ../imbuf ../ikplugin ../avi #/intern/elbeem/extern ../nodes'
 incs += ' #/intern/iksolver/extern ../blenloader'
 incs += ' #/extern/bullet2/src'
 incs += ' #/intern/opennl/extern #/intern/bsp/extern'
diff --git a/source/blender/blenkernel/intern/Makefile b/source/blender/blenkernel/intern/Makefile
index 6c2edc9e25f..f16b57c8469 100644
--- a/source/blender/blenkernel/intern/Makefile
+++ b/source/blender/blenkernel/intern/Makefile
@@ -47,6 +47,7 @@ CPPFLAGS += -I$(NAN_AUDASPACE)/include
 CPPFLAGS += -I../../makesdna
 CPPFLAGS += -I../../makesrna
 CPPFLAGS += -I../../imbuf
+CPPFLAGS += -I../../ikplugin
 # This mod uses the BLI and BLO module
 CPPFLAGS += -I../../blenlib
 CPPFLAGS += -I../../blenloader
diff --git a/source/blender/blenkernel/intern/action.c b/source/blender/blenkernel/intern/action.c
index 1ff5d9b5c01..4cfd35a494d 100644
--- a/source/blender/blenkernel/intern/action.c
+++ b/source/blender/blenkernel/intern/action.c
@@ -62,6 +62,7 @@
 #include "BKE_main.h"
 #include "BKE_object.h"
 #include "BKE_utildefines.h"
+#include "BIK_api.h"
 
 #include "BLI_arithb.h"
 #include "BLI_blenlib.h"
@@ -451,7 +452,7 @@ bPoseChannel *verify_pose_channel(bPose* pose, const char* name)
 	chan->limitmin[0]= chan->limitmin[1]= chan->limitmin[2]= -180.0f;
 	chan->limitmax[0]= chan->limitmax[1]= chan->limitmax[2]= 180.0f;
 	chan->stiffness[0]= chan->stiffness[1]= chan->stiffness[2]= 0.0f;
-	
+	chan->ikrotweight = chan->iklinweight = 0.0f;
 	Mat4One(chan->constinv);
 	
 	BLI_addtail(&pose->chanbase, chan);
@@ -477,7 +478,18 @@ bPoseChannel *get_active_posechannel (Object *ob)
 	return NULL;
 }
 
-
+const char *get_ikparam_name(bPose *pose)
+{
+	if (pose) {
+		switch (pose->iksolver) {
+		case IKSOLVER_LEGACY:
+			return NULL;
+		case IKSOLVER_ITASC:
+			return "bItasc";
+		}
+	}
+	return NULL;
+}
 /* dst should be freed already, makes entire duplicate */
 void copy_pose (bPose **dst, bPose *src, int copycon)
 {
@@ -499,7 +511,10 @@ void copy_pose (bPose **dst, bPose *src, int copycon)
 	outPose= MEM_callocN(sizeof(bPose), "pose");
 	
 	BLI_duplicatelist(&outPose->chanbase, &src->chanbase);
-	
+	outPose->iksolver = src->iksolver;
+	outPose->ikdata = NULL;
+	outPose->ikparam = MEM_dupallocN(src->ikparam);
+
 	if (copycon) {
 		for (pchan=outPose->chanbase.first; pchan; pchan=pchan->next) {
 			copy_constraints(&listb, &pchan->constraints);  // copy_constraints NULLs listb
@@ -511,6 +526,39 @@ void copy_pose (bPose **dst, bPose *src, int copycon)
 	*dst=outPose;
 }
 
+void init_pose_itasc(bItasc *itasc)
+{
+	if (itasc) {
+		itasc->iksolver = IKSOLVER_ITASC;
+		itasc->minstep = 0.01f;
+		itasc->maxstep = 0.06f;
+		itasc->numiter = 100;
+		itasc->numstep = 4;
+		itasc->precision = 0.005f;
+		itasc->flag = ITASC_AUTO_STEP|ITASC_INITIAL_REITERATION|ITASC_SIMULATION;
+		itasc->feedback = 20.f;
+		itasc->maxvel = 50.f;
+		itasc->solver = ITASC_SOLVER_SDLS;
+		itasc->dampmax = 0.5;
+		itasc->dampeps = 0.15;
+	}
+}
+void init_pose_ikparam(bPose *pose)
+{
+	bItasc *itasc;
+	switch (pose->iksolver) {
+	case IKSOLVER_ITASC:
+		itasc = MEM_callocN(sizeof(bItasc), "itasc");
+		init_pose_itasc(itasc);
+		pose->ikparam = itasc;
+		break;
+	case IKSOLVER_LEGACY:
+	default:
+		pose->ikparam = NULL;
+		break;
+	}
+}
+
 void free_pose_channels(bPose *pose) 
 {
 	bPoseChannel *pchan;
@@ -534,133 +582,15 @@ void free_pose(bPose *pose)
 		/* free pose-groups */
 		if (pose->agroups.first)
 			BLI_freelistN(&pose->agroups);
-		
-		/* free pose */
-		MEM_freeN(pose);
-	}
-}
-
-void game_copy_pose(bPose **dst, bPose *src)
-{
-	bPose *out;
-	bPoseChannel *pchan, *outpchan;
-	GHash *ghash;
-	
-	/* the game engine copies the current armature pose and then swaps
-	 * the object pose pointer. this makes it possible to change poses
-	 * without affecting the original blender data. */
-
-	if (!src) {
-		*dst=NULL;
-		return;
-	}
-	else if (*dst==src) {
-		printf("copy_pose source and target are the same\n");
-		*dst=NULL;
-		return;
-	}
-	
-	out= MEM_dupallocN(src);
-	out->agroups.first= out->agroups.last= NULL;
-	BLI_duplicatelist(&out->chanbase, &src->chanbase);
-
-	/* remap pointers */
-	ghash= BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp);
 
-	pchan= src->chanbase.first;
-	outpchan= out->chanbase.first;
-	for (; pchan; pchan=pchan->next, outpchan=outpchan->next)
-		BLI_ghash_insert(ghash, pchan, outpchan);
-
-	for (pchan=out->chanbase.first; pchan; pchan=pchan->next) {
-		pchan->parent= BLI_ghash_lookup(ghash, pchan->parent);
-		pchan->child= BLI_ghash_lookup(ghash, pchan->child);
-		pchan->path= NULL;
-	}
-
-	BLI_ghash_free(ghash, NULL, NULL);
-	
-	*dst=out;
-}
+		/* free IK solver state */
+		BIK_clear_data(pose);
 
+		/* free IK solver param */
+		if (pose->ikparam)
+			MEM_freeN(pose->ikparam);
 
-/* Only allowed for Poses with identical channels */
-void game_blend_poses(bPose *dst, bPose *src, float srcweight/*, short mode*/)
-{
-	short mode= ACTSTRIPMODE_BLEND;
-	
-	bPoseChannel *dchan;
-	const bPoseChannel *schan;
-	bConstraint *dcon, *scon;
-	float dstweight;
-	int i;
-
-	switch (mode){
-	case ACTSTRIPMODE_BLEND:
-		dstweight = 1.0F - srcweight;
-		break;
-	case ACTSTRIPMODE_ADD:
-		dstweight = 1.0F;
-		break;
-	default :
-		dstweight = 1.0F;
-	}
-	
-	schan= src->chanbase.first;
-	for (dchan = dst->chanbase.first; dchan; dchan=dchan->next, schan= schan->next){
-		if (schan->flag & (POSE_ROT|POSE_LOC|POSE_SIZE)) {
-			/* replaced quat->matrix->quat conversion with decent quaternion interpol (ton) */
-			
-			/* Do the transformation blend */
-			if (schan->flag & POSE_ROT) {
-				/* quat interpolation done separate */
-				if (schan->rotmode == PCHAN_ROT_QUAT) {
-					float dquat[4], squat[4];
-					
-					QUATCOPY(dquat, dchan->quat);
-					QUATCOPY(squat, schan->quat);
-					if (mode==ACTSTRIPMODE_BLEND)
-						QuatInterpol(dchan->quat, dquat, squat, srcweight);
-					else {
-						QuatMulFac(squat, srcweight);
-						QuatMul(dchan->quat, dquat, squat);
-					}
-					
-					NormalQuat(dchan->quat);
-				}
-			}
-
-			for (i=0; i<3; i++) {
-				/* blending for loc and scale are pretty self-explanatory... */
-				if (schan->flag & POSE_LOC)
-					dchan->loc[i] = (dchan->loc[i]*dstweight) + (schan->loc[i]*srcweight);
-				if (schan->flag & POSE_SIZE)
-					dchan->size[i] = 1.0f + ((dchan->size[i]-1.0f)*dstweight) + ((schan->size[i]-1.0f)*srcweight);
-				
-				/* euler-rotation interpolation done here instead... */
-				// FIXME: are these results decent?
-				if ((schan->flag & POSE_ROT) && (schan->rotmode))
-					dchan->eul[i] = (dchan->eul[i]*dstweight) + (schan->eul[i]*srcweight);
-			}
-			dchan->flag |= schan->flag;
-		}
-		for(dcon= dchan->constraints.first, scon= schan->constraints.first; dcon && scon; dcon= dcon->next, scon= scon->next) {
-			/* no 'add' option for constraint blending */
-			dcon->enforce= dcon->enforce*(1.0f-srcweight) + scon->enforce*srcweight;
-		}
-	}
-	
-	/* this pose is now in src time */
-	dst->ctime= src->ctime;
-}
-
-void game_free_pose(bPose *pose)
-{
-	if (pose) {
-		/* we don't free constraints, those are owned by the original pose */
-		if(pose->chanbase.first)
-			BLI_freelistN(&pose->chanbase);
-		
+		/* free pose */
 		MEM_freeN(pose);
 	}
 }
@@ -917,7 +847,6 @@ void calc_action_range(const bAction *act, float *start, float *end, short incl_
 	}
 }
 
-
 /* Return flags indicating which transforms the given object/posechannel has 
  *	- if 'curves' is provided, a list of links to these curves are also returned
  */
diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index c880925aa94..b2368451414 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -66,10 +66,9 @@
 #include "BKE_object.h"
 #include "BKE_object.h"
 #include "BKE_utildefines.h"
+#include "BIK_api.h"
 #include "BKE_sketch.h"
 
-#include "IK_solver.h"
-
 #ifdef HAVE_CONFIG_H
 #include <config.h>
 #endif
@@ -1569,409 +1568,10 @@ void armature_rebuild_pose(Object *ob, bArmature *arm)
 		DAG_pose_sort(ob);
 	
 	ob->pose->flag &= ~POSE_RECALC;
+	ob->pose->flag |= POSE_WAS_REBUILT;
 }
 
 
-/* ********************** THE IK SOLVER ******************* */
-
-
-
-/* allocates PoseTree, and links that to root bone/channel */
-/* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
-static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
-{
-	bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
-	PoseTree *tree;
-	PoseTarget *target;
-	bConstraint *con;
-	bKinematicConstraint *data= NULL;
-	int a, segcount= 0, size, newsize, *oldparent, parent;
-	
-	/* find IK constraint, and validate it */
-	for(con= pchan_tip->constraints.first; con; con= con->next) {
-		if(con->type==CONSTRAINT_TYPE_KINEMATIC) {
-			data=(bKinematicConstraint*)con->data;
-			if (data->flag & CONSTRAINT_IK_AUTO) break;
-			if (data->tar==NULL) continue;
-			if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue;
-			if ((con->flag & CONSTRAINT_DISABLE)==0 && (con->enforce!=0.0)) break;
-		}
-	}
-	if(con==NULL) return;
-	
-	/* exclude tip from chain? */
-	if(!(data->flag & CONSTRAINT_IK_TIP))
-		pchan_tip= pchan_tip->parent;
-	
-	/* Find the chain's root & count the segments needed */
-	for (curchan = pchan_tip; curchan; curchan=curchan->parent){
-		pchan_root = curchan;
-		
-		curchan->flag |= POSE_CHAIN;	// don't forget to clear this
-		chanlist[segcount]=curchan;
-		segcount++;
-		
-		if(segcount==data->rootbone || segcount>255) break; // 255 is weak
-	}
-	if (!segcount) return;
-
-	/* setup the chain data */
-	
-	/* we make tree-IK, unless all existing targets are in this chain */
-	for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
-		for(target= tree->targets.first; target; target= target->next) {
-			curchan= tree->pchan[target->tip];
-			if(curchan->flag & POSE_CHAIN)
-				curchan->flag &= ~POSE_CHAIN;
-			else
-				break;
-		}
-		if(target) break;
-	}
-
-	/* create a target */
-	target= MEM_callocN(sizeof(PoseTarget), "posetarget");
-	target->con= con;
-	pchan_tip->flag &= ~POSE_CHAIN;
-
-	if(tree==NULL) {
-		/* make new tree */
-		tree= MEM_callocN(sizeof(PoseTree), "posetree");
-
-		tree->iterations= data->iterations;
-		tree->totchannel= segcount;
-		tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
-		
-		tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
-		tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
-		for(a=0; a<segcount; a++) {
-			tree->pchan[a]= chanlist[segcount-a-1];
-			tree->parent[a]= a-1;
-		}
-		target->tip= segcount-1;
-		
-		/* AND! link the tree to the root */
-		BLI_addtail(&pchan_root->iktree, tree);
-	}
-	else {
-		tree->iterations= MAX2(data->iterations, tree->iterations);
-		tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
-
-		/* skip common pose channels and add remaining*/
-		size= MIN2(segcount, tree->totchannel);
-		for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
-		parent= a-1;
-
-		segcount= segcount-a;
-		target->tip= tree->totchannel + segcount - 1;
-
-		if (segcount > 0) {
-			/* resize array */
-			newsize= tree->totchannel + segcount;
-			oldchan= tree->pchan;
-			oldparent= tree->parent;
-
-			tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
-			tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
-			memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
-			memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
-			MEM_freeN(oldchan);
-			MEM_freeN(oldparent);
-
-			/* add new pose channels at the end, in reverse order */
-			for(a=0; a<segcount; a++) {
-				tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
-				tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
-			}
-			tree->parent[tree->totchannel]= parent;
-			
-			tree->totchannel= newsize;
-		}
-
-		/* move tree to end of list, for correct evaluation order */
-		BLI_remlink(&pchan_root->iktree, tree);
-		BLI_addtail(&pchan_root->iktree, tree);
-	}
-
-	/* add target to the tree */
-	BLI_addtail(&tree->targets, target);
-}
-
-/* called from within the core where_is_pose loop, all animsystems and constraints
-were executed & assigned. Now as last we do an IK pass */
-static void execute_posetree(Object *ob, PoseTree *tree)
-{
-	float R_parmat[3][3], identity[3][3];
-	float iR_parmat[3][3];
-	float R_bonemat[3][3];
-	float goalrot[3][3], goalpos[3];
-	float rootmat[4][4], imat[4][4];
-	float goal[4][4], goalinv[4][4];
-	float irest_basis[3][3], full_basis[3][3];
-	float end_pose[4][4], world_pose[4][4];
-	float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
-	float resultinf=0.0f;
-	int a, flag, hasstretch=0, resultblend=0;
-	bPoseChannel *pchan;
-	IK_Segment *seg, *parent, **iktree, *iktarget;
-	IK_Solver *solver;
-	PoseTarget *target;
-	bKinematicConstraint *data, *poleangledata=NULL;
-	Bone *bone;
-
-	if (tree->totchannel == 0)
-		return;
-	
-	iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
-
-	for(a=0; a<tree->totchannel; a++) {
-		pchan= tree->pchan[a];
-		bone= pchan->bone;
-		
-		/* set DoF flag */
-		flag= 0;
-		if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
-			flag |= IK_XDOF;
-		if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
-			flag |= IK_YDOF;
-		if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
-			flag |= IK_ZDOF;
-		
-		if(tree->stretch && (pchan->ikstretch > 0.0)) {
-			flag |= IK_TRANS_YDOF;
-			hasstretch = 1;
-		}
-		
-		seg= iktree[a]= IK_CreateSegment(flag);
-		
-		/* find parent */
-		if(a == 0)
-			parent= NULL;
-		else
-			parent= iktree[tree->parent[a]];
-			
-		IK_SetParent(seg, parent);
-			
-		/* get the matrix that transforms from prevbone into this bone */
-		Mat3CpyMat4(R_bonemat, pchan->pose_mat);
-		
-		/* gather transformations for this IK segment */
-		
-		if (pchan->parent)
-			Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
-		else
-			Mat3One(R_parmat);
-		
-		/* bone offset */
-		if (pchan->parent && (a > 0))
-			VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
-		else
-			/* only root bone (a = 0) has no parent */
-			start[0]= start[1]= start[2]= 0.0f;
-		
-		/* change length based on bone size */
-		length= bone->length*VecLength(R_bonemat[1]);
-		
-		/* compute rest basis and its inverse */
-		Mat3CpyMat3(rest_basis, bone->bone_mat);
-		Mat3CpyMat3(irest_basis, bone->bone_mat);
-		Mat3Transp(irest_basis);
-		
-		/* compute basis with rest_basis removed */
-		Mat3Inv(iR_parmat, R_parmat);
-		Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
-		Mat3MulMat3(basis, irest_basis, full_basis);
-		
-		/* basis must be pure rotation */
-		Mat3Ortho(basis);
-		
-		/* transform offset into local bone space */
-		Mat3Ortho(iR_parmat);
-		Mat3MulVecfl(iR_parmat, start);
-		
-		IK_SetTransform(seg, start, rest_basis, basis, length);
-		
-		if (pchan->ikflag & BONE_IK_XLIMIT)
-			IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
-		if (pchan->ikflag & BONE_IK_YLIMIT)
-			IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
-		if (pchan->ikflag & BONE_IK_ZLIMIT)
-			IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
-		
-		IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
-		IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
-		IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
-		
-		if(tree->stretch && (pchan->ikstretch > 0.0f)) {
-			float ikstretch = pchan->ikstretch*pchan->ikstretch;
-			IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0f-ikstretch, 0.99f));
-			IK_SetLimit(seg, IK_TRANS_Y, 0.001f, 1e10);
-		}
-	}
-
-	solver= IK_CreateSolver(iktree[0]);
-
-	/* set solver goals */
-
-	/* first set the goal inverse transform, assuming the root of tree was done ok! */
-	pchan= tree->pchan[0];
-	if (pchan->parent)
-		/* transform goal by parent mat, so this rotation is not part of the
-		   segment's basis. otherwise rotation limits do not work on the
-		   local transform of the segment itself. */
-		Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
-	else
-		Mat4One(rootmat);
-	VECCOPY(rootmat[3], pchan->pose_head);
-	
-	Mat4MulMat4 (imat, rootmat, ob->obmat);
-	Mat4Invert (goalinv, imat);
-	
-	for (target=tree->targets.first; target; target=target->next) {
-		float polepos[3];
-		int poleconstrain= 0;
-		
-		data= (bKinematicConstraint*)target->con->data;
-		
-		/* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
-		 * strictly speaking, it is a posechannel)
-		 */
-		get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
-		
-		/* and set and transform goal */
-		Mat4MulMat4(goal, rootmat, goalinv);
-		
-		VECCOPY(goalpos, goal[3]);
-		Mat3CpyMat4(goalrot, goal);
-		
-		/* same for pole vector target */
-		if(data->poletar) {
-			get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
-			
-			if(data->flag & CONSTRAINT_IK_SETANGLE) {
-				/* don't solve IK when we are setting the pole angle */
-				break;
-			}
-			else {
-				Mat4MulMat4(goal, rootmat, goalinv);
-				VECCOPY(polepos, goal[3]);
-				poleconstrain= 1;
-
-				/* for pole targets, we blend the result of the ik solver
-				 * instead of the target position, otherwise we can't get
-				 * a smooth transition */
-				resultblend= 1;
-				resultinf= target->con->enforce;
-				
-				if(data->flag & CONSTRAINT_IK_GETANGLE) {
-					poleangledata= data;
-					data->flag &= ~CONSTRAINT_IK_GETANGLE;
-				}
-			}
-		}
-
-		/* do we need blending? */
-		if (!resultblend && target->con->enforce!=1.0f) {
-			float q1[4], q2[4], q[4];
-			float fac= target->con->enforce;
-			float mfac= 1.0f-fac;
-			
-			pchan= tree->pchan[target->tip];
-			
-			/* end effector in world space */
-			Mat4CpyMat4(end_pose, pchan->pose_mat);
-			VECCOPY(end_pose[3], pchan->pose_tail);
-			Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
-			
-			/* blend position */
-			goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
-			goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
-			goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
-			
-			/* blend rotation */
-			Mat3ToQuat(goalrot, q1);
-			Mat4ToQuat(world_pose, q2);
-			QuatInterpol(q, q1, q2, mfac);
-			QuatToMat3(q, goalrot);
-		}
-		
-		iktarget= iktree[target->tip];
-		
-		if(data->weight != 0.0f) {
-			if(poleconstrain)
-				IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
-					polepos, data->poleangle*(float)M_PI/180.0f, (poleangledata == data));
-			IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
-		}
-		if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f))
-			if((data->flag & CONSTRAINT_IK_AUTO)==0)
-				IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
-					data->orientweight);
-	}
-
-	/* solve */
-	IK_Solve(solver, 0.0f, tree->iterations);
-
-	if(poleangledata)
-		poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180.0f/(float)M_PI;
-
-	IK_FreeSolver(solver);
-
-	/* gather basis changes */
-	tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
-	if(hasstretch)
-		ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
-	
-	for(a=0; a<tree->totchannel; a++) {
-		IK_GetBasisChange(iktree[a], tree->basis_change[a]);
-		
-		if(hasstretch) {
-			/* have to compensate for scaling received from parent */
-			float parentstretch, stretch;
-			
-			pchan= tree->pchan[a];
-			parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0f;
-			
-			if(tree->stretch && (pchan->ikstretch > 0.0f)) {
-				float trans[3], length;
-				
-				IK_GetTranslationChange(iktree[a], trans);
-				length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
-				
-				ikstretch[a]= (length == 0.0f)? 1.0f: (trans[1]+length)/length;
-			}
-			else
-				ikstretch[a] = 1.0f;
-			
-			stretch= (parentstretch == 0.0f)? 1.0f: ikstretch[a]/parentstretch;
-			
-			VecMulf(tree->basis_change[a][0], stretch);
-			VecMulf(tree->basis_change[a][1], stretch);
-			VecMulf(tree->basis_change[a][2], stretch);
-		}
-
-		if(resultblend && resultinf!=1.0f) {
-			Mat3One(identity);
-			Mat3BlendMat3(tree->basis_change[a], identity,
-				tree->basis_change[a], resultinf);
-		}
-		
-		IK_FreeSegment(iktree[a]);
-	}
-	
-	MEM_freeN(iktree);
-	if(ikstretch) MEM_freeN(ikstretch);
-}
-
-void free_posetree(PoseTree *tree)
-{
-	BLI_freelistN(&tree->targets);
-	if(tree->pchan) MEM_freeN(tree->pchan);
-	if(tree->parent) MEM_freeN(tree->parent);
-	if(tree->basis_change) MEM_freeN(tree->basis_change);
-	MEM_freeN(tree);
-}
-
 /* ********************** THE POSE SOLVER ******************* */
 
 
@@ -2012,41 +1612,6 @@ void chan_calc_mat(bPoseChannel *chan)
 	}
 }
 
-/* transform from bone(b) to bone(b+1), store in chan_mat */
-static void make_dmats(bPoseChannel *pchan)
-{
-	if (pchan->parent) {
-		float iR_parmat[4][4];
-		Mat4Invert(iR_parmat, pchan->parent->pose_mat);
-		Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);	// delta mat
-	}
-	else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
-}
-
-/* applies IK matrix to pchan, IK is done separated */
-/* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
-/* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
-static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
-{
-	float vec[3], ikmat[4][4];
-	
-	Mat4CpyMat3(ikmat, ik_mat);
-	
-	if (pchan->parent)
-		Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
-	else 
-		Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
-
-	/* calculate head */
-	VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
-	/* calculate tail */
-	VECCOPY(vec, pchan->pose_mat[1]);
-	VecMulf(vec, pchan->bone->length);
-	VecAddf(pchan->pose_tail, pchan->pose_head, vec);
-
-	pchan->flag |= POSE_DONE;
-}
-
 /* NLA strip modifiers */
 static void do_strip_modifiers(Scene *scene, Object *armob, Bone *bone, bPoseChannel *pchan)
 {
@@ -2172,7 +1737,7 @@ static void do_strip_modifiers(Scene *scene, Object *armob, Bone *bone, bPoseCha
 
 /* The main armature solver, does all constraints excluding IK */
 /* pchan is validated, as having bone and parent pointer */
-static void where_is_pose_bone(Scene *scene, Object *ob, bPoseChannel *pchan, float ctime)
+void where_is_pose_bone(Scene *scene, Object *ob, bPoseChannel *pchan, float ctime)
 {
 	Bone *bone, *parbone;
 	bPoseChannel *parchan;
@@ -2312,48 +1877,27 @@ void where_is_pose (Scene *scene, Object *ob)
 	else {
 		Mat4Invert(ob->imat, ob->obmat);	// imat is needed 
 
-		/* 1. construct the PoseTrees, clear flags */
+		/* 1. clear flags */
 		for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
-			pchan->flag &= ~(POSE_DONE|POSE_CHAIN);
-			if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
-				initialize_posetree(ob, pchan);	// will attach it to root!
+			pchan->flag &= ~(POSE_DONE|POSE_CHAIN|POSE_IKTREE);
 		}
-		
-		/* 2. the main loop, channels are already hierarchical sorted from root to children */
+		/* 2. construct the IK tree */
+		BIK_initialize_tree(scene, ob, ctime);
+
+		/* 3. the main loop, channels are already hierarchical sorted from root to children */
 		for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
 			
-			/* 3. if we find an IK root, we handle it separated */
-			if(pchan->iktree.first) {
-				while(pchan->iktree.first) {
-					PoseTree *tree= pchan->iktree.first;
-					int a;
-					
-					/* 4. walk over the tree for regular solving */
-					for(a=0; a<tree->totchannel; a++) {
-						if(!(tree->pchan[a]->flag & POSE_DONE))	// successive trees can set the flag
-							where_is_pose_bone(scene, ob, tree->pchan[a], ctime);
-					}
-					/* 5. execute the IK solver */
-					execute_posetree(ob, tree);
-					
-					/* 6. apply the differences to the channels, 
-						  we need to calculate the original differences first */
-					for(a=0; a<tree->totchannel; a++)
-						make_dmats(tree->pchan[a]);
-					
-					for(a=0; a<tree->totchannel; a++)
-						/* sets POSE_DONE */
-						where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
-					
-					/* 7. and free */
-					BLI_remlink(&pchan->iktree, tree);
-					free_posetree(tree);
-				}
+			/* 4. if we find an IK root, we handle it separated */
+			if(pchan->flag & POSE_IKTREE) {
+				BIK_execute_tree(scene, ob, pchan, ctime);
 			}
+			/* 5. otherwise just call the normal solver */
 			else if(!(pchan->flag & POSE_DONE)) {
 				where_is_pose_bone(scene, ob, pchan, ctime);
 			}
 		}
+		/* 6. release the IK tree */
+		BIK_release_tree(scene, ob, ctime);
 	}
 		
 	/* calculating deform matrices */
diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index a3d59720645..b8d6b333674 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -1053,6 +1053,7 @@ static void kinematic_new_data (void *cdata)
 	data->weight= (float)1.0;
 	data->orientweight= (float)1.0;
 	data->iterations = 500;
+	data->dist= (float)1.0;
 	data->flag= CONSTRAINT_IK_TIP|CONSTRAINT_IK_STRETCH|CONSTRAINT_IK_POS;
 }
 
@@ -3643,7 +3644,7 @@ void solve_constraints (ListBase *conlist, bConstraintOb *cob, float ctime)
 		
 		/* these we can skip completely (invalid constraints...) */
 		if (cti == NULL) continue;
-		if (con->flag & CONSTRAINT_DISABLE) continue;
+		if (con->flag & (CONSTRAINT_DISABLE|CONSTRAINT_OFF)) continue;
 		/* these constraints can't be evaluated anyway */
 		if (cti->evaluate_constraint == NULL) continue;
 		/* influence == 0 should be ignored */
diff --git a/source/blender/blenkernel/intern/pointcache.c b/source/blender/blenkernel/intern/pointcache.c
index dfc5b4cd770..e5f89727ab8 100644
--- a/source/blender/blenkernel/intern/pointcache.c
+++ b/source/blender/blenkernel/intern/pointcache.c
@@ -56,6 +56,7 @@
 #include "BKE_smoke.h"
 #include "BKE_softbody.h"
 #include "BKE_utildefines.h"
+#include "BIK_api.h"
 
 #include "BLI_blenlib.h"
 
@@ -2007,6 +2008,9 @@ int BKE_ptcache_object_reset(Scene *scene, Object *ob, int mode)
 		}
 	}
 
+	if (ob->type == OB_ARMATURE)
+		BIK_clear_cache(ob->pose);
+
 	return reset;
 }
 
diff --git a/source/blender/blenkernel/intern/sca.c b/source/blender/blenkernel/intern/sca.c
index de2118af202..5cd554725ff 100644
--- a/source/blender/blenkernel/intern/sca.c
+++ b/source/blender/blenkernel/intern/sca.c
@@ -128,6 +128,9 @@ void init_sensor(bSensor *sens)
 	case SENS_PROPERTY:
 		sens->data= MEM_callocN(sizeof(bPropertySensor), "propsens");
 		break;
+	case SENS_ARMATURE:
+		sens->data= MEM_callocN(sizeof(bArmatureSensor), "armsens");
+		break;
 	case SENS_ACTUATOR:
 		sens->data= MEM_callocN(sizeof(bActuatorSensor), "actsens");
 		break;
@@ -455,6 +458,9 @@ void init_actuator(bActuator *act)
 	case ACT_STATE:
         act->data = MEM_callocN(sizeof( bStateActuator ), "state act");
         break;
+	case ACT_ARMATURE:
+        act->data = MEM_callocN(sizeof( bArmatureActuator ), "armature act");
+        break;
 	default:
 		; /* this is very severe... I cannot make any memory for this        */
 		/* logic brick...                                                    */
@@ -596,6 +602,8 @@ void sca_remove_ob_poin(Object *obt, Object *ob)
 	bEditObjectActuator *eoa;
 	bPropertyActuator *pa;
 	bMessageActuator *ma;
+	bParentActuator *para;
+	bArmatureActuator *aa;
 
 	sens= obt->sensors.first;
 	while(sens) {
@@ -634,7 +642,15 @@ void sca_remove_ob_poin(Object *obt, Object *ob)
 			ma= act->data;
 			if(ma->toObject==ob) ma->toObject= NULL;
 			break;
-
+		case ACT_PARENT:
+			para = act->data;
+			if (para->ob==ob) para->ob = NULL;
+			break;
+		case ACT_ARMATURE:
+			aa = act->data;
+			if (aa->target == ob) aa->target = NULL;
+			if (aa->subtarget == ob) aa->subtarget = NULL;
+			break;
 		}
 		act= act->next;
 	}	
diff --git a/source/blender/blenlib/BLI_ghash.h b/source/blender/blenlib/BLI_ghash.h
index c77e82f0a2b..c9a8b1b841f 100644
--- a/source/blender/blenlib/BLI_ghash.h
+++ b/source/blender/blenlib/BLI_ghash.h
@@ -32,6 +32,10 @@
 #ifndef BLI_GHASH_H
 #define BLI_GHASH_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 struct GHash;
 typedef struct GHash GHash;
 
@@ -125,5 +129,9 @@ int				BLI_ghashutil_strcmp	(void *a, void *b);
 unsigned int	BLI_ghashutil_inthash	(void *ptr);
 int				BLI_ghashutil_intcmp(void *a, void *b);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
 
diff --git a/source/blender/blenloader/intern/readfile.c b/source/blender/blenloader/intern/readfile.c
index d26a2a79f05..204176f64c3 100644
--- a/source/blender/blenloader/intern/readfile.c
+++ b/source/blender/blenloader/intern/readfile.c
@@ -2184,6 +2184,8 @@ static void lib_link_constraints(FileData *fd, ID *id, ListBase *conlist)
 				data = ((bKinematicConstraint*)con->data);
 				data->tar = newlibadr(fd, id->lib, data->tar);
 				data->poletar = newlibadr(fd, id->lib, data->poletar);
+				con->lin_error = 0.f;
+				con->rot_error = 0.f;
 			}
 			break;
 		case CONSTRAINT_TYPE_TRACKTO:
@@ -3615,6 +3617,11 @@ static void lib_link_object(FileData *fd, Main *main)
 				else if(act->type==ACT_STATE) {
 					/* bStateActuator *statea = act->data; */
 				}
+				else if(act->type==ACT_ARMATURE) {
+					bArmatureActuator *arma= act->data;
+					arma->target= newlibadr(fd, ob->id.lib, arma->target);
+					arma->subtarget= newlibadr(fd, ob->id.lib, arma->subtarget);
+				}
 				act= act->next;
 			}
 			
@@ -3676,6 +3683,10 @@ static void direct_link_pose(FileData *fd, bPose *pose)
 		pchan->iktree.first= pchan->iktree.last= NULL;
 		pchan->path= NULL;
 	}
+	pose->ikdata = NULL;
+	if (pose->ikparam != NULL) {
+		pose->ikparam= newdataadr(fd, pose->ikparam);
+	}
 }
 
 static void direct_link_modifiers(FileData *fd, ListBase *lb)
@@ -10579,11 +10590,19 @@ static void expand_object(FileData *fd, Main *mainvar, Object *ob)
 			bObjectActuator *oa= act->data;
 			expand_doit(fd, mainvar, oa->reference);
 		}
+		else if(act->type==ACT_ADD_OBJECT) {
+			bAddObjectActuator *aoa= act->data;
+			expand_doit(fd, mainvar, aoa->ob);
+		}
 		else if(act->type==ACT_SCENE) {
 			bSceneActuator *sa= act->data;
 			expand_doit(fd, mainvar, sa->camera);
 			expand_doit(fd, mainvar, sa->scene);
 		}
+		else if(act->type==ACT_2DFILTER) {
+			bTwoDFilterActuator *tdfa= act->data;
+			expand_doit(fd, mainvar, tdfa->text);
+		}
 		else if(act->type==ACT_ACTION) {
 			bActionActuator *aa= act->data;
 			expand_doit(fd, mainvar, aa->act);
@@ -10600,6 +10619,14 @@ static void expand_object(FileData *fd, Main *mainvar, Object *ob)
 			bMessageActuator *ma= act->data;
 			expand_doit(fd, mainvar, ma->toObject);
 		}
+		else if(act->type==ACT_PARENT) {
+			bParentActuator *pa= act->data;
+			expand_doit(fd, mainvar, pa->ob);
+		}
+		else if(act->type==ACT_ARMATURE) {
+			bArmatureActuator *arma= act->data;
+			expand_doit(fd, mainvar, arma->target);
+		}
 		act= act->next;
 	}
 
diff --git a/source/blender/blenloader/intern/writefile.c b/source/blender/blenloader/intern/writefile.c
index 37d0ee58bb2..fda35d28d0e 100644
--- a/source/blender/blenloader/intern/writefile.c
+++ b/source/blender/blenloader/intern/writefile.c
@@ -712,6 +712,9 @@ static void write_sensors(WriteData *wd, ListBase *lb)
 		case SENS_PROPERTY:
 			writestruct(wd, DATA, "bPropertySensor", 1, sens->data);
 			break;
+		case SENS_ARMATURE:
+			writestruct(wd, DATA, "bArmatureSensor", 1, sens->data);
+			break;
 		case SENS_ACTUATOR:
 			writestruct(wd, DATA, "bActuatorSensor", 1, sens->data);
 			break;
@@ -830,6 +833,9 @@ static void write_actuators(WriteData *wd, ListBase *lb)
 		case ACT_STATE:
 			writestruct(wd, DATA, "bStateActuator", 1, act->data);
 			break;
+		case ACT_ARMATURE:
+			writestruct(wd, DATA, "bArmatureActuator", 1, act->data);
+			break;
 		default:
 			; /* error: don't know how to write this file */
 		}
@@ -1093,8 +1099,16 @@ static void write_pose(WriteData *wd, bPose *pose)
 	for (grp=pose->agroups.first; grp; grp=grp->next) 
 		writestruct(wd, DATA, "bActionGroup", 1, grp);
 
+	/* write IK param */
+	if (pose->ikparam) {
+		const char *structname = get_ikparam_name(pose);
+		if (structname)
+			writestruct(wd, DATA, structname, 1, pose->ikparam);
+	}
+
 	/* Write this pose */
 	writestruct(wd, DATA, "bPose", 1, pose);
+
 }
 
 static void write_defgroups(WriteData *wd, ListBase *defbase)
diff --git a/source/blender/editors/CMakeLists.txt b/source/blender/editors/CMakeLists.txt
index 066d42e723e..d13d7ce2ff2 100644
--- a/source/blender/editors/CMakeLists.txt
+++ b/source/blender/editors/CMakeLists.txt
@@ -40,6 +40,7 @@ SET(INC ../windowmanager
 	../nodes
 	../gpu
 	../blenfont
+	../ikplugin
 )
 
 IF(WITH_GAMEENGINE)
diff --git a/source/blender/editors/armature/editarmature.c b/source/blender/editors/armature/editarmature.c
index 80f8c2b07aa..bc210fbcb54 100644
--- a/source/blender/editors/armature/editarmature.c
+++ b/source/blender/editors/armature/editarmature.c
@@ -2539,6 +2539,8 @@ EditBone *duplicateEditBoneObjects(EditBone *curBone, char *name, ListBase *edit
 					VECCOPY(channew->limitmax, chanold->limitmax);
 					VECCOPY(channew->stiffness, chanold->stiffness);
 					channew->ikstretch= chanold->ikstretch;
+					channew->ikrotweight= chanold->ikrotweight;
+					channew->iklinweight= chanold->iklinweight;
 					
 					/* constraints */
 					listnew = &channew->constraints;
@@ -2642,6 +2644,8 @@ static int armature_duplicate_selected_exec(bContext *C, wmOperator *op)
 								VECCOPY(channew->limitmax, chanold->limitmax);
 								VECCOPY(channew->stiffness, chanold->stiffness);
 								channew->ikstretch= chanold->ikstretch;
+								channew->ikrotweight= chanold->ikrotweight;
+								channew->iklinweight= chanold->iklinweight;
 								
 								/* constraints */
 								listnew = &channew->constraints;
diff --git a/source/blender/editors/armature/poseobject.c b/source/blender/editors/armature/poseobject.c
index 305e153f805..f40476f6f59 100644
--- a/source/blender/editors/armature/poseobject.c
+++ b/source/blender/editors/armature/poseobject.c
@@ -798,6 +798,8 @@ void pose_copy_menu(Scene *scene)
 						VECCOPY(pchan->limitmax, pchanact->limitmax);
 						VECCOPY(pchan->stiffness, pchanact->stiffness);
 						pchan->ikstretch= pchanact->ikstretch;
+						pchan->ikrotweight= pchanact->ikrotweight;
+						pchan->iklinweight= pchanact->iklinweight;
 					}
 						break;
 					case 8: /* Custom Bone Shape */
diff --git a/source/blender/editors/include/ED_object.h b/source/blender/editors/include/ED_object.h
index 67dc6dada5f..363795afeab 100644
--- a/source/blender/editors/include/ED_object.h
+++ b/source/blender/editors/include/ED_object.h
@@ -88,6 +88,8 @@ void object_test_constraints(struct Object *ob);
 
 void ED_object_constraint_rename(struct Object *ob, struct bConstraint *con, char *oldname);
 void ED_object_constraint_set_active(struct Object *ob, struct bConstraint *con);
+void ED_object_constraint_update(struct Object *ob);
+void ED_object_constraint_dependency_update(struct Scene *scene, struct Object *ob);
 
 /* object_lattice.c */
 void mouse_lattice(struct bContext *C, short mval[2], int extend);
diff --git a/source/blender/editors/interface/interface_templates.c b/source/blender/editors/interface/interface_templates.c
index 51678327cdd..31f371c5553 100644
--- a/source/blender/editors/interface/interface_templates.c
+++ b/source/blender/editors/interface/interface_templates.c
@@ -676,6 +676,8 @@ void do_constraint_panels(bContext *C, void *arg, int event)
 	
 	if(ob->type==OB_ARMATURE) DAG_id_flush_update(&ob->id, OB_RECALC_DATA|OB_RECALC_OB);
 	else DAG_id_flush_update(&ob->id, OB_RECALC_OB);
+
+	WM_event_add_notifier(C, NC_OBJECT|ND_CONSTRAINT, ob);
 	
 	// XXX allqueue(REDRAWVIEW3D, 0);
 	// XXX allqueue(REDRAWBUTSOBJECT, 0);
@@ -687,19 +689,15 @@ static void constraint_active_func(bContext *C, void *ob_v, void *con_v)
 	ED_object_constraint_set_active(ob_v, con_v);
 }
 
-static void verify_constraint_name_func (bContext *C, void *con_v, void *name_v)
+static void verify_constraint_name_func (bContext *C, void *con_v, void *dummy)
 {
 	Object *ob= CTX_data_active_object(C);
 	bConstraint *con= con_v;
-	char oldname[32];	
 	
 	if (!con)
 		return;
 	
-	/* put on the stack */
-	BLI_strncpy(oldname, (char *)name_v, 32);
-	
-	ED_object_constraint_rename(ob, con, oldname);
+	ED_object_constraint_rename(ob, con, NULL);
 	ED_object_constraint_set_active(ob, con);
 	// XXX allqueue(REDRAWACTION, 0); 
 }
@@ -904,11 +902,13 @@ static uiLayout *draw_constraint(uiLayout *layout, Object *ob, bConstraint *con)
 					uiDefIconButO(block, BUT, "CONSTRAINT_OT_move_down", WM_OP_INVOKE_DEFAULT, VICON_MOVE_DOWN, xco+width-50+18, yco, 16, 18, "Move constraint down in constraint stack");
 			uiBlockEndAlign(block);
 		}
-		
-		
+	
 		/* Close 'button' - emboss calls here disable drawing of 'button' behind X */
 		uiBlockSetEmboss(block, UI_EMBOSSN);
+			uiBlockBeginAlign(block);
+			uiDefIconButBitS(block, ICONTOGN, CONSTRAINT_OFF, B_CONSTRAINT_TEST, ICON_CHECKBOX_DEHLT, xco+243, yco, 19, 19, &con->flag, 0.0, 0.0, 0.0, 0.0, "enable/disable constraint");
 			uiDefIconButO(block, BUT, "CONSTRAINT_OT_delete", WM_OP_INVOKE_DEFAULT, ICON_X, xco+262, yco, 19, 19, "Delete constraint");
+			uiBlockEndAlign(block);
 		uiBlockSetEmboss(block, UI_EMBOSS);
 	}
 	
diff --git a/source/blender/editors/object/Makefile b/source/blender/editors/object/Makefile
index 70ada46c80f..fd2af305d87 100644
--- a/source/blender/editors/object/Makefile
+++ b/source/blender/editors/object/Makefile
@@ -47,6 +47,7 @@ CPPFLAGS += -I../../makesdna
 CPPFLAGS += -I../../makesrna
 CPPFLAGS += -I../../python
 CPPFLAGS += -I../../imbuf
+CPPFLAGS += -I../../ikplugin
 
 # own include 
 
diff --git a/source/blender/editors/object/SConscript b/source/blender/editors/object/SConscript
index 3371e172a82..6ecc80f2d81 100644
--- a/source/blender/editors/object/SConscript
+++ b/source/blender/editors/object/SConscript
@@ -6,7 +6,7 @@ sources = env.Glob('*.c')
 incs = '../include ../../blenlib ../../blenkernel ../../makesdna ../../imbuf'
 incs += ' ../../windowmanager #/intern/guardedalloc'
 incs += ' #/intern/guardedalloc'
-incs += ' ../../makesrna ../../python'
+incs += ' ../../makesrna ../../python ../../ikplugin'
 
 defs = []
 
diff --git a/source/blender/editors/object/object_constraint.c b/source/blender/editors/object/object_constraint.c
index efdef506331..8c0da354938 100644
--- a/source/blender/editors/object/object_constraint.c
+++ b/source/blender/editors/object/object_constraint.c
@@ -56,6 +56,7 @@
 #include "BKE_object.h"
 #include "BKE_report.h"
 #include "BKE_utildefines.h"
+#include "BIK_api.h"
 
 #ifndef DISABLE_PYTHON
 #include "BPY_extern.h"
@@ -334,6 +335,7 @@ static void test_constraints (Object *owner, const char substring[])
 				 *		optional... otherwise poletarget must exist too or else
 				 *		the constraint is deemed invalid
 				 */
+				/* default IK check ... */
 				if (exist_object(data->tar) == 0) {
 					data->tar = NULL;
 					curcon->flag |= CONSTRAINT_DISABLE;
@@ -355,7 +357,8 @@ static void test_constraints (Object *owner, const char substring[])
 						}
 					}
 				}
-				
+				/* ... can be overwritten here */
+				BIK_test_constraint(owner, curcon);
 				/* targets have already been checked for this */
 				continue;
 			}
@@ -702,6 +705,25 @@ void ED_object_constraint_set_active(Object *ob, bConstraint *con)
 	}
 }
 
+void ED_object_constraint_update(Object *ob)
+{
+
+	if(ob->pose) update_pose_constraint_flags(ob->pose);
+
+	object_test_constraints(ob);
+
+	if(ob->type==OB_ARMATURE) DAG_id_flush_update(&ob->id, OB_RECALC_DATA|OB_RECALC_OB);
+	else DAG_id_flush_update(&ob->id, OB_RECALC_OB);
+}
+
+void ED_object_constraint_dependency_update(Scene *scene, Object *ob)
+{
+	ED_object_constraint_update(ob);
+
+	if(ob->pose) ob->pose->flag |= POSE_RECALC;	// checks & sorts pose channels
+    DAG_scene_sort(scene);
+}
+
 static int constraint_poll(bContext *C)
 {
 	PointerRNA ptr= CTX_data_pointer_get_type(C, "constraint", &RNA_Constraint);
@@ -717,6 +739,10 @@ static int constraint_delete_exec (bContext *C, wmOperator *op)
 	
 	/* remove constraint itself */
 	lb= get_active_constraints(ob);
+	if (BLI_findindex(lb, con) == -1)
+		/* abnormal situation which happens on bone constraint when the armature is not in pose mode */
+		return OPERATOR_CANCELLED;
+
 	free_constraint_data(con);
 	BLI_freelinkN(lb, con);
 	
diff --git a/source/blender/editors/space_logic/logic_window.c b/source/blender/editors/space_logic/logic_window.c
index b99f7b94170..dc8b111821d 100644
--- a/source/blender/editors/space_logic/logic_window.c
+++ b/source/blender/editors/space_logic/logic_window.c
@@ -39,6 +39,9 @@
 #include "DNA_screen_types.h"
 #include "DNA_sensor_types.h"
 #include "DNA_sound_types.h"
+#include "DNA_armature_types.h"
+#include "DNA_constraint_types.h"
+#include "DNA_action_types.h"
 #include "DNA_windowmanager_types.h"
 
 #include "MEM_guardedalloc.h"
@@ -608,6 +611,8 @@ static char *sensor_name(int type)
 		return "Keyboard";
 	case SENS_PROPERTY:
 		return "Property";
+	case SENS_ARMATURE:
+		return "Armature";
 	case SENS_ACTUATOR:
 		return "Actuator";
 	case SENS_DELAY:
@@ -635,7 +640,7 @@ static char *sensor_pup(void)
 	/* the number needs to match defines in game.h */
 	return "Sensors %t|Always %x0|Delay %x13|Keyboard %x3|Mouse %x5|"
 		"Touch %x1|Collision %x6|Near %x2|Radar %x7|"
-		"Property %x4|Random %x8|Ray %x9|Message %x10|Joystick %x11|Actuator %x12";
+		"Property %x4|Random %x8|Ray %x9|Message %x10|Joystick %x11|Actuator %x12|Armature %x14";
 }
 
 static char *controller_name(int type)
@@ -709,6 +714,8 @@ static char *actuator_name(int type)
 		return "Parent";
 	case ACT_STATE:
 		return "State";
+	case ACT_ARMATURE:
+		return "Armature";
 	}
 	return "unknown";
 }
@@ -721,7 +728,7 @@ static char *actuator_pup(Object *owner)
 	switch (owner->type)
 	{
 	case OB_ARMATURE:
-		return "Actuators  %t|Action %x15|Motion %x0|Constraint %x9|Ipo %x1"
+		return "Actuators  %t|Action %x15|Armature %x23|Motion %x0|Constraint %x9|Ipo %x1"
 			"|Camera %x3|Sound %x5|Property %x6|Edit Object %x10"
                         "|Scene %x11|Random %x13|Message %x14|Game %x17"
 			"|Visibility %x18|2D Filter %x19|Parent %x20|State %x22";
@@ -936,6 +943,7 @@ static int get_col_sensor(int type)
 	case SENS_NEAR:			return TH_PANEL; 
 	case SENS_KEYBOARD:		return TH_PANEL;
 	case SENS_PROPERTY:		return TH_PANEL;
+	case SENS_ARMATURE:		return TH_PANEL;
 	case SENS_ACTUATOR:		return TH_PANEL;
 	case SENS_MOUSE:		return TH_PANEL;
 	case SENS_RADAR:		return TH_PANEL;
@@ -1129,12 +1137,51 @@ static void draw_default_sensor_header(bSensor *sens,
 			 "Invert the level (output) of this sensor");
 }
 
-static short draw_sensorbuttons(bSensor *sens, uiBlock *block, short xco, short yco, short width,char* objectname)
+static void check_armature_bone_constraint(Object *ob, char *posechannel, char *constraint)
+{
+	/* check that bone exist in the active object */
+	if (ob->type == OB_ARMATURE && ob->pose) {
+		bPoseChannel *pchan;
+		bPose *pose = ob->pose;
+		for (pchan=pose->chanbase.first; pchan; pchan=pchan->next) {
+			if (!strcmp(pchan->name, posechannel)) {
+				/* found it, now look for constraint channel */
+				bConstraint *con;
+				for (con=pchan->constraints.first; con; con=con->next) {
+					if (!strcmp(con->name, constraint)) {
+						/* found it, all ok */
+						return;						
+					}
+				}
+				/* didn't find constraint, make empty */
+				constraint[0] = 0;
+				return;
+			}
+		}
+	}
+	/* didn't find any */
+	posechannel[0] = 0;
+	constraint[0] = 0;
+}
+
+static void check_armature_sensor(bContext *C, void *arg1_but, void *arg2_sens)
+{
+	bArmatureSensor *sens = arg2_sens;
+	uiBut *but = arg1_but;
+	Object *ob= CTX_data_active_object(C);
+
+	/* check that bone exist in the active object */
+	but->retval = B_REDR;
+	check_armature_bone_constraint(ob, sens->posechannel, sens->constraint);
+}
+
+static short draw_sensorbuttons(Object *ob, bSensor *sens, uiBlock *block, short xco, short yco, short width,char* objectname)
 {
 	bNearSensor      *ns           = NULL;
 	bTouchSensor     *ts           = NULL;
 	bKeyboardSensor  *ks           = NULL;
 	bPropertySensor  *ps           = NULL;
+	bArmatureSensor  *arm          = NULL;
 	bMouseSensor     *ms           = NULL;
 	bCollisionSensor *cs           = NULL;
 	bRadarSensor     *rs           = NULL;
@@ -1360,6 +1407,45 @@ static short draw_sensorbuttons(bSensor *sens, uiBlock *block, short xco, short
 			yco-= ysize;
 			break;
 		}
+	case SENS_ARMATURE:
+		{
+			ysize= 70;
+			
+			glRects(xco, yco-ysize, xco+width, yco);
+			uiEmboss((float)xco, (float)yco-ysize,
+				(float)xco+width, (float)yco, 1);
+			
+			draw_default_sensor_header(sens, block, xco, yco, width);
+			arm= sens->data;
+
+			if (ob->type == OB_ARMATURE) {
+				uiBlockBeginAlign(block);
+				but = uiDefBut(block, TEX, 1, "Bone: ",
+						(xco+10), (yco-44), (width-20)/2, 19,
+						arm->posechannel, 0, 31, 0, 0,
+						"Bone on which you want to check a constraint");
+				uiButSetFunc(but, check_armature_sensor, but, arm);
+				but = uiDefBut(block, TEX, 1, "Cons: ",
+						(xco+10)+(width-20)/2, (yco-44), (width-20)/2, 19,
+						arm->constraint, 0, 31, 0, 0,
+						"Name of the constraint you want to control");
+				uiButSetFunc(but, check_armature_sensor, but, arm);
+				uiBlockEndAlign(block);
+
+				str= "Type %t|State changed %x0|Lin error below %x1|Lin error above %x2|Rot error below %x3|Rot error above %x4"; 
+
+				uiDefButI(block, MENU, B_REDR, str,			xco+10,yco-66,0.4*(width-20), 19,
+					&arm->type, 0, 31, 0, 0, "Type");
+			
+				if (arm->type != SENS_ARM_STATE_CHANGED)
+				{
+					uiDefButF(block, NUM, 1, "Value: ",		xco+10+0.4*(width-20),yco-66,0.6*(width-20), 19,
+					&arm->value, -10000.0, 10000.0, 100, 0, "Test the error against this value");
+				}
+			}
+			yco-= ysize;
+			break;
+		}
 	case SENS_ACTUATOR:
 		{
 			ysize= 48;
@@ -1694,6 +1780,7 @@ static int get_col_actuator(int type)
 	case ACT_VISIBILITY:		return TH_PANEL;
 	case ACT_CONSTRAINT:		return TH_PANEL;
 	case ACT_STATE:			return TH_PANEL;
+	case ACT_ARMATURE:			return TH_PANEL;
 	default:				return TH_PANEL;
 	}
 }
@@ -1774,6 +1861,18 @@ static void check_state_mask(bContext *C, void *arg1_but, void *arg2_mask)
 	but->retval = B_REDR;
 }
 
+static void check_armature_actuator(bContext *C, void *arg1_but, void *arg2_act)
+{
+	bArmatureActuator *act = arg2_act;
+	uiBut *but = arg1_but;
+	Object *ob= CTX_data_active_object(C);
+
+	/* check that bone exist in the active object */
+	but->retval = B_REDR;
+	check_armature_bone_constraint(ob, act->posechannel, act->constraint);
+}
+
+
 static short draw_actuatorbuttons(Object *ob, bActuator *act, uiBlock *block, short xco, short yco, short width)
 {
 	bSoundActuator      *sa      = NULL;
@@ -1793,6 +1892,7 @@ static short draw_actuatorbuttons(Object *ob, bActuator *act, uiBlock *block, sh
 	bTwoDFilterActuator	*tdfa	 = NULL;
 	bParentActuator     *parAct  = NULL;
 	bStateActuator		*staAct  = NULL;
+	bArmatureActuator   *armAct  = NULL;
 	
 	float *fp;
 	short ysize = 0, wval;
@@ -2820,6 +2920,48 @@ static short draw_actuatorbuttons(Object *ob, bActuator *act, uiBlock *block, sh
 
   		yco-= ysize; 
   		break; 
+	case ACT_ARMATURE:
+  		armAct = act->data; 
+
+		if (ob->type == OB_ARMATURE) {
+			str= "Constraint %t|Run armature %x0|Enable %x1|Disable %x2|Set target %x3|Set weight %x4";
+			uiDefButI(block, MENU, B_REDR, str,		xco+5, yco-24, (width-10)*0.35, 19, &armAct->type, 0.0, 0.0, 0, 0, ""); 
+
+			switch (armAct->type) {
+			case ACT_ARM_RUN:
+				ysize = 28;
+				break;
+			default:
+				uiBlockBeginAlign(block);
+				but = uiDefBut(block, TEX, 1, "Bone: ",
+						(xco+5), (yco-44), (width-10)/2, 19,
+						armAct->posechannel, 0, 31, 0, 0,
+						"Bone on which the constraint is defined");
+				uiButSetFunc(but, check_armature_actuator, but, armAct);
+				but = uiDefBut(block, TEX, 1, "Cons: ",
+						(xco+5)+(width-10)/2, (yco-44), (width-10)/2, 19,
+						armAct->constraint, 0, 31, 0, 0,
+						"Name of the constraint you want to controle");
+				uiButSetFunc(but, check_armature_actuator, but, armAct);
+				uiBlockEndAlign(block);
+				ysize = 48;
+				switch (armAct->type) {
+				case ACT_ARM_SETTARGET:
+					uiDefIDPoinBut(block, test_obpoin_but, ID_OB, 1, "Target: ",		xco+5, yco-64, (width-10), 19, &(armAct->target), "Set this object as the target of the constraint"); 
+					uiDefIDPoinBut(block, test_obpoin_but, ID_OB, 1, "Secondary Target: ",		xco+5, yco-84, (width-10), 19, &(armAct->subtarget), "Set this object as the secondary target of the constraint (only IK polar target at the moment)"); 
+					ysize += 40;
+					break;
+				case ACT_ARM_SETWEIGHT:
+					uiDefButF(block, NUM, B_REDR, "Weight:", xco+5+(width-10)*0.35,yco-24,(width-10)*0.65,19,&armAct->weight,0.0,1.0,0.0,0.0,"Set weight of this constraint");
+					break;
+				}
+			}
+  		}
+		glRects(xco, yco-ysize, xco+width, yco); 
+		uiEmboss((float)xco, (float)yco-ysize, (float)xco+width, (float)yco, 1); 
+  		yco-= ysize; 
+  		break; 
+
  	default:
 		ysize= 4;
 
@@ -3334,7 +3476,7 @@ void logic_buttons(bContext *C, ARegion *ar)
 						uiButSetFunc(but, make_unique_prop_names_cb, sens->name, (void*) 0);
 
 						sens->otype= sens->type;
-						yco= draw_sensorbuttons(sens, block, xco, yco, width,ob->id.name);
+						yco= draw_sensorbuttons(ob, sens, block, xco, yco, width,ob->id.name);
 						if(yco-6 < ycoo) ycoo= (yco+ycoo-20)/2;
 					}
 					else {
diff --git a/source/blender/ikplugin/BIK_api.h b/source/blender/ikplugin/BIK_api.h
new file mode 100644
index 00000000000..a259d0aa62a
--- /dev/null
+++ b/source/blender/ikplugin/BIK_api.h
@@ -0,0 +1,93 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef BIK_API_H
+#define BIK_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct Object;
+struct bPoseChannel;
+struct bPose;
+struct bArmature;
+struct Scene;
+struct bConstraint;
+
+enum BIK_ParamType {
+	BIK_PARAM_TYPE_FLOAT = 0,
+	BIK_PARAM_TYPE_INT,
+	BIK_PARAM_TYPE_STRING,
+};
+
+struct BIK_ParamValue {
+	short type;			/* BIK_PARAM_TYPE_.. */
+	short length;		/* for string, does not include terminating 0 */
+	union {
+		float f[8];
+		int   i[8];
+		char  s[32];
+	} value;		
+};
+typedef struct BIK_ParamValue BIK_ParamValue;
+
+void BIK_initialize_tree(struct Scene *scene, struct Object *ob, float ctime);
+void BIK_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime);
+void BIK_release_tree(struct Scene *scene, struct Object *ob, float ctime);
+void BIK_clear_data(struct bPose *pose);
+void BIK_clear_cache(struct bPose *pose);
+void BIK_update_param(struct bPose *pose);
+void BIK_test_constraint(struct Object *ob, struct bConstraint *cons);
+// not yet implemented
+int BIK_get_constraint_param(struct bPose *pose, struct bConstraint *cons, int id, BIK_ParamValue *value);
+int BIK_get_channel_param(struct bPose *pose, struct bPoseChannel *pchan, int id, BIK_ParamValue *value);
+int BIK_get_solver_param(struct bPose *pose, struct bPoseChannel *pchan, int id, BIK_ParamValue *value);
+
+// number of solver available
+// 0 = iksolver
+// 1 = iTaSC
+#define BIK_SOLVER_COUNT		2
+
+/* for use in BIK_get_constraint_param */
+#define BIK_PARAM_CONSTRAINT_ERROR		0
+
+/* for use in BIK_get_channel_param */
+#define BIK_PARAM_CHANNEL_JOINT			0
+
+/* for use in BIK_get_solver_param */
+#define BIK_PARAM_SOLVER_RANK			0
+#define BIK_PARAM_SOLVER_ITERATION		1
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BIK_API_H
+
diff --git a/source/blender/ikplugin/CMakeLists.txt b/source/blender/ikplugin/CMakeLists.txt
new file mode 100644
index 00000000000..5790d4ef12f
--- /dev/null
+++ b/source/blender/ikplugin/CMakeLists.txt
@@ -0,0 +1,35 @@
+# $Id: CMakeLists.txt 20156 2009-05-11 16:31:30Z 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) 2006, Blender Foundation
+# All rights reserved.
+#
+# The Original Code is: all of this file.
+#
+# Contributor(s): Jacques Beaurain.
+#
+# ***** END GPL LICENSE BLOCK *****
+
+FILE(GLOB SRC intern/*.c intern/*.cpp)
+
+SET(INC 
+  ../../../intern/guardedalloc ../../../intern/iksolver/extern
+  ../../../intern/itasc ../../../extern/Eigen2
+  ../blenlib ../makesdna ../blenkernel ../include ../ikplugin
+)
+
+BLENDERLIB(bf_ikplugin "${SRC}" "${INC}")
diff --git a/source/blender/ikplugin/Makefile b/source/blender/ikplugin/Makefile
new file mode 100644
index 00000000000..370ed418464
--- /dev/null
+++ b/source/blender/ikplugin/Makefile
@@ -0,0 +1,31 @@
+# ***** 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): none yet.
+#
+# ***** END GPL LICENSE BLOCK *****
+#
+# Bounces make to subdirectories.
+
+SOURCEDIR = source/blender/ikplugin
+DIRS = intern
+
+include nan_subdirs.mk
diff --git a/source/blender/ikplugin/SConscript b/source/blender/ikplugin/SConscript
new file mode 100644
index 00000000000..a745a93077a
--- /dev/null
+++ b/source/blender/ikplugin/SConscript
@@ -0,0 +1,9 @@
+#!/usr/bin/python
+Import ('env')
+
+sources = env.Glob('intern/*.c') + env.Glob('intern/*.cpp')
+
+incs = '#/intern/guardedalloc #/intern/iksolver/extern ../makesdna ../blenlib'
+incs += ' ../blenkernel ../include ../ikplugin #/intern/itasc #/extern/Eigen2'
+
+env.BlenderLib ( 'bf_ikplugin', sources, Split(incs), [], libtype=['core','player'], priority=[180, 190] )
diff --git a/source/blender/ikplugin/intern/Makefile b/source/blender/ikplugin/intern/Makefile
new file mode 100644
index 00000000000..9254b65b7b7
--- /dev/null
+++ b/source/blender/ikplugin/intern/Makefile
@@ -0,0 +1,49 @@
+# ***** 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): none yet.
+#
+# ***** END GPL LICENSE BLOCK *****
+#
+#
+
+LIBNAME = ikplugin
+DIR = $(OCGDIR)/blender/ikplugin
+
+include nan_compile.mk
+
+CFLAGS += $(LEVEL_1_C_WARNINGS)
+CFLAGS += -I$(NAN_GUARDEDALLOC)/include
+CFLAGS += -I../../makesdna
+CFLAGS += -I../../blenkernel
+CFLAGS += -I../../blenlib
+CFLAGS += -I../../include
+CFLAGS += -I../../../intern/itasc
+CFLAGS += -I../../../extern/Eigen2
+CFLAGS += -I..
+
+CPPFLAGS += -I$(NAN_GUARDEDALLOC)/include
+CPPFLAGS += -I$(NAN_IKSOLVER)/include
+CPPFLAGS += -I../../makesdna
+CPPFLAGS += -I../../blenkernel
+CPPFLAGS += -I../../blenlib
+CPPFLAGS += -I../../include
+CPPFLAGS += -I..
diff --git a/source/blender/ikplugin/intern/ikplugin_api.c b/source/blender/ikplugin/intern/ikplugin_api.c
new file mode 100644
index 00000000000..714843fc5e5
--- /dev/null
+++ b/source/blender/ikplugin/intern/ikplugin_api.c
@@ -0,0 +1,140 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include "MEM_guardedalloc.h"
+
+#include "BIK_api.h"
+#include "BLI_blenlib.h"
+#include "BLI_arithb.h"
+
+#include "BKE_armature.h"
+#include "BKE_utildefines.h"
+#include "DNA_object_types.h"
+#include "DNA_action_types.h"
+#include "DNA_scene_types.h"
+#include "DNA_constraint_types.h"
+#include "DNA_armature_types.h"
+
+#include "ikplugin_api.h"
+#include "iksolver_plugin.h"
+#include "itasc_plugin.h"
+
+
+static IKPlugin ikplugin_tab[BIK_SOLVER_COUNT] = {
+	/* Legacy IK solver */
+	{
+		iksolver_initialize_tree,
+		iksolver_execute_tree,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+	},
+	/* iTaSC IK solver */
+	{
+		itasc_initialize_tree,
+		itasc_execute_tree,
+		itasc_release_tree,
+		itasc_clear_data,
+		itasc_clear_cache,
+		itasc_update_param,
+		itasc_test_constraint,
+	}
+};
+
+
+static IKPlugin *get_plugin(bPose *pose)
+{
+	IKPlugin *plugin;
+
+	if (!pose || pose->iksolver < 0 || pose->iksolver >= BIK_SOLVER_COUNT)
+		return NULL;
+
+	return &ikplugin_tab[pose->iksolver];
+}
+
+/*----------------------------------------*/
+/* Plugin API							  */
+
+void BIK_initialize_tree(Scene *scene, Object *ob, float ctime) 
+{
+	IKPlugin *plugin = get_plugin(ob->pose);
+
+	if (plugin && plugin->initialize_tree_func)
+		plugin->initialize_tree_func(scene, ob, ctime);
+}
+
+void BIK_execute_tree(struct Scene *scene, Object *ob, bPoseChannel *pchan, float ctime) 
+{
+	IKPlugin *plugin = get_plugin(ob->pose);
+
+	if (plugin && plugin->execute_tree_func)
+		plugin->execute_tree_func(scene, ob, pchan, ctime);
+}
+
+void BIK_release_tree(struct Scene *scene, Object *ob, float ctime) 
+{
+	IKPlugin *plugin = get_plugin(ob->pose);
+
+	if (plugin && plugin->release_tree_func)
+		plugin->release_tree_func(scene, ob, ctime);
+}
+
+void BIK_clear_data(struct bPose *pose)
+{
+	IKPlugin *plugin = get_plugin(pose);
+
+	if (plugin && plugin->remove_armature_func)
+		plugin->remove_armature_func(pose);
+}
+
+void BIK_clear_cache(struct bPose *pose)
+{
+	IKPlugin *plugin = get_plugin(pose);
+
+	if (plugin && plugin->clear_cache)
+		plugin->clear_cache(pose);
+}
+
+void BIK_update_param(struct bPose *pose)
+{
+	IKPlugin *plugin = get_plugin(pose);
+
+	if (plugin && plugin->update_param)
+		plugin->update_param(pose);
+}
+
+void BIK_test_constraint(struct Object *ob, struct bConstraint *cons)
+{
+	IKPlugin *plugin = get_plugin(ob->pose);
+
+	if (plugin && plugin->test_constraint)
+		plugin->test_constraint(ob, cons);
+}
diff --git a/source/blender/ikplugin/intern/ikplugin_api.h b/source/blender/ikplugin/intern/ikplugin_api.h
new file mode 100644
index 00000000000..cc4dff4ec75
--- /dev/null
+++ b/source/blender/ikplugin/intern/ikplugin_api.h
@@ -0,0 +1,60 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef IKPLUGIN_API_H
+#define IKPLUGIN_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct Object;
+struct bPoseChannel;
+struct bArmature;
+struct Scene;
+
+
+struct IKPlugin {
+	void (*initialize_tree_func)(struct Scene *scene, struct Object *ob, float ctime);
+	void (*execute_tree_func)(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime);
+	void (*release_tree_func)(struct Scene *scene, struct Object *ob, float ctime);
+	void (*remove_armature_func)(struct bPose *pose);
+	void (*clear_cache)(struct bPose *pose);
+	void (*update_param)(struct bPose *pose);
+	void (*test_constraint)(struct Object *ob, struct bConstraint *cons);
+};
+
+typedef struct IKPlugin IKPlugin;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // IKPLUGIN_API_H
+
diff --git a/source/blender/ikplugin/intern/iksolver_plugin.c b/source/blender/ikplugin/intern/iksolver_plugin.c
new file mode 100644
index 00000000000..262185fef1b
--- /dev/null
+++ b/source/blender/ikplugin/intern/iksolver_plugin.c
@@ -0,0 +1,527 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include "MEM_guardedalloc.h"
+
+#include "BIK_api.h"
+#include "BLI_blenlib.h"
+#include "BLI_arithb.h"
+
+#include "BKE_armature.h"
+#include "BKE_utildefines.h"
+#include "DNA_object_types.h"
+#include "DNA_action_types.h"
+#include "DNA_constraint_types.h"
+#include "DNA_armature_types.h"
+
+#include "IK_solver.h"
+#include "iksolver_plugin.h"
+
+/* ********************** THE IK SOLVER ******************* */
+
+/* allocates PoseTree, and links that to root bone/channel */
+/* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */
+static void initialize_posetree(struct Object *ob, bPoseChannel *pchan_tip)
+{
+	bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
+	PoseTree *tree;
+	PoseTarget *target;
+	bConstraint *con;
+	bKinematicConstraint *data;
+	int a, segcount= 0, size, newsize, *oldparent, parent;
+	
+	/* find IK constraint, and validate it */
+	for(con= pchan_tip->constraints.first; con; con= con->next) {
+		if(con->type==CONSTRAINT_TYPE_KINEMATIC) {
+			data=(bKinematicConstraint*)con->data;
+			if (data->flag & CONSTRAINT_IK_AUTO) break;
+			if (data->tar==NULL) continue;
+			if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue;
+			if ((con->flag & (CONSTRAINT_DISABLE|CONSTRAINT_OFF))==0 && (con->enforce!=0.0)) break;
+		}
+	}
+	if(con==NULL) return;
+	
+	/* exclude tip from chain? */
+	if(!(data->flag & CONSTRAINT_IK_TIP))
+		pchan_tip= pchan_tip->parent;
+	
+	/* Find the chain's root & count the segments needed */
+	for (curchan = pchan_tip; curchan; curchan=curchan->parent){
+		pchan_root = curchan;
+		
+		curchan->flag |= POSE_CHAIN;	// don't forget to clear this
+		chanlist[segcount]=curchan;
+		segcount++;
+		
+		if(segcount==data->rootbone || segcount>255) break; // 255 is weak
+	}
+	if (!segcount) return;
+
+	/* setup the chain data */
+	
+	/* we make tree-IK, unless all existing targets are in this chain */
+	for(tree= pchan_root->iktree.first; tree; tree= tree->next) {
+		for(target= tree->targets.first; target; target= target->next) {
+			curchan= tree->pchan[target->tip];
+			if(curchan->flag & POSE_CHAIN)
+				curchan->flag &= ~POSE_CHAIN;
+			else
+				break;
+		}
+		if(target) break;
+	}
+
+	/* create a target */
+	target= MEM_callocN(sizeof(PoseTarget), "posetarget");
+	target->con= con;
+	pchan_tip->flag &= ~POSE_CHAIN;
+
+	if(tree==NULL) {
+		/* make new tree */
+		tree= MEM_callocN(sizeof(PoseTree), "posetree");
+
+		tree->iterations= data->iterations;
+		tree->totchannel= segcount;
+		tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
+		
+		tree->pchan= MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
+		tree->parent= MEM_callocN(segcount*sizeof(int), "ik tree parent");
+		for(a=0; a<segcount; a++) {
+			tree->pchan[a]= chanlist[segcount-a-1];
+			tree->parent[a]= a-1;
+		}
+		target->tip= segcount-1;
+		
+		/* AND! link the tree to the root */
+		BLI_addtail(&pchan_root->iktree, tree);
+	}
+	else {
+		tree->iterations= MAX2(data->iterations, tree->iterations);
+		tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
+
+		/* skip common pose channels and add remaining*/
+		size= MIN2(segcount, tree->totchannel);
+		for(a=0; a<size && tree->pchan[a]==chanlist[segcount-a-1]; a++);
+		parent= a-1;
+
+		segcount= segcount-a;
+		target->tip= tree->totchannel + segcount - 1;
+
+		if (segcount > 0) {
+			/* resize array */
+			newsize= tree->totchannel + segcount;
+			oldchan= tree->pchan;
+			oldparent= tree->parent;
+
+			tree->pchan= MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
+			tree->parent= MEM_callocN(newsize*sizeof(int), "ik tree parent");
+			memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
+			memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
+			MEM_freeN(oldchan);
+			MEM_freeN(oldparent);
+
+			/* add new pose channels at the end, in reverse order */
+			for(a=0; a<segcount; a++) {
+				tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
+				tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
+			}
+			tree->parent[tree->totchannel]= parent;
+			
+			tree->totchannel= newsize;
+		}
+
+		/* move tree to end of list, for correct evaluation order */
+		BLI_remlink(&pchan_root->iktree, tree);
+		BLI_addtail(&pchan_root->iktree, tree);
+	}
+
+	/* add target to the tree */
+	BLI_addtail(&tree->targets, target);
+	/* mark root channel having an IK tree */
+	pchan_root->flag |= POSE_IKTREE;
+}
+
+
+/* transform from bone(b) to bone(b+1), store in chan_mat */
+static void make_dmats(bPoseChannel *pchan)
+{
+	if (pchan->parent) {
+		float iR_parmat[4][4];
+		Mat4Invert(iR_parmat, pchan->parent->pose_mat);
+		Mat4MulMat4(pchan->chan_mat,  pchan->pose_mat, iR_parmat);	// delta mat
+	}
+	else Mat4CpyMat4(pchan->chan_mat, pchan->pose_mat);
+}
+
+/* applies IK matrix to pchan, IK is done separated */
+/* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */
+/* to make this work, the diffmats have to be precalculated! Stored in chan_mat */
+static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3])   // nr = to detect if this is first bone
+{
+	float vec[3], ikmat[4][4];
+	
+	Mat4CpyMat3(ikmat, ik_mat);
+	
+	if (pchan->parent)
+		Mat4MulSerie(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL);
+	else 
+		Mat4MulMat4(pchan->pose_mat, ikmat, pchan->chan_mat);
+
+	/* calculate head */
+	VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
+	/* calculate tail */
+	VECCOPY(vec, pchan->pose_mat[1]);
+	VecMulf(vec, pchan->bone->length);
+	VecAddf(pchan->pose_tail, pchan->pose_head, vec);
+
+	pchan->flag |= POSE_DONE;
+}
+
+
+/* called from within the core where_is_pose loop, all animsystems and constraints
+were executed & assigned. Now as last we do an IK pass */
+static void execute_posetree(Object *ob, PoseTree *tree)
+{
+	float R_parmat[3][3], identity[3][3];
+	float iR_parmat[3][3];
+	float R_bonemat[3][3];
+	float goalrot[3][3], goalpos[3];
+	float rootmat[4][4], imat[4][4];
+	float goal[4][4], goalinv[4][4];
+	float irest_basis[3][3], full_basis[3][3];
+	float end_pose[4][4], world_pose[4][4];
+	float length, basis[3][3], rest_basis[3][3], start[3], *ikstretch=NULL;
+	float resultinf=0.0f;
+	int a, flag, hasstretch=0, resultblend=0;
+	bPoseChannel *pchan;
+	IK_Segment *seg, *parent, **iktree, *iktarget;
+	IK_Solver *solver;
+	PoseTarget *target;
+	bKinematicConstraint *data, *poleangledata=NULL;
+	Bone *bone;
+
+	if (tree->totchannel == 0)
+		return;
+	
+	iktree= MEM_mallocN(sizeof(void*)*tree->totchannel, "ik tree");
+
+	for(a=0; a<tree->totchannel; a++) {
+		pchan= tree->pchan[a];
+		bone= pchan->bone;
+		
+		/* set DoF flag */
+		flag= 0;
+		if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP))
+			flag |= IK_XDOF;
+		if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP))
+			flag |= IK_YDOF;
+		if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP))
+			flag |= IK_ZDOF;
+		
+		if(tree->stretch && (pchan->ikstretch > 0.0)) {
+			flag |= IK_TRANS_YDOF;
+			hasstretch = 1;
+		}
+		
+		seg= iktree[a]= IK_CreateSegment(flag);
+		
+		/* find parent */
+		if(a == 0)
+			parent= NULL;
+		else
+			parent= iktree[tree->parent[a]];
+			
+		IK_SetParent(seg, parent);
+			
+		/* get the matrix that transforms from prevbone into this bone */
+		Mat3CpyMat4(R_bonemat, pchan->pose_mat);
+		
+		/* gather transformations for this IK segment */
+		
+		if (pchan->parent)
+			Mat3CpyMat4(R_parmat, pchan->parent->pose_mat);
+		else
+			Mat3One(R_parmat);
+		
+		/* bone offset */
+		if (pchan->parent && (a > 0))
+			VecSubf(start, pchan->pose_head, pchan->parent->pose_tail);
+		else
+			/* only root bone (a = 0) has no parent */
+			start[0]= start[1]= start[2]= 0.0f;
+		
+		/* change length based on bone size */
+		length= bone->length*VecLength(R_bonemat[1]);
+		
+		/* compute rest basis and its inverse */
+		Mat3CpyMat3(rest_basis, bone->bone_mat);
+		Mat3CpyMat3(irest_basis, bone->bone_mat);
+		Mat3Transp(irest_basis);
+		
+		/* compute basis with rest_basis removed */
+		Mat3Inv(iR_parmat, R_parmat);
+		Mat3MulMat3(full_basis, iR_parmat, R_bonemat);
+		Mat3MulMat3(basis, irest_basis, full_basis);
+		
+		/* basis must be pure rotation */
+		Mat3Ortho(basis);
+		
+		/* transform offset into local bone space */
+		Mat3Ortho(iR_parmat);
+		Mat3MulVecfl(iR_parmat, start);
+		
+		IK_SetTransform(seg, start, rest_basis, basis, length);
+		
+		if (pchan->ikflag & BONE_IK_XLIMIT)
+			IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]);
+		if (pchan->ikflag & BONE_IK_YLIMIT)
+			IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]);
+		if (pchan->ikflag & BONE_IK_ZLIMIT)
+			IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]);
+		
+		IK_SetStiffness(seg, IK_X, pchan->stiffness[0]);
+		IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]);
+		IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]);
+		
+		if(tree->stretch && (pchan->ikstretch > 0.0)) {
+			float ikstretch = pchan->ikstretch*pchan->ikstretch;
+			IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0-ikstretch, 0.99));
+			IK_SetLimit(seg, IK_TRANS_Y, 0.001, 1e10);
+		}
+	}
+
+	solver= IK_CreateSolver(iktree[0]);
+
+	/* set solver goals */
+
+	/* first set the goal inverse transform, assuming the root of tree was done ok! */
+	pchan= tree->pchan[0];
+	if (pchan->parent)
+		/* transform goal by parent mat, so this rotation is not part of the
+		   segment's basis. otherwise rotation limits do not work on the
+		   local transform of the segment itself. */
+		Mat4CpyMat4(rootmat, pchan->parent->pose_mat);
+	else
+		Mat4One(rootmat);
+	VECCOPY(rootmat[3], pchan->pose_head);
+	
+	Mat4MulMat4 (imat, rootmat, ob->obmat);
+	Mat4Invert (goalinv, imat);
+	
+	for (target=tree->targets.first; target; target=target->next) {
+		float polepos[3];
+		int poleconstrain= 0;
+		
+		data= (bKinematicConstraint*)target->con->data;
+		
+		/* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though
+		 * strictly speaking, it is a posechannel)
+		 */
+		get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
+		
+		/* and set and transform goal */
+		Mat4MulMat4(goal, rootmat, goalinv);
+		
+		VECCOPY(goalpos, goal[3]);
+		Mat3CpyMat4(goalrot, goal);
+		
+		/* same for pole vector target */
+		if(data->poletar) {
+			get_constraint_target_matrix(target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
+			
+			if(data->flag & CONSTRAINT_IK_SETANGLE) {
+				/* don't solve IK when we are setting the pole angle */
+				break;
+			}
+			else {
+				Mat4MulMat4(goal, rootmat, goalinv);
+				VECCOPY(polepos, goal[3]);
+				poleconstrain= 1;
+
+				/* for pole targets, we blend the result of the ik solver
+				 * instead of the target position, otherwise we can't get
+				 * a smooth transition */
+				resultblend= 1;
+				resultinf= target->con->enforce;
+				
+				if(data->flag & CONSTRAINT_IK_GETANGLE) {
+					poleangledata= data;
+					data->flag &= ~CONSTRAINT_IK_GETANGLE;
+				}
+			}
+		}
+
+		/* do we need blending? */
+		if (!resultblend && target->con->enforce!=1.0) {
+			float q1[4], q2[4], q[4];
+			float fac= target->con->enforce;
+			float mfac= 1.0-fac;
+			
+			pchan= tree->pchan[target->tip];
+			
+			/* end effector in world space */
+			Mat4CpyMat4(end_pose, pchan->pose_mat);
+			VECCOPY(end_pose[3], pchan->pose_tail);
+			Mat4MulSerie(world_pose, goalinv, ob->obmat, end_pose, 0, 0, 0, 0, 0);
+			
+			/* blend position */
+			goalpos[0]= fac*goalpos[0] + mfac*world_pose[3][0];
+			goalpos[1]= fac*goalpos[1] + mfac*world_pose[3][1];
+			goalpos[2]= fac*goalpos[2] + mfac*world_pose[3][2];
+			
+			/* blend rotation */
+			Mat3ToQuat(goalrot, q1);
+			Mat4ToQuat(world_pose, q2);
+			QuatInterpol(q, q1, q2, mfac);
+			QuatToMat3(q, goalrot);
+		}
+		
+		iktarget= iktree[target->tip];
+		
+		if(data->weight != 0.0) {
+			if(poleconstrain)
+				IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos,
+					polepos, data->poleangle*M_PI/180, (poleangledata == data));
+			IK_SolverAddGoal(solver, iktarget, goalpos, data->weight);
+		}
+		if((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0))
+			if((data->flag & CONSTRAINT_IK_AUTO)==0)
+				IK_SolverAddGoalOrientation(solver, iktarget, goalrot,
+					data->orientweight);
+	}
+
+	/* solve */
+	IK_Solve(solver, 0.0f, tree->iterations);
+
+	if(poleangledata)
+		poleangledata->poleangle= IK_SolverGetPoleAngle(solver)*180/M_PI;
+
+	IK_FreeSolver(solver);
+
+	/* gather basis changes */
+	tree->basis_change= MEM_mallocN(sizeof(float[3][3])*tree->totchannel, "ik basis change");
+	if(hasstretch)
+		ikstretch= MEM_mallocN(sizeof(float)*tree->totchannel, "ik stretch");
+	
+	for(a=0; a<tree->totchannel; a++) {
+		IK_GetBasisChange(iktree[a], tree->basis_change[a]);
+		
+		if(hasstretch) {
+			/* have to compensate for scaling received from parent */
+			float parentstretch, stretch;
+			
+			pchan= tree->pchan[a];
+			parentstretch= (tree->parent[a] >= 0)? ikstretch[tree->parent[a]]: 1.0;
+			
+			if(tree->stretch && (pchan->ikstretch > 0.0)) {
+				float trans[3], length;
+				
+				IK_GetTranslationChange(iktree[a], trans);
+				length= pchan->bone->length*VecLength(pchan->pose_mat[1]);
+				
+				ikstretch[a]= (length == 0.0)? 1.0: (trans[1]+length)/length;
+			}
+			else
+				ikstretch[a] = 1.0;
+			
+			stretch= (parentstretch == 0.0)? 1.0: ikstretch[a]/parentstretch;
+			
+			VecMulf(tree->basis_change[a][0], stretch);
+			VecMulf(tree->basis_change[a][1], stretch);
+			VecMulf(tree->basis_change[a][2], stretch);
+		}
+
+		if(resultblend && resultinf!=1.0f) {
+			Mat3One(identity);
+			Mat3BlendMat3(tree->basis_change[a], identity,
+				tree->basis_change[a], resultinf);
+		}
+		
+		IK_FreeSegment(iktree[a]);
+	}
+	
+	MEM_freeN(iktree);
+	if(ikstretch) MEM_freeN(ikstretch);
+}
+
+static void free_posetree(PoseTree *tree)
+{
+	BLI_freelistN(&tree->targets);
+	if(tree->pchan) MEM_freeN(tree->pchan);
+	if(tree->parent) MEM_freeN(tree->parent);
+	if(tree->basis_change) MEM_freeN(tree->basis_change);
+	MEM_freeN(tree);
+}
+
+///----------------------------------------
+/// Plugin API for legacy iksolver
+
+void iksolver_initialize_tree(struct Scene *scene, struct Object *ob, float ctime)
+{
+	bPoseChannel *pchan;
+	
+	for(pchan= ob->pose->chanbase.first; pchan; pchan= pchan->next) {
+		if(pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints
+			initialize_posetree(ob, pchan);	// will attach it to root!
+	}
+	ob->pose->flag &= ~POSE_WAS_REBUILT;
+}
+
+void iksolver_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime)
+{
+	while(pchan->iktree.first) {
+		PoseTree *tree= pchan->iktree.first;
+		int a;
+		
+		/* 4. walk over the tree for regular solving */
+		for(a=0; a<tree->totchannel; a++) {
+			if(!(tree->pchan[a]->flag & POSE_DONE))	// successive trees can set the flag
+				where_is_pose_bone(scene, ob, tree->pchan[a], ctime);
+			// tell blender that this channel was controlled by IK, it's cleared on each where_is_pose()
+			tree->pchan[a]->flag |= POSE_CHAIN;
+		}
+		/* 5. execute the IK solver */
+		execute_posetree(ob, tree);
+		
+		/* 6. apply the differences to the channels, 
+			  we need to calculate the original differences first */
+		for(a=0; a<tree->totchannel; a++)
+			make_dmats(tree->pchan[a]);
+		
+		for(a=0; a<tree->totchannel; a++)
+			/* sets POSE_DONE */
+			where_is_ik_bone(tree->pchan[a], tree->basis_change[a]);
+		
+		/* 7. and free */
+		BLI_remlink(&pchan->iktree, tree);
+		free_posetree(tree);
+	}
+}
+
diff --git a/source/blender/ikplugin/intern/iksolver_plugin.h b/source/blender/ikplugin/intern/iksolver_plugin.h
new file mode 100644
index 00000000000..d5d1d9a77da
--- /dev/null
+++ b/source/blender/ikplugin/intern/iksolver_plugin.h
@@ -0,0 +1,47 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef IKSOLVER_PLUGIN_H
+#define IKSOLVER_PLUGIN_H
+
+#include "ikplugin_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void iksolver_initialize_tree(struct Scene *scene, struct Object *ob, float ctime);
+void iksolver_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // IKSOLVER_PLUGIN_H
+
diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp
new file mode 100644
index 00000000000..b6278e40ea5
--- /dev/null
+++ b/source/blender/ikplugin/intern/itasc_plugin.cpp
@@ -0,0 +1,1786 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+
+// iTaSC headers
+#include "Armature.hpp"
+#include "MovingFrame.hpp"
+#include "CopyPose.hpp"
+#include "WSDLSSolver.hpp"
+#include "WDLSSolver.hpp"
+#include "Scene.hpp"
+#include "Cache.hpp"
+#include "Distance.hpp"
+
+#include "MEM_guardedalloc.h"
+
+extern "C" {
+#include "BIK_api.h"
+#include "BLI_blenlib.h"
+#include "BLI_arithb.h"
+
+#include "BKE_global.h"
+#include "BKE_armature.h"
+#include "BKE_action.h"
+#include "BKE_utildefines.h"
+#include "BKE_constraint.h"
+#include "DNA_object_types.h"
+#include "DNA_action_types.h"
+#include "DNA_constraint_types.h"
+#include "DNA_armature_types.h"
+#include "DNA_scene_types.h"
+};
+
+#include "itasc_plugin.h"
+
+// default parameters
+bItasc DefIKParam;
+
+// in case of animation mode, feedback and timestep is fixed
+#define ANIM_TIMESTEP	1.0
+#define ANIM_FEEDBACK	0.8
+#define ANIM_QMAX		0.52
+
+
+// Structure pointed by bPose.ikdata
+// It contains everything needed to simulate the armatures
+// There can be several simulation islands independent to each other
+struct IK_Data
+{
+	struct IK_Scene* first;
+};
+
+typedef float Vector3[3];
+typedef float Vector4[4];
+struct IK_Target;
+typedef void (*ErrorCallback)(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget);
+// For some reason, gcc doesn't find the declaration of this function in linux
+void KDL::SetToZero(JntArray& array);
+
+// one structure for each target in the scene
+struct IK_Target
+{
+	iTaSC::MovingFrame*		target;
+	iTaSC::ConstraintSet*	constraint;
+	struct bConstraint*		blenderConstraint;
+	struct bPoseChannel*    rootChannel;
+	Object*					owner;			//for auto IK
+	ErrorCallback			errorCallback;
+	std::string				targetName;
+	std::string				constraintName;
+	unsigned short			controlType;
+	short					channel;		//index in IK channel array of channel on which this target is defined
+	short					ee;				//end effector number
+	bool					simulation;		//true when simulation mode is used (update feedback)
+	bool					eeBlend;		//end effector affected by enforce blending
+	float					eeRest[4][4];	//end effector initial pose relative to armature
+
+	IK_Target() {
+		target = NULL;
+		constraint = NULL;
+		blenderConstraint = NULL;
+		rootChannel = NULL;
+		owner = NULL;
+		controlType = 0;
+		channel = 0;
+		ee = 0;
+		eeBlend = true;
+		simulation = true;
+		targetName.reserve(32);
+		constraintName.reserve(32);
+	}
+	~IK_Target() {
+		if (constraint)
+			delete constraint;
+		if (target)
+			delete target;
+	}
+};
+
+struct IK_Channel {
+	bPoseChannel*	pchan;		// channel where we must copy matrix back
+	KDL::Frame		frame;		// frame of the bone relative to object base, not armature base
+	std::string		tail;		// segment name of the joint from which we get the bone tail
+	std::string     head;		// segment name of the joint from which we get the bone head
+	int				parent;		// index in this array of the parent channel
+	short			jointType;	// type of joint, combination of IK_SegmentFlag
+	char			ndof;		// number of joint angles for this channel
+	char			jointValid;	// set to 1 when jointValue has been computed
+	// for joint constraint
+	Object*			owner;				// for pose and IK param
+	double			jointValue[4];		// computed joint value
+
+	IK_Channel() {
+		pchan = NULL;
+		parent = -1;
+		jointType = 0;
+		ndof = 0;
+		jointValid = 0;
+		owner = NULL;
+		jointValue[0] = 0.0;
+		jointValue[1] = 0.0;
+		jointValue[2] = 0.0;
+		jointValue[3] = 0.0;
+	}
+};
+
+struct IK_Scene
+{
+	IK_Scene*			next;
+	int					numchan;	// number of channel in pchan
+	int					numjoint;	// number of joint in jointArray
+	// array of bone information, one per channel in the tree
+	IK_Channel*			channels;
+	iTaSC::Armature*	armature;
+	iTaSC::Cache*		cache;
+	iTaSC::Scene*		scene;
+	iTaSC::MovingFrame* base;		// armature base object
+	KDL::Frame			baseFrame;	// frame of armature base relative to blArmature
+	KDL::JntArray		jointArray;	// buffer for storing temporary joint array
+	iTaSC::Solver*		solver;
+	Object*				blArmature;
+	struct bConstraint*	polarConstraint;
+	std::vector<IK_Target*>		targets;
+
+	IK_Scene() {
+		next = NULL;
+		channels = NULL;
+		armature = NULL;
+		cache = NULL;
+		scene = NULL;
+		base = NULL;
+		solver = NULL;
+		blArmature = NULL;
+		numchan = 0;
+		numjoint = 0;
+		polarConstraint = NULL;
+	}
+
+	~IK_Scene() {
+		// delete scene first
+		if (scene)
+			delete scene;
+		for(std::vector<IK_Target*>::iterator it = targets.begin();	it != targets.end(); ++it)
+			delete (*it);
+		targets.clear();
+		if (channels)
+			delete [] channels;
+		if (solver)
+			delete solver;
+		if (armature)
+			delete armature;
+		if (base)
+			delete base;
+		// delete cache last
+		if (cache)
+			delete cache;
+	}
+};
+
+// type of IK joint, can be combined to list the joints corresponding to a bone
+enum IK_SegmentFlag {
+	IK_XDOF = 1,
+	IK_YDOF = 2,
+	IK_ZDOF = 4,
+	IK_SWING = 8,
+	IK_REVOLUTE = 16,
+	IK_TRANSY = 32,
+};
+
+enum IK_SegmentAxis {
+	IK_X = 0,
+	IK_Y = 1,
+	IK_Z = 2,
+	IK_TRANS_X = 3,
+	IK_TRANS_Y = 4,
+	IK_TRANS_Z = 5
+};
+
+static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con)
+{
+	bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
+	PoseTree *tree;
+	PoseTarget *target;
+	bKinematicConstraint *data;
+	int a, t, segcount= 0, size, newsize, *oldparent, parent, rootbone, treecount;
+
+	data=(bKinematicConstraint*)con->data;
+	
+	/* exclude tip from chain? */
+	if(!(data->flag & CONSTRAINT_IK_TIP))
+		pchan_tip= pchan_tip->parent;
+	
+	rootbone = data->rootbone;
+	/* Find the chain's root & count the segments needed */
+	for (curchan = pchan_tip; curchan; curchan=curchan->parent){
+		pchan_root = curchan;
+		
+		if (++segcount > 255)		// 255 is weak
+			break;
+
+		if(segcount==rootbone){
+			// reached this end of the chain but if the chain is overlapping with a 
+			// previous one, we must go back up to the root of the other chain
+			if ((curchan->flag & POSE_CHAIN) && curchan->iktree.first == NULL){
+				rootbone++;
+				continue;
+			}
+			break; 
+		}
+
+		if (curchan->iktree.first != NULL)
+			// Oh oh, there is already a chain starting from this channel and our chain is longer... 
+			// Should handle this by moving the previous chain up to the begining of our chain
+			// For now we just stop here
+			break;
+	}
+	if (!segcount) return 0;
+	// we reached a limit and still not the end of a previous chain, quit
+	if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.first == NULL) return 0;
+
+	// now that we know how many segment we have, set the flag
+	for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan=curchan->parent) {
+		chanlist[segcount]=curchan;
+		curchan->flag |= POSE_CHAIN;
+	}
+
+	/* setup the chain data */
+	/* create a target */
+	target= (PoseTarget*)MEM_callocN(sizeof(PoseTarget), "posetarget");
+	target->con= con;
+	// by contruction there can be only one tree per channel and each channel can be part of at most one tree.
+	tree = (PoseTree*)pchan_root->iktree.first;
+
+	if(tree==NULL) {
+		/* make new tree */
+		tree= (PoseTree*)MEM_callocN(sizeof(PoseTree), "posetree");
+
+		tree->iterations= data->iterations;
+		tree->totchannel= segcount;
+		tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
+		
+		tree->pchan= (bPoseChannel**)MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
+		tree->parent= (int*)MEM_callocN(segcount*sizeof(int), "ik tree parent");
+		for(a=0; a<segcount; a++) {
+			tree->pchan[a]= chanlist[segcount-a-1];
+			tree->parent[a]= a-1;
+		}
+		target->tip= segcount-1;
+		
+		/* AND! link the tree to the root */
+		BLI_addtail(&pchan_root->iktree, tree);
+		// new tree
+		treecount = 1;
+	}
+	else {
+		tree->iterations= MAX2(data->iterations, tree->iterations);
+		tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
+
+		/* skip common pose channels and add remaining*/
+		size= MIN2(segcount, tree->totchannel);
+		a = t = 0;
+		while (a<size && t<tree->totchannel) {
+			// locate first matching channel
+			for (;t<tree->totchannel && tree->pchan[t]!=chanlist[segcount-a-1];t++);
+			if (t>=tree->totchannel)
+				break;
+			for(; a<size && t<tree->totchannel && tree->pchan[t]==chanlist[segcount-a-1]; a++, t++);
+		}
+		parent= a-1;
+		segcount= segcount-a;
+		target->tip= tree->totchannel + segcount - 1;
+
+		if (segcount > 0) {
+			/* resize array */
+			newsize= tree->totchannel + segcount;
+			oldchan= tree->pchan;
+			oldparent= tree->parent;
+
+			tree->pchan= (bPoseChannel**)MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
+			tree->parent= (int*)MEM_callocN(newsize*sizeof(int), "ik tree parent");
+			memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
+			memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
+			MEM_freeN(oldchan);
+			MEM_freeN(oldparent);
+
+			/* add new pose channels at the end, in reverse order */
+			for(a=0; a<segcount; a++) {
+				tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
+				tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
+			}
+			tree->parent[tree->totchannel]= parent;
+			
+			tree->totchannel= newsize;
+		}
+		// reusing tree
+		treecount = 0;
+	}
+
+	/* add target to the tree */
+	BLI_addtail(&tree->targets, target);
+	/* mark root channel having an IK tree */
+	pchan_root->flag |= POSE_IKTREE;
+	return treecount;
+}
+
+static bool is_cartesian_constraint(bConstraint *con)
+{
+	bKinematicConstraint* data=(bKinematicConstraint*)con->data;
+
+	return true;
+}
+
+static bool constraint_valid(bConstraint *con)
+{
+	bKinematicConstraint* data=(bKinematicConstraint*)con->data;
+
+	if (data->flag & CONSTRAINT_IK_AUTO)
+		return true;
+	if (con->flag & CONSTRAINT_DISABLE)
+		return false;
+	if (is_cartesian_constraint(con)) {
+		/* cartesian space constraint */
+		if (data->tar==NULL) 
+			return false;
+		if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) 
+			return false;
+	}
+	return true;
+}
+
+int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
+{
+	bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
+	PoseTree *tree;
+	PoseTarget *target;
+	bConstraint *con;
+	bKinematicConstraint *data;
+	int a, segcount= 0, size, newsize, *oldparent, parent, rootbone, treecount;
+
+	/* find all IK constraints and validate them */
+	treecount = 0;
+	for(con= (bConstraint *)pchan_tip->constraints.first; con; con= (bConstraint *)con->next) {
+		if(con->type==CONSTRAINT_TYPE_KINEMATIC) {
+			if (constraint_valid(con))
+				treecount += initialize_chain(ob, pchan_tip, con);
+		}
+	}
+	return treecount;
+}
+
+static IK_Data* get_ikdata(bPose *pose)
+{
+	if (pose->ikdata)
+		return (IK_Data*)pose->ikdata;
+	pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata");
+	// here init ikdata if needed
+	// now that we have scene, make sure the default param are initialized
+	if (!DefIKParam.iksolver)
+		init_pose_itasc(&DefIKParam);
+
+	return (IK_Data*)pose->ikdata;
+}
+static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis)
+{
+	double t = KDL::sqrt(R(0,0)*R(0,0) + R(0,1)*R(0,1));
+
+	if (t > 16.0*KDL::epsilon) {
+		if (axis == 0) return -KDL::atan2(R(1,2), R(2,2));
+        else if(axis == 1) return KDL::atan2(-R(0,2), t);
+        else return -KDL::atan2(R(0,1), R(0,0));
+    } else {
+		if (axis == 0) return -KDL::atan2(-R(2,1), R(1,1));
+        else if(axis == 1) return KDL::atan2(-R(0,2), t);
+        else return 0.0f;
+    }
+}
+
+static double ComputeTwist(const KDL::Rotation& R)
+{
+	// qy and qw are the y and w components of the quaternion from R
+	double qy = R(0,2) - R(2,0);
+	double qw = R(0,0) + R(1,1) + R(2,2) + 1;
+
+	double tau = 2*KDL::atan2(qy, qw);
+
+	return tau;
+}
+
+static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis)
+{
+	// compute twist parameter
+	KDL::Rotation T;
+	switch (axis) {
+	case 0:
+		T = KDL::Rotation::RotX(-angle);
+		break;
+	case 1:
+		T = KDL::Rotation::RotY(-angle);
+		break;
+	case 2:
+		T = KDL::Rotation::RotZ(-angle);
+		break;
+	default:
+		return;
+	}
+	// remove angle
+	R = R*T;
+}
+
+static void GetEulerXZY(const KDL::Rotation& R, double& X,double& Z,double& Y)
+{
+	if (fabs(R(0,1)) > 1.0 - KDL::epsilon ) {
+        X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0));
+        Z = -KDL::sign(R(0,1)) * KDL::PI / 2;
+        Y = 0.0 ;
+    } else {
+        X = KDL::atan2(R(2,1), R(1,1));
+        Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2))));
+        Y = KDL::atan2(R(0,2), R(0,0));
+    }
+}
+
+static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z)
+{
+	if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) {
+        X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1));
+        Y = KDL::sign(R(0,2)) * KDL::PI / 2;
+        Z = 0.0 ;
+    } else {
+        X = KDL::atan2(-R(1,2), R(2,2));
+        Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1))));
+        Z = KDL::atan2(-R(0,1), R(0,0));
+    }
+}
+
+static void GetJointRotation(KDL::Rotation& boneRot, int type, double* rot)
+{
+	switch (type & ~IK_TRANSY)
+	{
+	default:
+		// fixed bone, no joint
+		break;
+	case IK_XDOF:
+		// RX only, get the X rotation
+		rot[0] = EulerAngleFromMatrix(boneRot, 0);
+		break;
+	case IK_YDOF:
+		// RY only, get the Y rotation
+		rot[0] = ComputeTwist(boneRot);
+		break;
+	case IK_ZDOF:
+		// RZ only, get the Z rotation
+		rot[0] = EulerAngleFromMatrix(boneRot, 2);
+		break;
+	case IK_XDOF|IK_YDOF:
+		rot[1] = ComputeTwist(boneRot);
+		RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
+		rot[0] = EulerAngleFromMatrix(boneRot, 0);
+		break;
+	case IK_SWING:
+		// RX+RZ
+		boneRot.GetXZRot().GetValue(rot);
+		break;
+	case IK_YDOF|IK_ZDOF:
+		// RZ+RY
+		rot[1] = ComputeTwist(boneRot);
+		RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
+		rot[0] = EulerAngleFromMatrix(boneRot, 2);
+		break;
+	case IK_SWING|IK_YDOF:
+		rot[2] = ComputeTwist(boneRot);
+		RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
+		boneRot.GetXZRot().GetValue(rot);
+		break;
+	case IK_REVOLUTE:
+		boneRot.GetRot().GetValue(rot);
+		break;
+	}
+}
+
+static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
+{
+	IK_Target* target = (IK_Target*)param;
+	// compute next target position
+	// get target matrix from constraint.
+	bConstraint* constraint = (bConstraint*)target->blenderConstraint;
+	float tarmat[4][4];
+
+	get_constraint_target_matrix(constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
+
+	// rootmat contains the target pose in world coordinate
+	// if enforce is != 1.0, blend the target position with the end effector position
+	// if the armature was in rest position. This information is available in eeRest
+	if (constraint->enforce != 1.0f && target->eeBlend) {
+		// eeRest is relative to the reference frame of the IK root
+		// get this frame in world reference
+		float restmat[4][4];
+		bPoseChannel* pchan = target->rootChannel;
+		if (pchan->parent) {
+			pchan = pchan->parent;
+			float chanmat[4][4];
+			Mat4CpyMat4(chanmat, pchan->pose_mat);
+			VECCOPY(chanmat[3], pchan->pose_tail);
+			Mat4MulSerie(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL);
+		} 
+		else {
+			Mat4MulMat4(restmat, target->eeRest, target->owner->obmat);
+		}
+		// blend the target
+		Mat4BlendMat4(tarmat, restmat, tarmat, constraint->enforce);
+	}
+	next.setValue(&tarmat[0][0]);
+	return true;
+}
+
+static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
+{
+	IK_Scene* ikscene = (IK_Scene*)param;
+	// compute next armature base pose
+	// algorithm: 
+	// ikscene->pchan[0] is the root channel of the tree
+	// if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail
+	// then multiply by the armature matrix to get ikscene->armature base position
+	bPoseChannel* pchan = ikscene->channels[0].pchan;
+	float rootmat[4][4];
+	if (pchan->parent) {
+		pchan = pchan->parent;
+		float chanmat[4][4];
+		Mat4CpyMat4(chanmat, pchan->pose_mat);
+		VECCOPY(chanmat[3], pchan->pose_tail);
+		// save the base as a frame too so that we can compute deformation
+		// after simulation
+		ikscene->baseFrame.setValue(&chanmat[0][0]);
+		Mat4MulMat4(rootmat, chanmat, ikscene->blArmature->obmat);
+	} 
+	else {
+		Mat4CpyMat4(rootmat, ikscene->blArmature->obmat);
+		ikscene->baseFrame = iTaSC::F_identity;
+	}
+	next.setValue(&rootmat[0][0]);
+	// if there is a polar target (only during solving otherwise we don't have end efffector)
+	if (ikscene->polarConstraint && timestamp.update) {
+		// compute additional rotation of base frame so that armature follows the polar target
+		float imat[4][4];		// IK tree base inverse matrix
+		float polemat[4][4];	// polar target in IK tree base frame
+		float goalmat[4][4];	// target in IK tree base frame
+		float mat[4][4];		// temp matrix
+		bKinematicConstraint* poledata = (bKinematicConstraint*)ikscene->polarConstraint->data;
+
+		Mat4Invert(imat, rootmat);
+		// polar constraint imply only one target
+		IK_Target *iktarget = ikscene->targets[0];
+		// root channel from which we take the bone initial orientation
+		IK_Channel &rootchan = ikscene->channels[0];
+
+		// get polar target matrix in world space
+		get_constraint_target_matrix(ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0);
+		// convert to armature space
+		Mat4MulMat4(polemat, mat, imat);
+		// get the target in world space (was computed before as target object are defined before base object)
+		iktarget->target->getPose().getValue(mat[0]);
+		// convert to armature space
+		Mat4MulMat4(goalmat, mat, imat);
+		// take position of target, polar target, end effector, in armature space
+		KDL::Vector goalpos(goalmat[3]);
+		KDL::Vector polepos(polemat[3]);
+		KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p;
+		// get root bone orientation
+		KDL::Frame rootframe;
+		ikscene->armature->getRelativeFrame(rootframe, rootchan.tail);
+		KDL::Vector rootx = rootframe.M.UnitX();
+		KDL::Vector rootz = rootframe.M.UnitZ();
+		// and compute root bone head
+		double q_rest[3], q[3], length;
+		const KDL::Joint* joint;
+		const KDL::Frame* tip;
+		ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
+		length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
+		KDL::Vector rootpos = rootframe.p - length*rootframe.M.UnitY();
+
+		// compute main directions 
+		KDL::Vector dir = KDL::Normalize(endpos - rootpos);
+		KDL::Vector poledir = KDL::Normalize(goalpos-rootpos);
+		// compute up directions
+		KDL::Vector poleup = KDL::Normalize(polepos-rootpos);
+		KDL::Vector up = rootx*KDL::cos(poledata->poleangle) + rootz*KDL::sin(poledata->poleangle);
+		// from which we build rotation matrix
+		KDL::Rotation endrot, polerot;
+		// for the armature, using the root bone orientation
+		KDL::Vector x = KDL::Normalize(dir*up);
+		endrot.UnitX(x);
+		endrot.UnitY(KDL::Normalize(x*dir));
+		endrot.UnitZ(-dir);
+		// for the polar target 
+		x = KDL::Normalize(poledir*poleup);
+		polerot.UnitX(x);
+		polerot.UnitY(KDL::Normalize(x*poledir));
+		polerot.UnitZ(-poledir);
+		// the difference between the two is the rotation we want to apply
+		KDL::Rotation result(polerot*endrot.Inverse());
+		// apply on base frame as this is an artificial additional rotation
+		next.M = next.M*result;
+		ikscene->baseFrame.M = ikscene->baseFrame.M*result;
+	}
+	return true;
+}
+
+static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+	IK_Target* iktarget =(IK_Target*)_param;
+	bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
+	iTaSC::ConstraintValues* values = _values;
+	bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
+	iTaSC::ConstraintSingleValue* value;
+	double error;
+	int i;
+
+	// we need default parameters
+	if (!ikparam) 
+		ikparam = &DefIKParam;
+
+	if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
+		if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
+			values->alpha = 0.0;
+			values->action = iTaSC::ACT_ALPHA;
+			values++;
+		}
+		if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
+			values->alpha = 0.0;
+			values->action = iTaSC::ACT_ALPHA;
+			values++;
+		}
+	} else {
+		if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
+			// update error
+			values->alpha = condata->weight;
+			values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK;
+			values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
+			values++;
+		}
+		if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
+			// update error
+			values->alpha = condata->orientweight;
+			values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK;
+			values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
+			values++;
+		}
+	}
+	return true;
+}
+
+static void copypose_error(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget)
+{
+	iTaSC::ConstraintSingleValue* value;
+	double error;
+	int i;
+
+	if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
+		// update error
+		for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
+			error += KDL::sqr(value->y - value->yd);
+		iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error);
+		values++;
+	}
+	if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
+		// update error
+		for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
+			error += KDL::sqr(value->y - value->yd);
+		iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error);
+		values++;
+	}
+}
+
+static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+	IK_Target* iktarget =(IK_Target*)_param;
+	bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
+	iTaSC::ConstraintValues* values = _values;
+	bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
+	// we need default parameters
+	if (!ikparam) 
+		ikparam = &DefIKParam;
+
+	// update weight according to mode
+	if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
+		values->alpha = 0.0;
+	} else {
+		switch (condata->mode) {
+		case LIMITDIST_INSIDE:
+			values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0;
+			break;
+		case LIMITDIST_OUTSIDE:
+			values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0;
+			break;
+		default:
+			values->alpha = condata->weight;
+			break;
+		}	
+		if (!timestamp.substep) {
+			// only update value on first timestep
+			switch (condata->mode) {
+			case LIMITDIST_INSIDE:
+				values->values[0].yd = condata->dist*0.95;
+				break;
+			case LIMITDIST_OUTSIDE:
+				values->values[0].yd = condata->dist*1.05;
+				break;
+			default:
+				values->values[0].yd = condata->dist;
+				break;
+			}
+			values->values[0].action = iTaSC::ACT_VALUE|iTaSC::ACT_FEEDBACK;
+			values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
+		}
+	}
+	values->action |= iTaSC::ACT_ALPHA;
+	return true;
+}
+
+static void distance_error(const iTaSC::ConstraintValues* values, unsigned int _nvalues, IK_Target* iktarget)
+{
+	iktarget->blenderConstraint->lin_error = (float)(values->values[0].y - values->values[0].yd);
+}
+
+static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+{
+	IK_Channel* ikchan = (IK_Channel*)_param;
+	bItasc* ikparam = (bItasc*)ikchan->owner->pose->ikparam;
+	bPoseChannel* chan = ikchan->pchan;
+	int dof;
+
+	// a channel can be splitted into multiple joints, so we get called multiple
+	// times for one channel (this callback is only for 1 joint in the armature)
+	// the IK_JointTarget structure is shared between multiple joint constraint
+	// and the target joint values is computed only once, remember this in jointValid
+	// Don't forget to reset it before each frame
+	if (!ikchan->jointValid) {
+		float rmat[3][3];
+
+		if (chan->rotmode > 0) {
+			/* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */
+			EulOToMat3(chan->eul, chan->rotmode, rmat);
+		}
+		else if (chan->rotmode == PCHAN_ROT_AXISANGLE) {
+			/* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */
+			AxisAngleToMat3(&chan->quat[1], chan->quat[0], rmat);
+		}
+		else {
+			/* quats are normalised before use to eliminate scaling issues */
+			NormalQuat(chan->quat);
+			QuatToMat3(chan->quat, rmat);
+		}
+		KDL::Rotation jointRot(
+			rmat[0][0], rmat[1][0], rmat[2][0],
+			rmat[0][1], rmat[1][1], rmat[2][1],
+			rmat[0][2], rmat[1][2], rmat[2][2]);
+		GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
+		ikchan->jointValid = 1;
+	}
+	// determine which part of jointValue is used for this joint
+	// closely related to the way the joints are defined
+	switch (ikchan->jointType & ~IK_TRANSY)
+	{
+	case IK_XDOF:
+	case IK_YDOF:
+	case IK_ZDOF:
+		dof = 0;
+		break;
+	case IK_XDOF|IK_YDOF:
+		// X + Y
+		dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
+		break;
+	case IK_SWING:
+		// XZ
+		dof = 0;
+		break;
+	case IK_YDOF|IK_ZDOF:
+		// Z + Y
+		dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
+		break;
+	case IK_SWING|IK_YDOF:
+		// XZ + Y
+		dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
+		break;
+	case IK_REVOLUTE:
+		dof = 0;
+		break;
+	default:
+		dof = -1;
+		break;
+	}
+	if (dof >= 0) {
+		for (int i=0; i<_nvalues; i++, dof++) {
+			_values[i].values[0].yd = ikchan->jointValue[dof];
+			_values[i].alpha = chan->ikrotweight;
+			_values[i].feedback = ikparam->feedback;
+		}
+	}
+	return true;
+}
+
+// build array of joint corresponding to IK chain
+static int convert_channels(IK_Scene *ikscene, PoseTree *tree)
+{
+	IK_Channel *ikchan;
+	bPoseChannel *pchan;
+	PoseTarget* target;
+	Bone *bone;
+	int a, flag, njoint;
+
+	njoint = 0;
+	for(a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) {
+		pchan= tree->pchan[a];
+		bone= pchan->bone;
+		ikchan->pchan = pchan;
+		ikchan->parent = (a>0) ? tree->parent[a] : -1;
+		ikchan->owner = ikscene->blArmature;
+		
+		/* set DoF flag */
+		flag = 0;
+		if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) && 
+			(!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0]<0.f || pchan->limitmax[0]>0.f))
+			flag |= IK_XDOF;
+		if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
+			(!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1]<0.f || pchan->limitmax[1]>0.f))
+			flag |= IK_YDOF;
+		if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
+			(!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2]<0.f || pchan->limitmax[2]>0.f))
+			flag |= IK_ZDOF;
+		
+		if(tree->stretch && (pchan->ikstretch > 0.0)) {
+			flag |= IK_TRANSY;
+		}
+		/*
+		Logic to create the segments:
+		RX,RY,RZ = rotational joints with no length
+		RY(tip) = rotational joints with a fixed length arm = (0,length,0)
+		TY = translational joint on Y axis
+		F(pos) = fixed joint with an arm at position pos 
+		Conversion rule of the above flags:
+		-   ==> F(tip)
+		X   ==> RX(tip)
+		Y   ==> RY(tip)
+		Z   ==> RZ(tip)
+		XY  ==> RX+RY(tip)
+		XZ  ==> RX+RZ(tip)
+		YZ  ==> RZ+RY(tip)
+		XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
+		In case of stretch, tip=(0,0,0) and there is an additional TY joint
+		The frame at last of these joints represents the tail of the bone. 
+		The head is computed by a reverse translation on Y axis of the bone length
+		or in case of TY joint, by the frame at previous joint.
+		In case of separation of bones, there is an additional F(head) joint
+
+		Computing rest pose and length is complicated: the solver works in world space
+		Here is the logic:
+		rest position is computed only from bone->bone_mat.
+		bone length is computed from bone->length multiplied by the scaling factor of
+		the armature. Non-uniform scaling will give bad result!
+
+		*/
+		switch (flag & (IK_XDOF|IK_YDOF|IK_ZDOF))
+		{
+		default:
+			ikchan->jointType = 0;
+			ikchan->ndof = 0;
+			break;
+		case IK_XDOF:
+			// RX only, get the X rotation
+			ikchan->jointType = IK_XDOF;
+			ikchan->ndof = 1;
+			break;
+		case IK_YDOF:
+			// RY only, get the Y rotation
+			ikchan->jointType = IK_YDOF;
+			ikchan->ndof = 1;
+			break;
+		case IK_ZDOF:
+			// RZ only, get the Zz rotation
+			ikchan->jointType = IK_ZDOF;
+			ikchan->ndof = 1;
+			break;
+		case IK_XDOF|IK_YDOF:
+			ikchan->jointType = IK_XDOF|IK_YDOF;
+			ikchan->ndof = 2;
+			break;
+		case IK_XDOF|IK_ZDOF:
+			// RX+RZ
+			ikchan->jointType = IK_SWING;
+			ikchan->ndof = 2;
+			break;
+		case IK_YDOF|IK_ZDOF:
+			// RZ+RY
+			ikchan->jointType = IK_ZDOF|IK_YDOF;
+			ikchan->ndof = 2;
+			break;
+		case IK_XDOF|IK_YDOF|IK_ZDOF:
+			// spherical joint
+			if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT))
+				// decompose in a Swing+RotY joint
+				ikchan->jointType = IK_SWING|IK_YDOF;
+			else
+				ikchan->jointType = IK_REVOLUTE;
+			ikchan->ndof = 3;
+			break;
+		}
+		if (flag & IK_TRANSY) {
+			ikchan->jointType |= IK_TRANSY;
+			ikchan->ndof++;
+		}
+		njoint += ikchan->ndof;
+	}
+	// njoint is the joint coordinate, create the Joint Array
+	ikscene->jointArray.resize(njoint);
+	ikscene->numjoint = njoint;
+	return njoint;
+}
+
+// compute array of joint value corresponding to current pose
+static void convert_pose(IK_Scene *ikscene)
+{
+	KDL::Rotation boneRot;
+	bPoseChannel *pchan;
+	IK_Channel *ikchan;
+	Bone *bone;
+	float rmat[4][4];	// rest pose of bone with parent taken into account
+	float bmat[4][4];	// difference
+	float scale;
+	double *rot;
+	int a, joint;
+
+	// assume uniform scaling and take Y scale as general scale for the armature
+	scale = VecLength(ikscene->blArmature->obmat[1]);
+	rot = &ikscene->jointArray(0);
+	for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
+		pchan= ikchan->pchan;
+		bone= pchan->bone;
+		
+		if (pchan->parent) {
+			Mat4One(bmat);
+			Mat4MulMat43(bmat, pchan->parent->pose_mat, bone->bone_mat);
+		} else {
+			Mat4CpyMat4(bmat, bone->arm_mat);
+		}
+		Mat4Invert(rmat, bmat);
+		Mat4MulMat4(bmat, pchan->pose_mat, rmat);
+		Mat4Ortho(bmat);
+		boneRot.setValue(bmat[0]);
+		GetJointRotation(boneRot, ikchan->jointType, rot);
+		if (ikchan->jointType & IK_TRANSY) {
+			// compute actual length 
+			rot[ikchan->ndof-1] = VecLenf(pchan->pose_tail, pchan->pose_head) * scale;
+		} 
+		rot += ikchan->ndof;
+		joint += ikchan->ndof;
+	}
+}
+
+// compute array of joint value corresponding to current pose
+static void rest_pose(IK_Scene *ikscene)
+{
+	bPoseChannel *pchan;
+	IK_Channel *ikchan;
+	Bone *bone;
+	float scale;
+	double *rot;
+	int a, joint;
+
+	// assume uniform scaling and take Y scale as general scale for the armature
+	scale = VecLength(ikscene->blArmature->obmat[1]);
+	// rest pose is 0 
+	KDL::SetToZero(ikscene->jointArray);
+	// except for transY joints
+	rot = &ikscene->jointArray(0);
+	for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
+		pchan= ikchan->pchan;
+		bone= pchan->bone;
+
+		if (ikchan->jointType & IK_TRANSY)
+			rot[ikchan->ndof-1] = bone->length*scale;
+		rot += ikchan->ndof;
+		joint += ikchan->ndof;
+	}
+}
+
+static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
+{
+	PoseTree *tree = (PoseTree*)pchan->iktree.first;
+	PoseTarget *target;
+	bKinematicConstraint *condata;
+	bConstraint *polarcon;
+	bItasc *ikparam;
+	iTaSC::Armature* arm;
+	iTaSC::Scene* scene;
+	IK_Scene* ikscene;
+	IK_Channel* ikchan;
+	KDL::Frame initPose;
+	KDL::Rotation boneRot;
+	Bone *bone;
+	int a, t, numtarget;
+	float length;
+	bool ret = true, ingame;
+	double *rot;
+	double lmin[3], lmax[3];
+
+	if (tree->totchannel == 0)
+		return NULL;
+
+	ikscene = new IK_Scene;
+	arm = new iTaSC::Armature();
+	scene = new iTaSC::Scene();
+	ikscene->channels = new IK_Channel[tree->totchannel];
+	ikscene->numchan = tree->totchannel;
+	ikscene->armature = arm;
+	ikscene->scene = scene;
+	ikparam = (bItasc*)ob->pose->ikparam;
+	ingame = (ob->pose->flag & POSE_GAME_ENGINE);
+	if (!ikparam) {
+		// you must have our own copy
+		ikparam = &DefIKParam;
+	} else if (ingame) {
+		// tweak the param when in game to have efficient stepping
+		// using fixed substep is not effecient since frames in the GE are often
+		// shorter than in animation => move to auto step automatically and set
+		// the target substep duration via min/max
+		if (!(ikparam->flag & ITASC_AUTO_STEP)) {
+			float timestep = blscene->r.frs_sec_base/blscene->r.frs_sec;
+			if (ikparam->numstep > 0)
+				timestep /= ikparam->numstep;
+			// with equal min and max, the algorythm will take this step and the indicative substep most of the time
+			ikparam->minstep = ikparam->maxstep = timestep;
+			ikparam->flag |= ITASC_AUTO_STEP;
+		}
+	}
+	if ((ikparam->flag & ITASC_SIMULATION) && !ingame)
+		// no cache in animation mode
+		ikscene->cache = new iTaSC::Cache();
+
+	switch (ikparam->solver) {
+	case ITASC_SOLVER_SDLS:
+		ikscene->solver = new iTaSC::WSDLSSolver();
+		break;
+	case ITASC_SOLVER_DLS:
+		ikscene->solver = new iTaSC::WDLSSolver();
+		break;
+	default:
+		delete ikscene;
+		return NULL;
+	}
+	ikscene->blArmature = ob;
+
+	std::string  joint;
+	std::string  root("root");
+	std::string  parent;
+	std::vector<double> weights;
+	double weight[3];
+	// assume uniform scaling and take Y scale as general scale for the armature
+	float scale = VecLength(ob->obmat[1]);
+	double X, Y, Z;
+	// build the array of joints corresponding to the IK chain
+	convert_channels(ikscene, tree);
+	if (ingame) {
+		// in the GE, set the initial joint angle to match the current pose
+		// this will update the jointArray in ikscene
+		convert_pose(ikscene);
+	} else {
+		// in Blender, the rest pose is always 0 for joints
+		rest_pose(ikscene);
+	}
+	rot = &ikscene->jointArray(0);
+	for(a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) {
+		pchan= ikchan->pchan;
+		bone= pchan->bone;
+
+		KDL::Frame tip(iTaSC::F_identity);
+		Vector3 *fl = bone->bone_mat;
+		KDL::Frame head(KDL::Rotation(
+			fl[0][0], fl[1][0], fl[2][0],
+			fl[0][1], fl[1][1], fl[2][1],
+			fl[0][2], fl[1][2], fl[2][2]),
+			KDL::Vector(bone->head[0], bone->head[1], bone->head[2])*scale);
+
+		// rest pose length of the bone taking scaling into account
+		length= bone->length*scale;
+		parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
+		// first the fixed segment to the bone head
+		if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
+			joint = bone->name;
+			joint += ":H";
+			ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
+			parent = joint;
+		}
+		if (!(ikchan->jointType & IK_TRANSY)) {
+			// fixed length, put it in tip
+			tip.p[1] = length;
+		}
+		joint = bone->name;
+		weight[0] = (1.0-pchan->stiffness[0]);
+		weight[1] = (1.0-pchan->stiffness[1]);
+		weight[2] = (1.0-pchan->stiffness[2]);
+		switch (ikchan->jointType & ~IK_TRANSY)
+		{
+		case 0:
+			// fixed bone
+			if (!(ikchan->jointType & IK_TRANSY)) {
+				joint += ":F";
+				ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
+			}
+			break;
+		case IK_XDOF:
+			// RX only, get the X rotation
+			joint += ":RX";
+			ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
+			weights.push_back(weight[0]);
+			break;
+		case IK_YDOF:
+			// RY only, get the Y rotation
+			joint += ":RY";
+			ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
+			weights.push_back(weight[1]);
+			break;
+		case IK_ZDOF:
+			// RZ only, get the Zz rotation
+			joint += ":RZ";
+			ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
+			weights.push_back(weight[2]);
+			break;
+		case IK_XDOF|IK_YDOF:
+			joint += ":RX";
+			ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
+			weights.push_back(weight[0]);
+			if (ret) {
+				parent = joint;
+				joint = bone->name;
+				joint += ":RY";
+				ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
+				weights.push_back(weight[1]);
+			}
+			break;
+		case IK_SWING:
+			joint += ":SW";
+			ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
+			weights.push_back(weight[0]);
+			weights.push_back(weight[2]);
+			break;
+		case IK_YDOF|IK_ZDOF:
+			// RZ+RY
+			joint += ":RZ";
+			ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
+			weights.push_back(weight[2]);
+			if (ret) {
+				parent = joint;
+				joint = bone->name;
+				joint += ":RY";
+				ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
+				weights.push_back(weight[1]);
+			}
+			break;
+		case IK_SWING|IK_YDOF:
+			// decompose in a Swing+RotY joint
+			joint += ":SW";
+			ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
+			weights.push_back(weight[0]);
+			weights.push_back(weight[2]);
+			if (ret) {
+				parent = joint;
+				joint = bone->name;
+				joint += ":RY";
+				ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
+				weights.push_back(weight[1]);
+			}
+			break;
+		case IK_REVOLUTE:
+			joint += ":SJ";
+			ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
+			weights.push_back(weight[0]);
+			weights.push_back(weight[1]);
+			weights.push_back(weight[2]);
+			break;
+		}
+		if (ret && (ikchan->jointType & IK_TRANSY)) {
+			parent = joint;
+			joint = bone->name;
+			joint += ":TY";
+			ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof-1]);
+			float ikstretch = pchan->ikstretch*pchan->ikstretch;
+			weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99));
+			weights.push_back(weight[1]);
+		}
+		if (!ret)
+			// error making the armature??
+			break;
+		// joint points to the segment that correspond to the bone per say
+		ikchan->tail = joint;
+		ikchan->head = parent;
+		// in case of error
+		ret = false;
+		if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ROTCTL))) {
+			joint = bone->name;
+			joint += ":RX";
+			if (pchan->ikflag & BONE_IK_XLIMIT) {
+				if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
+					break;
+			}
+			if (pchan->ikflag & BONE_IK_ROTCTL) {
+				if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
+					break;
+			}
+		}
+		if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT|BONE_IK_ROTCTL))) {
+			joint = bone->name;
+			joint += ":RY";
+			if (pchan->ikflag & BONE_IK_YLIMIT) {
+				if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0)
+					break;
+			}
+			if (pchan->ikflag & BONE_IK_ROTCTL) {
+				if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
+					break;
+			}
+		}
+		if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
+			joint = bone->name;
+			joint += ":RZ";
+			if (pchan->ikflag & BONE_IK_ZLIMIT) {
+				if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0)
+					break;
+			}
+			if (pchan->ikflag & BONE_IK_ROTCTL) {
+				if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
+					break;
+			}
+		}
+		if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
+			joint = bone->name;
+			joint += ":SW";
+			if (pchan->ikflag & BONE_IK_XLIMIT) {
+				if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
+					break;
+			}
+			if (pchan->ikflag & BONE_IK_ZLIMIT) {
+				if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0)
+					break;
+			}
+			if (pchan->ikflag & BONE_IK_ROTCTL) {
+				if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
+					break;
+			}
+		}
+		if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) {
+			joint = bone->name;
+			joint += ":SJ";
+			if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
+				break;
+		}
+		//  no error, so restore
+		ret = true;
+		rot += ikchan->ndof;
+	}
+	if (!ret) {
+		delete ikscene;
+		return NULL;
+	}
+	// for each target, we need to add an end effector in the armature
+	for (numtarget=0, polarcon=NULL, ret = true, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next) {
+		condata= (bKinematicConstraint*)target->con->data;
+		pchan = tree->pchan[target->tip];
+
+		if (is_cartesian_constraint(target->con)) {
+			// add the end effector
+			IK_Target* iktarget = new IK_Target();
+			ikscene->targets.push_back(iktarget);
+			iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
+			if (iktarget->ee == -1) {
+				ret = false;
+				break;
+			}
+			// initialize all the fields that we can set at this time
+			iktarget->blenderConstraint = target->con;
+			iktarget->channel = target->tip;
+			iktarget->simulation = (ikparam->flag & ITASC_SIMULATION);
+			iktarget->rootChannel = ikscene->channels[0].pchan;
+			iktarget->owner = ob;
+			iktarget->targetName = pchan->bone->name;
+			iktarget->targetName += ":T:";
+			iktarget->targetName += target->con->name;
+			iktarget->constraintName = pchan->bone->name;
+			iktarget->constraintName += ":C:";
+			iktarget->constraintName += target->con->name;
+			numtarget++;
+			if (condata->poletar)
+				// this constraint has a polar target
+				polarcon = target->con;
+		}
+	}
+	// deal with polar target if any
+	if (numtarget == 1 && polarcon) {
+		ikscene->polarConstraint = polarcon;
+	}
+	// we can now add the armature
+	// the armature is based on a moving frame. 
+	// initialize with the correct position in case there is no cache
+	base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
+	ikscene->base = new iTaSC::MovingFrame(initPose);
+	ikscene->base->setCallback(base_callback, ikscene);
+	std::string armname;
+	armname = ob->id.name;
+	armname += ":B";
+	ret = scene->addObject(armname, ikscene->base);
+	armname = ob->id.name;
+	armname += ":AR";
+	if (ret)
+		ret = scene->addObject(armname, ikscene->armature, ikscene->base);
+	if (!ret) {
+		delete ikscene;
+		return NULL;
+	}
+	// set the weight
+	e_matrix& Wq = arm->getWq();
+	assert(Wq.cols() == weights.size());
+	for (unsigned int q=0; q<Wq.cols(); q++)
+		Wq(q,q)=weights[q];
+	// get the inverse rest pose frame of the base to compute relative rest pose of end effectors
+	// this is needed to handle the enforce parameter
+	// ikscene->pchan[0] is the root channel of the tree
+	// if it has no parent, then it's just the identify Frame
+	float invBaseFrame[4][4];
+	pchan = ikscene->channels[0].pchan;
+	if (pchan->parent) {
+		// it has a parent, get the pose matrix from it 
+		float baseFrame[4][4];
+		pchan = pchan->parent;	
+		Mat4CpyMat4(baseFrame, pchan->bone->arm_mat);
+		// move to the tail and scale to get rest pose of armature base
+		VecCopyf(baseFrame[3], pchan->bone->arm_tail);
+		Mat4Invert(invBaseFrame, baseFrame);
+	} else {
+		Mat4One(invBaseFrame);
+	}
+	// finally add the constraint
+	for (t=0; t<ikscene->targets.size(); t++) {
+		IK_Target* iktarget = ikscene->targets[t];
+		condata= (bKinematicConstraint*)iktarget->blenderConstraint->data;
+		pchan = tree->pchan[iktarget->channel];
+		unsigned int controltype, bonecnt;
+		double bonelen;
+		float mat[4][4];
+
+		// add the end effector
+		// estimate the average bone length, used to clamp feedback error
+		for (bonecnt=0, bonelen=0.f, a=iktarget->channel; a>=0; a=tree->parent[a], bonecnt++)
+			bonelen += scale*tree->pchan[a]->bone->length;
+		bonelen /= bonecnt;		
+
+		// store the rest pose of the end effector to compute enforce target
+		Mat4CpyMat4(mat, pchan->bone->arm_mat);
+		VecCopyf(mat[3], pchan->bone->arm_tail);
+		// get the rest pose relative to the armature base
+		Mat4MulMat4(iktarget->eeRest, mat, invBaseFrame);
+		iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false;
+		// use target_callback to make sure the initPose includes enforce coefficient
+		target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
+		iktarget->target = new iTaSC::MovingFrame(initPose);
+		iktarget->target->setCallback(target_callback, iktarget);
+		ret = scene->addObject(iktarget->targetName, iktarget->target);
+		if (!ret)
+			break;
+
+		switch (condata->type) {
+		case CONSTRAINT_IK_COPYPOSE:
+			controltype = 0;
+			if ((condata->flag & CONSTRAINT_IK_ROT) && (condata->orientweight != 0.0))
+				controltype |= iTaSC::CopyPose::CTL_ROTATION;
+			if ((condata->weight != 0.0))
+				controltype |= iTaSC::CopyPose::CTL_POSITION;
+			if (controltype) {
+				iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
+				// set the gain
+				if (controltype & iTaSC::CopyPose::CTL_POSITION)
+					iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
+				if (controltype & iTaSC::CopyPose::CTL_ROTATION)
+					iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
+				iktarget->constraint->registerCallback(copypose_callback, iktarget);
+				iktarget->errorCallback = copypose_error;
+				iktarget->controlType = controltype;
+				// add the constraint
+				ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
+			}
+			break;
+		case CONSTRAINT_IK_DISTANCE:
+			iktarget->constraint = new iTaSC::Distance(bonelen);
+			iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
+			iktarget->constraint->registerCallback(distance_callback, iktarget);
+			iktarget->errorCallback = distance_error;
+			// we can update the weight on each substep
+			iktarget->constraint->substep(true);
+			// add the constraint
+			ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
+			break;
+		}
+		if (!ret)
+			break;
+	}
+	if (!ret ||
+		!scene->addCache(ikscene->cache) ||
+		!scene->addSolver(ikscene->solver) ||
+		!scene->initialize()) {
+		delete ikscene;
+		ikscene = NULL;
+	}
+	return ikscene;
+}
+
+static void create_scene(Scene *scene, Object *ob)
+{
+	bPoseChannel *pchan;
+
+	// create the IK scene
+	for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
+		// by construction there is only one tree
+		PoseTree *tree = (PoseTree*)pchan->iktree.first;
+		if (tree) {
+			IK_Data* ikdata = get_ikdata(ob->pose);
+			// convert tree in iTaSC::Scene
+			IK_Scene* ikscene = convert_tree(scene, ob, pchan);
+			if (ikscene) {
+				ikscene->next = ikdata->first;
+				ikdata->first = ikscene;
+			}
+			// delete the trees once we are done
+			while(tree) {
+				BLI_remlink(&pchan->iktree, tree);
+				BLI_freelistN(&tree->targets);
+				if(tree->pchan) MEM_freeN(tree->pchan);
+				if(tree->parent) MEM_freeN(tree->parent);
+				if(tree->basis_change) MEM_freeN(tree->basis_change);
+				MEM_freeN(tree);
+				tree = (PoseTree*)pchan->iktree.first;
+			}
+		}
+	}
+}
+
+static void init_scene(Object *ob)
+{
+	bPoseChannel *pchan;
+
+	if (ob->pose->ikdata) {
+		for(IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first;
+			scene != NULL;
+			scene = scene->next) {
+			scene->channels[0].pchan->flag |= POSE_IKTREE;
+		}
+	}
+}
+
+static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
+{
+	int i;
+	IK_Channel* ikchan;
+	if (ikparam->flag & ITASC_SIMULATION) {
+		for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
+			// In simulation mode we don't allow external contraint to change our bones, mark the channel done
+			// also tell Blender that this channel is part of IK tree (cleared on each where_is_pose()
+			ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
+			ikchan->jointValid = 0;
+		}
+	} else {
+		// in animation mode, we must get the bone position from action and constraints
+		for(i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
+			if (!(ikchan->pchan->flag & POSE_DONE))
+				where_is_pose_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime);
+			// tell blender that this channel was controlled by IK, it's cleared on each where_is_pose()
+			ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
+			ikchan->jointValid = 0;
+		}
+	}
+	// only run execute the scene if at least one of our target is enabled
+	for (i=ikscene->targets.size(); i > 0; --i) {
+		IK_Target* iktarget = ikscene->targets[i-1];
+		if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF))
+			break;
+	}
+	if (i == 0 && ikscene->armature->getNrOfConstraints() == 0)
+		// all constraint disabled
+		return;
+
+	// compute timestep
+	double timestamp = ctime * frtime + 2147483.648;
+	double timestep = frtime;
+	bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false;
+	int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep;
+	bool simulation = true;
+
+	if (ikparam->flag & ITASC_SIMULATION) {
+		ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
+	} 
+	else {
+		// in animation mode we start from the pose after action and constraint
+		convert_pose(ikscene);
+		ikscene->armature->setJointArray(ikscene->jointArray);
+		// and we don't handle velocity
+		reiterate = true;
+		simulation = false;
+		// time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
+		// and choose 1s timestep to allow having velocity parameters in radiant 
+		timestep = 1.0;
+		// use auto setup to let the solver test the variation of the joints
+		numstep = 0;
+	}
+		
+	if (ikscene->cache && !reiterate && simulation) {
+		iTaSC::CacheTS sts, cts, dts;
+		sts = cts = (iTaSC::CacheTS)(timestamp*1000.0+0.5);
+		if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) {
+			// the cache is empty before this time, reiterate
+			if (ikparam->flag & ITASC_INITIAL_REITERATION)
+				reiterate = true;
+		} else {
+			// can take the cache as a start point.
+			sts -= cts;
+			timestep = sts/1000.0;
+		}
+	}
+	// don't cache if we are reiterating because we don't want to distroy the cache unnecessarily
+	ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
+	if (reiterate) {
+		// how many times do we reiterate?
+		for (i=0; i<ikparam->numiter; i++) {
+			if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
+				ikscene->armature->getMaxEndEffectorChange() < ikparam->precision)
+				break;
+			ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation);
+		}
+		if (simulation) {
+			// one more fake iteration to cache
+			ikscene->scene->update(timestamp, 0.0, 1, true, true, true);
+		}
+	}
+	// compute constraint error
+	for (i=ikscene->targets.size(); i > 0; --i) {
+		IK_Target* iktarget = ikscene->targets[i-1];
+		if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) {
+			unsigned int nvalues;
+			const iTaSC::ConstraintValues* values;
+			values = iktarget->constraint->getControlParameters(&nvalues);
+			iktarget->errorCallback(values, nvalues, iktarget);
+		}
+	}
+	// Apply result to bone:
+	// walk the ikscene->channels
+	// for each, get the Frame of the joint corresponding to the bone relative to its parent
+	// combine the parent and the joint frame to get the frame relative to armature
+	// a backward translation of the bone length gives the head
+	// if TY, compute the scale as the ratio of the joint length with rest pose length
+	iTaSC::Armature* arm = ikscene->armature;
+	KDL::Frame frame;
+	double q_rest[3], q[3];
+	const KDL::Joint* joint;
+	const KDL::Frame* tip;
+	bPoseChannel* pchan;
+	float scale;
+	float length;
+	float yaxis[3];
+	for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; ++i, ++ikchan) {
+		if (i == 0) {
+			if (!arm->getRelativeFrame(frame, ikchan->tail))
+				break;
+			// this frame is relative to base, make it relative to object
+			ikchan->frame = ikscene->baseFrame * frame;
+		} 
+		else {
+			if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail))
+				break;
+			// combine with parent frame to get frame relative to object
+			ikchan->frame = ikscene->channels[ikchan->parent].frame * frame;
+		}
+		// ikchan->frame is the tail frame relative to object
+		// get bone length
+		if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip))
+			break;
+		if (joint->getType() == KDL::Joint::TransY) {
+			// stretch bones have a TY joint, compute the scale
+			scale = (float)(q[0]/q_rest[0]);
+			// the length is the joint itself
+			length = (float)q[0];
+		} 
+		else {
+			scale = 1.0f;
+			// for fixed bone, the length is in the tip (always along Y axis)
+			length = tip->p(1);
+		}
+		// ready to compute the pose mat
+		pchan = ikchan->pchan;
+		// tail mat
+		ikchan->frame.getValue(&pchan->pose_mat[0][0]);
+		VECCOPY(pchan->pose_tail, pchan->pose_mat[3]);
+		// shift to head
+		VECCOPY(yaxis, pchan->pose_mat[1]);
+		VecMulf(yaxis, length);
+		VecSubf(pchan->pose_mat[3], pchan->pose_mat[3], yaxis);
+		VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
+		// add scale
+		VecMulf(pchan->pose_mat[0], scale);
+		VecMulf(pchan->pose_mat[1], scale);
+		VecMulf(pchan->pose_mat[2], scale);
+	}
+	if (i<ikscene->numchan) {
+		// big problem
+		;
+	}
+}
+
+//---------------------------------------------------
+// plugin interface
+//
+void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
+{
+	bPoseChannel *pchan;
+	int count = 0;
+
+	if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
+		init_scene(ob);
+		return;
+	}
+	// first remove old scene
+	itasc_clear_data(ob->pose);
+	// we should handle all the constraint and mark them all disabled
+	// for blender but we'll start with the IK constraint alone
+	for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
+		if(pchan->constflag & PCHAN_HAS_IK)
+			count += initialize_scene(ob, pchan);
+	}
+	// if at least one tree, create the scenes from the PoseTree stored in the channels 
+	if (count)
+		create_scene(scene, ob);
+	itasc_update_param(ob->pose);
+	// make sure we don't rebuilt until the user changes something important
+	ob->pose->flag &= ~POSE_WAS_REBUILT;
+}
+
+void itasc_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime)
+{
+	if (ob->pose->ikdata) {
+		IK_Data* ikdata = (IK_Data*)ob->pose->ikdata;
+		bItasc* ikparam = (bItasc*) ob->pose->ikparam;
+		// we need default parameters
+		if (!ikparam) ikparam = &DefIKParam;
+
+		for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
+			if (ikscene->channels[0].pchan == pchan) {
+				float timestep = scene->r.frs_sec_base/scene->r.frs_sec;
+				if (ob->pose->flag & POSE_GAME_ENGINE) {
+					timestep = ob->pose->ctime;
+					// limit the timestep to avoid excessive number of iteration
+					if (timestep > 0.2f)
+						timestep = 0.2f;
+				}
+				execute_scene(scene, ikscene, ikparam, ctime, timestep);
+				break;
+			}
+		}
+	}
+}
+
+void itasc_release_tree(struct Scene *scene, struct Object *ob,  float ctime)
+{
+	// not used for iTaSC
+}
+
+void itasc_clear_data(struct bPose *pose)
+{
+	if (pose->ikdata) {
+		IK_Data* ikdata = (IK_Data*)pose->ikdata;
+		for (IK_Scene* scene = ikdata->first; scene; scene = ikdata->first) {
+			ikdata->first = scene->next;
+			delete scene;
+		}
+		MEM_freeN(ikdata);
+		pose->ikdata = NULL;
+	}
+}
+
+void itasc_clear_cache(struct bPose *pose)
+{
+	if (pose->ikdata) {
+		IK_Data* ikdata = (IK_Data*)pose->ikdata;
+		for (IK_Scene* scene = ikdata->first; scene; scene = scene->next) {
+			if (scene->cache)
+				// clear all cache but leaving the timestamp 0 (=rest pose)
+				scene->cache->clearCacheFrom(NULL, 1);
+		}
+	}
+}
+
+void itasc_update_param(struct bPose *pose)
+{
+	if (pose->ikdata && pose->ikparam) {
+		IK_Data* ikdata = (IK_Data*)pose->ikdata;
+		bItasc* ikparam = (bItasc*)pose->ikparam;
+		for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
+			double armlength = ikscene->armature->getArmLength();
+			ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax*armlength);
+			ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps*armlength);
+			if (ikparam->flag & ITASC_SIMULATION) {
+				ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
+				ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
+				ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
+				ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback);
+			} else {
+				// in animation mode timestep is 1s by convention => 
+				// qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration
+				ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0);
+				ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0);
+				ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52);
+				ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8);
+			}
+		}
+	}
+}
+
+void itasc_test_constraint(struct Object *ob, struct bConstraint *cons)
+{
+	struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data;
+
+	/* only for IK constraint */
+	if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == NULL)
+		return;
+
+	switch (data->type) {
+	case CONSTRAINT_IK_COPYPOSE:
+	case CONSTRAINT_IK_DISTANCE:
+		/* cartesian space constraint */
+		break;
+	}
+}
+
diff --git a/source/blender/ikplugin/intern/itasc_plugin.h b/source/blender/ikplugin/intern/itasc_plugin.h
new file mode 100644
index 00000000000..25e48965a52
--- /dev/null
+++ b/source/blender/ikplugin/intern/itasc_plugin.h
@@ -0,0 +1,52 @@
+/**
+ * $Id$
+ * ***** 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.
+ *
+ * Original author: Benoit Bolsee
+ * Contributor(s): 
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef ITASC_PLUGIN_H
+#define ITASC_PLUGIN_H
+
+#include "ikplugin_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void itasc_initialize_tree(struct Scene *scene, struct Object *ob, float ctime);
+void itasc_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime);
+void itasc_release_tree(struct Scene *scene, struct Object *ob,  float ctime);
+void itasc_clear_data(struct bPose *pose);
+void itasc_clear_cache(struct bPose *pose);
+void itasc_update_param(struct bPose *pose);
+void itasc_test_constraint(struct Object *ob, struct bConstraint *cons);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ITASC_PLUGIN_H
+
diff --git a/source/blender/makesdna/DNA_action_types.h b/source/blender/makesdna/DNA_action_types.h
index 42696eeb092..43ef9f28828 100644
--- a/source/blender/makesdna/DNA_action_types.h
+++ b/source/blender/makesdna/DNA_action_types.h
@@ -146,7 +146,9 @@ typedef struct bPoseChannel {
 	float		limitmin[3], limitmax[3];	/* DOF constraint */
 	float		stiffness[3];				/* DOF stiffness */
 	float		ikstretch;
-	
+	float		ikrotweight;		/* weight of joint rotation constraint */
+	float		iklinweight;		/* weight of joint stretch constraint */
+
 	float		*path;				/* totpath x 3 x float */
 	struct Object *custom;			/* draws custom object instead of this channel */
 } bPoseChannel;
@@ -166,7 +168,8 @@ typedef enum ePchan_Flag {
 	POSE_CHAIN		=	0x0200,
 	POSE_DONE		=   0x0400,
 	POSE_KEY		=	0x1000,
-	POSE_STRIDE		=	0x2000
+	POSE_STRIDE		=	0x2000,
+	POSE_IKTREE		=   0x4000,
 } ePchan_Flag;
 
 /* PoseChannel constflag (constraint detection) */
@@ -190,9 +193,13 @@ typedef enum ePchan_IkFlag {
 	BONE_IK_YLIMIT	= (1<<4),
 	BONE_IK_ZLIMIT	= (1<<5),
 	
+	BONE_IK_ROTCTL  = (1<<6),
+	BONE_IK_LINCTL  = (1<<7),
+
 	BONE_IK_NO_XDOF_TEMP = (1<<10),
 	BONE_IK_NO_YDOF_TEMP = (1<<11),
-	BONE_IK_NO_ZDOF_TEMP = (1<<12)
+	BONE_IK_NO_ZDOF_TEMP = (1<<12),
+
 } ePchan_IkFlag;
 
 /* PoseChannel->rotmode */
@@ -209,6 +216,7 @@ typedef enum ePchan_RotMode {
 	/* NOTE: space is reserved here for 18 other possible 
 	 * euler rotation orders not implemented 
 	 */
+	PCHAN_ROT_MAX,	/* sentinel for Py API*/
 		/* axis angle rotations */
 	PCHAN_ROT_AXISANGLE = -1
 } ePchan_RotMode;
@@ -233,7 +241,9 @@ typedef struct bPose {
 	ListBase agroups;			/* list of bActionGroups */
 	
 	int active_group;			/* index of active group (starts from 1) */
-	int pad;
+	int	iksolver;				/* ik solver to use, see ePose_IKSolverType */
+	void *ikdata;				/* temporary IK data, depends on the IK solver. Not saved in file */
+	void *ikparam;				/* IK solver parameters, structure depends on iksolver */ 
 } bPose;
 
 
@@ -249,8 +259,53 @@ typedef enum ePose_Flags {
 	POSE_CONSTRAINTS_TIMEDEPEND = (1<<3),
 		/* recalculate bone paths */
 	POSE_RECALCPATHS = (1<<4),
+	    /* set by armature_rebuild_pose to give a chance to the IK solver to rebuild IK tree */
+	POSE_WAS_REBUILT = (1<<5),
+	    /* set by game_copy_pose to indicate that this pose is used in the game engine */
+	POSE_GAME_ENGINE = (1<<6),
 } ePose_Flags;
 
+/* bPose->iksolver and bPose->ikparam->iksolver */
+typedef enum {
+	IKSOLVER_LEGACY = 0,
+	IKSOLVER_ITASC,
+} ePose_IKSolverType;
+
+/* header for all bPose->ikparam structures */
+typedef struct bIKParam {
+	int   iksolver;
+} bIKParam;
+
+/* bPose->ikparam when bPose->iksolver=1 */
+typedef struct bItasc {
+	int   iksolver;
+	float precision;
+	short numiter;
+	short numstep;
+	float minstep;
+	float maxstep;
+	short solver;	
+	short flag;
+	float feedback;
+	float maxvel;	/* max velocity to SDLS solver */
+	float dampmax;	/* maximum damping for DLS solver */
+	float dampeps;	/* threshold of singular value from which the damping start progressively */
+} bItasc;
+
+/* bItasc->flag */
+typedef enum {
+	ITASC_AUTO_STEP = (1<<0),
+	ITASC_INITIAL_REITERATION = (1<<1),
+	ITASC_REITERATION = (1<<2),
+	ITASC_SIMULATION = (1<<3),
+} eItasc_Flags;
+
+/* bItasc->solver */
+typedef enum {
+	ITASC_SOLVER_SDLS = 0,	/* selective damped least square, suitable for CopyPose constraint */
+	ITASC_SOLVER_DLS		/* damped least square with numerical filtering of damping */
+} eItasc_Solver;
+
 /* ************************************************ */
 /* Action */
 
diff --git a/source/blender/makesdna/DNA_actuator_types.h b/source/blender/makesdna/DNA_actuator_types.h
index 278da27faf9..58fa38ae159 100644
--- a/source/blender/makesdna/DNA_actuator_types.h
+++ b/source/blender/makesdna/DNA_actuator_types.h
@@ -142,7 +142,7 @@ typedef struct bGroupActuator {
 	char name[32];		/* property or groupkey */
 	
 	short pad[3], cur, butsta, butend;/* not referenced, can remove? */
-	struct Group *group;		/* only during game */
+	/* struct Group *group;		not used, remove */
 	
 } bGroupActuator;
 
@@ -224,6 +224,15 @@ typedef struct bStateActuator {
 	unsigned int mask;	/* the bits to change */
 } bStateActuator;
 
+typedef struct bArmatureActuator {
+	char posechannel[32];
+	char constraint[32];
+	int type;		/* 0=run, 1=enable, 2=disable, 3=set target, 4=set weight */
+	float weight;
+	struct Object *target;
+	struct Object *subtarget;
+} bArmatureActuator;
+
 typedef struct bActuator {
 	struct bActuator *next, *prev, *mynew;
 	short type;
@@ -295,6 +304,7 @@ typedef struct FreeCamera {
 #define ACT_PARENT      20
 #define ACT_SHAPEACTION 21
 #define ACT_STATE		22
+#define ACT_ARMATURE	23
 
 /* actuator flag */
 #define ACT_SHOW		1
@@ -484,6 +494,15 @@ typedef struct FreeCamera {
 #define ACT_PARENT_COMPOUND	1
 #define ACT_PARENT_GHOST	2
 
+/* armatureactuator->type */
+#define ACT_ARM_RUN			0
+#define ACT_ARM_ENABLE		1
+#define ACT_ARM_DISABLE		2
+#define ACT_ARM_SETTARGET	3
+#define ACT_ARM_SETWEIGHT	4
+/* update this define if more type are addedd */
+#define ACT_ARM_MAXTYPE		4
+
 #endif
 
 
diff --git a/source/blender/makesdna/DNA_constraint_types.h b/source/blender/makesdna/DNA_constraint_types.h
index 70430af3fc8..fccec7a556f 100644
--- a/source/blender/makesdna/DNA_constraint_types.h
+++ b/source/blender/makesdna/DNA_constraint_types.h
@@ -66,6 +66,9 @@ typedef struct bConstraint {
 	int			pad;
 	
 	struct Ipo *ipo;		/* local influence ipo or driver */ // XXX depreceated for 2.5... old animation system hack
+	/* below are readonly fields that are set at runtime by the solver for use in the GE (only IK atm) */
+	float       lin_error;		/* residual error on constraint expressed in blender unit*/
+	float       rot_error;		/* residual error on constraint expressed in radiant */
 } bConstraint;
 
 
@@ -119,24 +122,34 @@ typedef struct bPythonConstraint {
 } bPythonConstraint;
 
 
-/* Inverse-Kinematics (IK) constraint */
+/* inverse-Kinematics (IK) constraint
+   This constraint supports a variety of mode determine by the type field 
+   according to B_CONSTRAINT_IK_TYPE.
+   Some fields are used by all types, some are specific to some types
+   This is indicated in the comments for each field
+ */
 typedef struct bKinematicConstraint {
-	Object		*tar;
-	short		iterations;		/* Maximum number of iterations to try */
-	short		flag;			/* Like CONSTRAINT_IK_TIP */
-	short		rootbone;		/* index to rootbone, if zero go all the way to mother bone */
-	short		max_rootbone;	/* for auto-ik, maximum length of chain */
-	char		subtarget[32];	/* String to specify sub-object target */
-
-	Object		*poletar;			/* Pole vector target */
-	char		polesubtarget[32];	/* Pole vector sub-object target */
-	float		poleangle;			/* Pole vector rest angle */
-
-	float		weight;			/* Weight of goal in IK tree */
-	float		orientweight;	/* Amount of rotation a target applies on chain */
-	float		grabtarget[3];	/* for target-less IK */
+	Object		*tar;			/* All: target object in case constraint needs a target */
+	short		iterations;		/* All: Maximum number of iterations to try */
+	short		flag;			/* All & CopyPose: some options Like CONSTRAINT_IK_TIP */
+	short		rootbone;		/* All: index to rootbone, if zero go all the way to mother bone */
+	short		max_rootbone;	/* CopyPose: for auto-ik, maximum length of chain */
+	char		subtarget[32];	/* All: String to specify sub-object target */
+	Object		*poletar;			/* All: Pole vector target */
+	char		polesubtarget[32];	/* All: Pole vector sub-object target */
+	float		poleangle;			/* All: Pole vector rest angle */
+	float		weight;			/* All: Weight of constraint in IK tree */
+	float		orientweight;	/* CopyPose: Amount of rotation a target applies on chain */
+	float		grabtarget[3];	/* CopyPose: for target-less IK */
+	short		type;			/* subtype of IK constraint: B_CONSTRAINT_IK_TYPE */
+	short 		mode;			/* Distance: how to limit in relation to clamping sphere: LIMITDIST_.. */
+	float 		dist;			/* Distance: distance (radius of clamping sphere) from target */
 } bKinematicConstraint;
 
+typedef enum B_CONSTRAINT_IK_TYPE {
+	CONSTRAINT_IK_COPYPOSE = 0,		/* 'standard' IK constraint: match position and/or orientation of target */
+	CONSTRAINT_IK_DISTANCE			/* maintain distance with target */
+} B_CONSTRAINT_IK_TYPE;
 
 /* Single-target subobject constraints ---------------------  */
 /* Track To Constraint */
@@ -376,7 +389,9 @@ typedef enum B_CONSTRAINT_FLAG {
 		/* influence ipo is on constraint itself, not in action channel */
 	CONSTRAINT_OWN_IPO	= (1<<7),
 		/* indicates that constraint was added locally (i.e.  didn't come from the proxy-lib) */
-	CONSTRAINT_PROXY_LOCAL = (1<<8)
+	CONSTRAINT_PROXY_LOCAL = (1<<8),
+		/* indicates that constraint is temporarily disabled (only used in GE) */
+	CONSTRAINT_OFF = (1<<9)
 } B_CONSTRAINT_FLAG;
 
 /* bConstraint->ownspace/tarspace */
diff --git a/source/blender/makesdna/DNA_sensor_types.h b/source/blender/makesdna/DNA_sensor_types.h
index cc998de7eec..a5ba5886ed5 100644
--- a/source/blender/makesdna/DNA_sensor_types.h
+++ b/source/blender/makesdna/DNA_sensor_types.h
@@ -126,6 +126,13 @@ typedef struct bRaySensor {
 	int axisflag;
 } bRaySensor;
 
+typedef struct bArmatureSensor {
+	char posechannel[32];
+	char constraint[32];
+	int  type;
+	float value;
+} bArmatureSensor;
+
 typedef struct bMessageSensor {
 	/**
 	 * (Possible future use) pointer to a single sender object
@@ -202,6 +209,15 @@ typedef struct bJoystickSensor {
 #define SENS_MESG_MESG		0
 #define SENS_MESG_PROP		1
 
+/* bArmatureSensor->type */
+#define SENS_ARM_STATE_CHANGED		0
+#define SENS_ARM_LIN_ERROR_BELOW	1
+#define SENS_ARM_LIN_ERROR_ABOVE	2
+#define SENS_ARM_ROT_ERROR_BELOW	3
+#define SENS_ARM_ROT_ERROR_ABOVE	4
+/* update this when adding new type */
+#define SENS_ARM_MAXTYPE			4
+
 /* sensor->type */
 #define SENS_ALWAYS		0
 #define SENS_TOUCH		1
@@ -217,6 +233,7 @@ typedef struct bJoystickSensor {
 #define SENS_JOYSTICK  11
 #define SENS_ACTUATOR  12
 #define SENS_DELAY     13
+#define SENS_ARMATURE  14
 /* sensor->flag */
 #define SENS_SHOW		1
 #define SENS_DEL		2
diff --git a/source/blender/makesrna/RNA_access.h b/source/blender/makesrna/RNA_access.h
index b1f5292f9d4..9ea7725b855 100644
--- a/source/blender/makesrna/RNA_access.h
+++ b/source/blender/makesrna/RNA_access.h
@@ -333,6 +333,7 @@ extern StructRNA RNA_Pose;
 extern StructRNA RNA_PoseChannel;
 extern StructRNA RNA_Property;
 extern StructRNA RNA_PropertySensor;
+extern StructRNA RNA_ArmatureSensor;
 extern StructRNA RNA_PythonConstraint;
 extern StructRNA RNA_PythonController;
 extern StructRNA RNA_RadarSensor;
diff --git a/source/blender/makesrna/SConscript b/source/blender/makesrna/SConscript
index 845abf636e2..8fc9df0fbf6 100644
--- a/source/blender/makesrna/SConscript
+++ b/source/blender/makesrna/SConscript
@@ -7,7 +7,7 @@ o = SConscript('intern/SConscript')
 objs += o
 
 incs = '#/intern/guardedalloc ../blenkernel ../blenlib ../makesdna intern .'
-incs += ' ../windowmanager ../editors/include ../imbuf'
+incs += ' ../windowmanager ../editors/include ../imbuf ../ikplugin'
 incs += ' ../render/extern/include'
 
 defs = []
diff --git a/source/blender/makesrna/intern/CMakeLists.txt b/source/blender/makesrna/intern/CMakeLists.txt
index 709c5d017ec..50cf0b00b84 100644
--- a/source/blender/makesrna/intern/CMakeLists.txt
+++ b/source/blender/makesrna/intern/CMakeLists.txt
@@ -39,7 +39,7 @@ SET(SRC
 		../../../../intern/guardedalloc/intern/mallocn.c
 		../../../../intern/guardedalloc/intern/mmap_win.c)
 
-INCLUDE_DIRECTORIES(../../../../intern/guardedalloc .. ../../makesdna ../../blenkernel ../../blenlib ../../windowmanager ../../editors/include ../../imbuf ../../render/extern/include .)
+INCLUDE_DIRECTORIES(../../../../intern/guardedalloc .. ../../makesdna ../../blenkernel ../../blenlib ../../ikplugin ../../windowmanager ../../editors/include ../../imbuf ../../render/extern/include .)
 FILE(GLOB INC_FILES ../*.h ../../makesdna/*.h)
 
 IF(WITH_GAMEENGINE)
diff --git a/source/blender/makesrna/intern/Makefile b/source/blender/makesrna/intern/Makefile
index 4a4e41edd15..7923ea1e7de 100644
--- a/source/blender/makesrna/intern/Makefile
+++ b/source/blender/makesrna/intern/Makefile
@@ -49,6 +49,7 @@ CPPFLAGS += -I$(NAN_GUARDEDALLOC)/include
 CPPFLAGS += -I../../blenlib
 CPPFLAGS += -I../../blenkernel
 CPPFLAGS += -I../../imbuf
+CPPFLAGS += -I../../ikplugin
 CPPFLAGS += -I../../makesdna
 CPPFLAGS += -I../../windowmanager
 CPPFLAGS += -I../../editors/include
diff --git a/source/blender/makesrna/intern/SConscript b/source/blender/makesrna/intern/SConscript
index 569f0547731..0f8bc752f09 100644
--- a/source/blender/makesrna/intern/SConscript
+++ b/source/blender/makesrna/intern/SConscript
@@ -30,7 +30,7 @@ makesrna_tool.Append(CCFLAGS = '-DBASE_HEADER="\\"source/blender/makesrna/\\"" '
 defs = []
 
 incs = '#/intern/guardedalloc ../../blenlib ../../blenkernel'
-incs += ' ../../imbuf ../../makesdna ../../makesrna'
+incs += ' ../../imbuf ../../makesdna ../../makesrna ../../ikplugin'
 incs += ' ../../windowmanager ../../editors/include'
 incs += ' ../../render/extern/include'
 
diff --git a/source/blender/makesrna/intern/rna_actuator.c b/source/blender/makesrna/intern/rna_actuator.c
index 473e726db60..ce83d1c469b 100644
--- a/source/blender/makesrna/intern/rna_actuator.c
+++ b/source/blender/makesrna/intern/rna_actuator.c
@@ -58,6 +58,7 @@ void RNA_def_actuator(BlenderRNA *brna)
 		{ACT_PARENT, "PARENT", 0, "Parent", ""},
 		{ACT_SHAPEACTION, "SHAPE_ACTION", 0, "Shape Action", ""},
 		{ACT_STATE, "STATE", 0, "State", ""},
+		{ACT_ARMATURE, "ARMATURE", 0, "Armature", ""},
 		{0, NULL, 0, NULL, NULL}};
 
 	srna= RNA_def_struct(brna, "Actuator", NULL);
diff --git a/source/blender/makesrna/intern/rna_constraint.c b/source/blender/makesrna/intern/rna_constraint.c
index 86aa2a1d135..b630e61a680 100644
--- a/source/blender/makesrna/intern/rna_constraint.c
+++ b/source/blender/makesrna/intern/rna_constraint.c
@@ -33,6 +33,7 @@
 #include "DNA_object_types.h"
 #include "DNA_scene_types.h"
 
+#include "ED_object.h"
 #include "WM_types.h"
 
 EnumPropertyItem constraint_type_items[] ={
@@ -80,6 +81,19 @@ EnumPropertyItem space_object_items[] = {
 	{1, "LOCAL", 0, "Local (Without Parent) Space", ""},
 	{0, NULL, 0, NULL, NULL}};
 
+EnumPropertyItem constraint_ik_type_items[] ={
+	{CONSTRAINT_IK_COPYPOSE, "COPY_POSE", 0, "Copy Pose", ""},
+	{CONSTRAINT_IK_DISTANCE, "DISTANCE", 0, "Distance", ""},
+	{0, NULL, 0, NULL, NULL},
+};
+
+static EnumPropertyItem constraint_distance_items[] = {
+	{LIMITDIST_INSIDE, "LIMITDIST_INSIDE", 0, "Inside", ""},
+	{LIMITDIST_OUTSIDE, "LIMITDIST_OUTSIDE", 0, "Outside", ""},
+	{LIMITDIST_ONSURFACE, "LIMITDIST_ONSURFACE", 0, "On Surface", ""},
+	{0, NULL, 0, NULL, NULL}
+};
+
 #ifdef RNA_RUNTIME
 
 #include "BKE_action.h"
@@ -160,24 +174,12 @@ static char *rna_Constraint_path(PointerRNA *ptr)
 
 static void rna_Constraint_update(bContext *C, PointerRNA *ptr)
 {
-	Object *ob= ptr->id.data;
-
-	if(ob->pose) update_pose_constraint_flags(ob->pose);
-
-	object_test_constraints(ob);
-
-	if(ob->type==OB_ARMATURE) DAG_id_flush_update(&ob->id, OB_RECALC_DATA|OB_RECALC_OB);
-	else DAG_id_flush_update(&ob->id, OB_RECALC_OB);
+	ED_object_constraint_update(ptr->id.data);
 }
 
 static void rna_Constraint_dependency_update(bContext *C, PointerRNA *ptr)
 {
-	Object *ob= ptr->id.data;
-
-	rna_Constraint_update(C, ptr);
-
-	if(ob->pose) ob->pose->flag |= POSE_RECALC;	// checks & sorts pose channels
-    DAG_scene_sort(CTX_data_scene(C));
+	ED_object_constraint_dependency_update(CTX_data_scene(C), ptr->id.data);
 }
 
 static void rna_Constraint_influence_update(bContext *C, PointerRNA *ptr)
@@ -190,6 +192,24 @@ static void rna_Constraint_influence_update(bContext *C, PointerRNA *ptr)
 	rna_Constraint_update(C, ptr);
 }
 
+static void rna_Constraint_ik_type_set(struct PointerRNA *ptr, int value)
+{
+	bConstraint *con = ptr->data;
+	bKinematicConstraint *ikdata = con->data;
+
+	if (ikdata->type != value) {
+		// the type of IK constraint has changed, set suitable default values
+		// in case constraints reuse same fields incompatible
+		switch (value) {
+		case CONSTRAINT_IK_COPYPOSE:
+			break;
+		case CONSTRAINT_IK_DISTANCE:
+			break;
+		}
+		ikdata->type = value;
+	}
+}
+
 static EnumPropertyItem *rna_Constraint_owner_space_itemf(bContext *C, PointerRNA *ptr, int *free)
 {
 	Object *ob= (Object*)ptr->id.data;
@@ -456,21 +476,40 @@ static void rna_def_constraint_kinematic(BlenderRNA *brna)
 	prop= RNA_def_property(srna, "tail", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", CONSTRAINT_IK_TIP);
 	RNA_def_property_ui_text(prop, "Use Tail", "Include bone's tail as last element in chain.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
 
 	prop= RNA_def_property(srna, "rotation", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", CONSTRAINT_IK_ROT);
 	RNA_def_property_ui_text(prop, "Rotation", "Chain follows rotation of target.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
 
 	prop= RNA_def_property(srna, "targetless", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", CONSTRAINT_IK_AUTO);
 	RNA_def_property_ui_text(prop, "Targetless", "Use targetless IK.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
 
 	prop= RNA_def_property(srna, "stretch", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "flag", CONSTRAINT_IK_STRETCH);
 	RNA_def_property_ui_text(prop, "Stretch", "Enable IK Stretching.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
+
+	prop= RNA_def_property(srna, "ik_type", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "type");
+	RNA_def_property_enum_funcs(prop, NULL, "rna_Constraint_ik_type_set", NULL);
+	RNA_def_property_enum_items(prop, constraint_ik_type_items);
+	RNA_def_property_ui_text(prop, "IK Type", "");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
+
+	prop= RNA_def_property(srna, "limit_mode", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "mode");
+	RNA_def_property_enum_items(prop, constraint_distance_items);
+	RNA_def_property_ui_text(prop, "Limit Mode", "Distances in relation to sphere of influence to allow.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_dependency_update");
+
+	prop= RNA_def_property(srna, "distance", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "dist");
+	RNA_def_property_range(prop, 0.0, 100.f);
+	RNA_def_property_ui_text(prop, "Distance", "Radius of limiting sphere.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_update");
 }
 
@@ -1475,12 +1514,6 @@ static void rna_def_constraint_distance_limit(BlenderRNA *brna)
 	StructRNA *srna;
 	PropertyRNA *prop;
 
-	static EnumPropertyItem distance_items[] = {
-		{LIMITDIST_INSIDE, "LIMITDIST_INSIDE", 0, "Inside", ""},
-		{LIMITDIST_OUTSIDE, "LIMITDIST_OUTSIDE", 0, "Outside", ""},
-		{LIMITDIST_ONSURFACE, "LIMITDIST_ONSURFACE", 0, "On Surface", ""},
-		{0, NULL, 0, NULL, NULL}};
-
 	srna= RNA_def_struct(brna, "LimitDistanceConstraint", "Constraint");
 	RNA_def_struct_ui_text(srna, "Limit Distance Constraint", "Limits the distance from target object.");
 	RNA_def_struct_sdna_from(srna, "bDistLimitConstraint", "data");
@@ -1504,7 +1537,7 @@ static void rna_def_constraint_distance_limit(BlenderRNA *brna)
 
 	prop= RNA_def_property(srna, "limit_mode", PROP_ENUM, PROP_NONE);
 	RNA_def_property_enum_sdna(prop, NULL, "mode");
-	RNA_def_property_enum_items(prop, distance_items);
+	RNA_def_property_enum_items(prop, constraint_distance_items);
 	RNA_def_property_ui_text(prop, "Limit Mode", "Distances in relation to sphere of influence to allow.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_update");
 }
@@ -1622,7 +1655,18 @@ void RNA_def_constraint(BlenderRNA *brna)
 	RNA_def_property_range(prop, 0.0f, 1.0f);
 	RNA_def_property_ui_text(prop, "Influence", "Amount of influence constraint will have on the final solution.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_CONSTRAINT, "rna_Constraint_influence_update");
-	
+
+	/* readonly values */
+	prop= RNA_def_property(srna, "lin_error", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "lin_error");
+	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
+	RNA_def_property_ui_text(prop, "Lin error", "Amount of residual error in Blender space unit for constraints that work on position.");
+
+	prop= RNA_def_property(srna, "rot_error", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "rot_error");
+	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
+	RNA_def_property_ui_text(prop, "Rot error", "Amount of residual error in radiant for constraints that work on orientation.");
+
 	/* pointers */
 	rna_def_constrainttarget(brna);
 
diff --git a/source/blender/makesrna/intern/rna_pose.c b/source/blender/makesrna/intern/rna_pose.c
index 7cb7513f474..e76cd56af4e 100644
--- a/source/blender/makesrna/intern/rna_pose.c
+++ b/source/blender/makesrna/intern/rna_pose.c
@@ -23,6 +23,7 @@
  */
 
 #include <stdlib.h>
+#include <string.h>
 
 #include "RNA_define.h"
 #include "RNA_types.h"
@@ -39,8 +40,8 @@
 
 #ifdef RNA_RUNTIME
 
-#include <string.h>
-
+#include "BIK_api.h"
+#include "BKE_action.h"
 #include "BLI_arithb.h"
 
 #include "DNA_userdef_types.h"
@@ -49,8 +50,11 @@
 #include "BKE_depsgraph.h"
 #include "BKE_idprop.h"
 
+#include "ED_object.h"
 #include "ED_armature.h"
 
+#include "MEM_guardedalloc.h"
+
 static void rna_Pose_update(bContext *C, PointerRNA *ptr)
 {
 	// XXX when to use this? ob->pose->flag |= (POSE_LOCKED|POSE_DO_UNLOCK);
@@ -58,6 +62,15 @@ static void rna_Pose_update(bContext *C, PointerRNA *ptr)
 	DAG_id_flush_update(ptr->id.data, OB_RECALC_DATA);
 }
 
+static void rna_Pose_IK_update(bContext *C, PointerRNA *ptr)
+{
+	// XXX when to use this? ob->pose->flag |= (POSE_LOCKED|POSE_DO_UNLOCK);
+	Object *ob= ptr->id.data;
+
+	DAG_id_flush_update(&ob->id, OB_RECALC_DATA);
+	BIK_clear_data(ob->pose);
+}
+
 static char *rna_PoseChannel_path(PointerRNA *ptr)
 {
 	return BLI_sprintfN("pose.pose_channels[\"%s\"]", ((bPoseChannel*)ptr->data)->name);
@@ -110,6 +123,38 @@ static IDProperty *rna_PoseChannel_idproperties(PointerRNA *ptr, int create)
 	return pchan->prop;
 }
 
+static void rna_Pose_ik_solver_set(struct PointerRNA *ptr, int value)
+{
+	bPose *pose= (bPose*)ptr->data;
+
+	if (pose->iksolver != value) {
+		// the solver has changed, must clean any temporary structures
+		BIK_clear_data(pose);
+		if (pose->ikparam) {
+			MEM_freeN(pose->ikparam);
+			pose->ikparam = NULL;
+		}
+		pose->iksolver = value;
+		init_pose_ikparam(pose);
+	}
+}
+
+static void rna_Pose_ik_solver_update(bContext *C, PointerRNA *ptr)
+{
+	Object *ob= ptr->id.data;
+	bPose *pose = ptr->data;
+	Scene *scene = CTX_data_scene(C);
+
+	pose->flag |= POSE_RECALC;	// checks & sorts pose channels
+	DAG_scene_sort(scene);
+	
+	update_pose_constraint_flags(pose);
+	
+	object_test_constraints(ob);
+
+	DAG_id_flush_update(&ob->id, OB_RECALC_DATA|OB_RECALC_OB);
+}
+
 /* rotation - euler angles */
 static void rna_PoseChannel_euler_rotation_get(PointerRNA *ptr, float *value)
 {
@@ -236,6 +281,70 @@ static int rna_PoseChannel_has_ik_get(PointerRNA *ptr)
 	return ED_pose_channel_in_IK_chain(ob, pchan);
 }
 
+StructRNA *rna_IKParam_refine(PointerRNA *ptr)
+{
+	bIKParam *param = (bIKParam *)ptr->data;
+
+	switch (param->iksolver) {
+	case IKSOLVER_ITASC:
+		return &RNA_Itasc;
+	default:
+		return &RNA_IKParam;
+	}
+}
+
+PointerRNA rna_Pose_ikparam_get(struct PointerRNA *ptr)
+{
+	bPose *pose= (bPose*)ptr->data;
+	return rna_pointer_inherit_refine(ptr, &RNA_IKParam, pose->ikparam);
+}
+
+static StructRNA *rna_Pose_ikparam_typef(PointerRNA *ptr)
+{
+	bPose *pose= (bPose*)ptr->data;
+
+	switch (pose->iksolver) {
+	case IKSOLVER_ITASC:
+		return &RNA_Itasc;
+	default:
+		return &RNA_IKParam;
+	}
+}
+
+static void rna_Itasc_update(bContext *C, PointerRNA *ptr)
+{
+	Object *ob = ptr->id.data;
+	bItasc *itasc = ptr->data;
+
+	/* verify values */
+	if (itasc->precision < 0.0001f)
+		itasc->precision = 0.0001f;
+	if (itasc->minstep < 0.001f)
+		itasc->minstep = 0.001f;
+	if (itasc->maxstep < itasc->minstep)
+		itasc->maxstep = itasc->minstep;
+	if (itasc->feedback < 0.01f)
+		itasc->feedback = 0.01f;
+	if (itasc->feedback > 100.f)
+		itasc->feedback = 100.f;
+	if (itasc->maxvel < 0.01f)
+		itasc->maxvel = 0.01f;
+	if (itasc->maxvel > 100.f)
+		itasc->maxvel = 100.f;
+	BIK_update_param(ob->pose);
+
+	DAG_id_flush_update(&ob->id, OB_RECALC_DATA);
+}
+
+static void rna_Itasc_update_rebuild(bContext *C, PointerRNA *ptr)
+{
+	Object *ob= ptr->id.data;
+	bPose *pose = ob->pose;
+
+	pose->flag |= POSE_RECALC;	// checks & sorts pose channels
+	rna_Itasc_update(C, ptr);
+}
+
 static PointerRNA rna_PoseChannel_bone_group_get(PointerRNA *ptr)
 {
 	Object *ob= (Object*)ptr->id.data;
@@ -438,6 +547,16 @@ static void rna_def_bone_group(BlenderRNA *brna)
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 }
 
+static EnumPropertyItem prop_iksolver_items[] = {
+	{IKSOLVER_LEGACY, "LEGACY", 0, "Legacy", "Original IK solver."},
+	{IKSOLVER_ITASC, "ITASC", 0, "iTaSC", "Multi constraint, stateful IK solver."},
+	{0, NULL, 0, NULL, NULL}};
+
+static EnumPropertyItem prop_solver_items[] = {
+	{ITASC_SOLVER_SDLS, "SDLS", 0, "SDLS", "Selective Damped Least Square"},
+	{ITASC_SOLVER_DLS, "DLS", 0, "DLS", "Damped Least Square with Numerical Filtering"},
+	{0, NULL, 0, NULL, NULL}};
+
 static void rna_def_pose_channel(BlenderRNA *brna)
 {
 	static EnumPropertyItem prop_rotmode_items[] = {
@@ -470,7 +589,7 @@ static void rna_def_pose_channel(BlenderRNA *brna)
 	RNA_def_property_string_funcs(prop, NULL, NULL, "rna_PoseChannel_name_set");
 	RNA_def_property_ui_text(prop, "Name", "");
 	RNA_def_struct_name_property(srna, prop);
-	
+
 	prop= RNA_def_property(srna, "selected", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "selectflag", BONE_SELECTED);
 	RNA_def_property_ui_text(prop, "Selected", "");
@@ -480,13 +599,13 @@ static void rna_def_pose_channel(BlenderRNA *brna)
 	RNA_def_property_int_sdna(prop, NULL, "pathsf");
 	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
 	RNA_def_property_ui_text(prop, "Bone Paths Calculation Start Frame", "Starting frame of range of frames to use for Bone Path calculations.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE|ND_TRANSFORM, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 
 	prop= RNA_def_property(srna, "path_end_frame", PROP_INT, PROP_TIME);
 	RNA_def_property_int_sdna(prop, NULL, "pathef");
 	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
 	RNA_def_property_ui_text(prop, "Bone Paths Calculation End Frame", "End frame of range of frames to use for Bone Path calculations.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE|ND_TRANSFORM, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 	
 	/* Relationships to other bones */
 	prop= RNA_def_property(srna, "bone", PROP_POINTER, PROP_NONE);
@@ -581,96 +700,118 @@ static void rna_def_pose_channel(BlenderRNA *brna)
 	RNA_def_property_boolean_funcs(prop,  "rna_PoseChannel_has_ik_get", NULL);
 	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
 	RNA_def_property_ui_text(prop, "Has IK", "Is part of an IK chain.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_dof_x", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_negative_sdna(prop, NULL, "ikflag", BONE_IK_NO_XDOF);
 	RNA_def_property_ui_text(prop, "IK X DoF", "Allow movement around the X axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_dof_y", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_negative_sdna(prop, NULL, "ikflag", BONE_IK_NO_YDOF);
 	RNA_def_property_ui_text(prop, "IK Y DoF", "Allow movement around the Y axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE|ND_TRANSFORM, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_dof_z", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_negative_sdna(prop, NULL, "ikflag", BONE_IK_NO_ZDOF);
 	RNA_def_property_ui_text(prop, "IK Z DoF", "Allow movement around the Z axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_limit_x", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "ikflag", BONE_IK_XLIMIT);
 	RNA_def_property_ui_text(prop, "IK X Limit", "Limit movement around the X axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_limit_y", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "ikflag", BONE_IK_YLIMIT);
 	RNA_def_property_ui_text(prop, "IK Y Limit", "Limit movement around the Y axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_limit_z", PROP_BOOLEAN, PROP_NONE);
 	RNA_def_property_boolean_sdna(prop, NULL, "ikflag", BONE_IK_ZLIMIT);
 	RNA_def_property_ui_text(prop, "IK Z Limit", "Limit movement around the Z axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
+	
+	prop= RNA_def_property(srna, "ik_rot_control", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "ikflag", BONE_IK_ROTCTL);
+	RNA_def_property_ui_text(prop, "IK rot control", "Apply channel rotation as IK constraint");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
+	
+	prop= RNA_def_property(srna, "ik_lin_control", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "ikflag", BONE_IK_LINCTL);
+	RNA_def_property_ui_text(prop, "IK rot control", "Apply channel size as IK constraint if stretching is enabled");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 	
 	prop= RNA_def_property(srna, "ik_min_x", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmin[0]");
 	RNA_def_property_range(prop, -180.0f, 0.0f);
 	RNA_def_property_ui_text(prop, "IK X Minimum", "Minimum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_max_x", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmax[0]");
 	RNA_def_property_range(prop, 0.0f, 180.0f);
 	RNA_def_property_ui_text(prop, "IK X Maximum", "Maximum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_min_y", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmin[1]");
 	RNA_def_property_range(prop, -180.0f, 0.0f);
 	RNA_def_property_ui_text(prop, "IK Y Minimum", "Minimum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_max_y", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmax[1]");
 	RNA_def_property_range(prop, 0.0f, 180.0f);
 	RNA_def_property_ui_text(prop, "IK Y Maximum", "Maximum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_min_z", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmin[2]");
 	RNA_def_property_range(prop, -180.0f, 0.0f);
 	RNA_def_property_ui_text(prop, "IK Z Minimum", "Minimum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_max_z", PROP_FLOAT, PROP_ANGLE);
 	RNA_def_property_float_sdna(prop, NULL, "limitmax[2]");
 	RNA_def_property_range(prop, 0.0f, 180.0f);
 	RNA_def_property_ui_text(prop, "IK Z Maximum", "Maximum angles for IK Limit");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_stiffness_x", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "stiffness[0]");
 	RNA_def_property_range(prop, 0.0f, 0.99f);
 	RNA_def_property_ui_text(prop, "IK X Stiffness", "IK stiffness around the X axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_stiffness_y", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "stiffness[1]");
 	RNA_def_property_range(prop, 0.0f, 0.99f);
 	RNA_def_property_ui_text(prop, "IK Y Stiffness", "IK stiffness around the Y axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_stiffness_z", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "stiffness[2]");
 	RNA_def_property_range(prop, 0.0f, 0.99f);
 	RNA_def_property_ui_text(prop, "IK Z Stiffness", "IK stiffness around the Z axis.");
-	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
 
 	prop= RNA_def_property(srna, "ik_stretch", PROP_FLOAT, PROP_NONE);
 	RNA_def_property_float_sdna(prop, NULL, "ikstretch");
 	RNA_def_property_range(prop, 0.0f,1.0f);
 	RNA_def_property_ui_text(prop, "IK Stretch", "Allow scaling of the bone for IK.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_IK_update");
+	
+	prop= RNA_def_property(srna, "ik_rot_weight", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "ikrotweight");
+	RNA_def_property_range(prop, 0.0f,1.0f);
+	RNA_def_property_ui_text(prop, "IK Rot Weight", "Weight of rotation constraint for IK.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
+	
+	prop= RNA_def_property(srna, "ik_lin_weight", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "iklinweight");
+	RNA_def_property_range(prop, 0.0f,1.0f);
+	RNA_def_property_ui_text(prop, "IK Lin Weight", "Weight of scale constraint for IK.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 	
 	/* custom bone shapes */
@@ -727,6 +868,113 @@ static void rna_def_pose_channel(BlenderRNA *brna)
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 }
 
+static void rna_def_pose_itasc(BlenderRNA *brna)
+{
+	StructRNA *srna;
+	PropertyRNA *prop;
+
+	srna= RNA_def_struct(brna, "Itasc", "IKParam");
+	RNA_def_struct_sdna(srna, "bItasc");
+	RNA_def_struct_ui_text(srna, "bItasc", "Parameters for the iTaSC IK solver.");
+
+	prop= RNA_def_property(srna, "precision", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "precision");
+	RNA_def_property_range(prop, 0.0f,0.1f);
+	RNA_def_property_ui_text(prop, "Precision", "Precision of convergence in case of reiteration.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "num_iter", PROP_INT, PROP_NONE);
+	RNA_def_property_int_sdna(prop, NULL, "numiter");
+	RNA_def_property_range(prop, 1.f,1000.f);
+	RNA_def_property_ui_text(prop, "Iterations", "Maximum number of iterations for convergence in case of reiteration.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "num_step", PROP_INT, PROP_NONE);
+	RNA_def_property_int_sdna(prop, NULL, "numstep");
+	RNA_def_property_range(prop, 1.f, 50.f);
+	RNA_def_property_ui_text(prop, "Num steps", "Divides the frame interval into this many steps.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "simulation", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "flag", ITASC_SIMULATION);
+	RNA_def_property_ui_text(prop, "Simulation", "Simulation mode: solver is statefull, runs in real-time context and ignores actions and non-IK constraints (i.e. solver is in full charge of the IK chain).");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update_rebuild");
+
+	prop= RNA_def_property(srna, "initial_reiteration", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "flag", ITASC_INITIAL_REITERATION);
+	RNA_def_property_ui_text(prop, "Initial Reiteration", "Allow reiteration for initial frame.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "reiteration", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "flag", ITASC_REITERATION);
+	RNA_def_property_ui_text(prop, "Reiteration", "Allow reiteration for all frames.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "auto_step", PROP_BOOLEAN, PROP_NONE);
+	RNA_def_property_boolean_sdna(prop, NULL, "flag", ITASC_AUTO_STEP);
+	RNA_def_property_ui_text(prop, "Auto step", "Automatically determine the optimal number of steps for best performance/accurary trade off.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "min_step", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "minstep");
+	RNA_def_property_range(prop, 0.0f,0.1f);
+	RNA_def_property_ui_text(prop, "Min step", "Lower bound for timestep in second in case of automatic substeps.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "max_step", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "maxstep");
+	RNA_def_property_range(prop, 0.0f,1.0f);
+	RNA_def_property_ui_text(prop, "Max step", "Higher bound for timestep in second in case of automatic substeps.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "feedback", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "feedback");
+	RNA_def_property_range(prop, 0.0f,100.0f);
+	RNA_def_property_ui_text(prop, "Feedback", "Feedback coefficient for error correction. Average response time=1/feedback. Default=20.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "max_velocity", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "maxvel");
+	RNA_def_property_range(prop, 0.0f,100.0f);
+	RNA_def_property_ui_text(prop, "Max Velocity", "Maximum joint velocity in rad/s. Default=50.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "solver", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "solver");
+	RNA_def_property_enum_items(prop, prop_solver_items);
+	RNA_def_property_ui_text(prop, "Solver", "Solving method selection: Automatic damping or manual damping");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update_rebuild");
+
+	prop= RNA_def_property(srna, "dampmax", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "dampmax");
+	RNA_def_property_range(prop, 0.0f,1.0f);
+	RNA_def_property_ui_text(prop, "Damp", "Maximum damping coefficient when singular value is nearly 0. Higher values=more stability, less reactivity. Default=0.5");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+
+	prop= RNA_def_property(srna, "dampeps", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "dampeps");
+	RNA_def_property_range(prop, 0.0f,1.0f);
+	RNA_def_property_ui_text(prop, "Epsilon", "Singular value under which damping is progressively applied. Higher values=more stability, less reactivity. Default=0.1");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Itasc_update");
+}
+
+static void rna_def_pose_ikparam(BlenderRNA *brna)
+{
+	StructRNA *srna;
+	PropertyRNA *prop;
+
+	srna= RNA_def_struct(brna, "IKParam", NULL);
+	RNA_def_struct_sdna(srna, "bIKParam");
+	RNA_def_struct_ui_text(srna, "IKParam", "Base type for IK solver parameters.");
+	RNA_def_struct_refine_func(srna, "rna_IKParam_refine");
+
+	prop= RNA_def_property(srna, "ik_solver", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "iksolver");
+	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
+	RNA_def_property_enum_items(prop, prop_iksolver_items);
+	RNA_def_property_ui_text(prop, "IK Solver", "IK solver for which these parameters are defined, 0 for Legacy, 1 for iTaSC.");
+}
+
 static void rna_def_pose(BlenderRNA *brna)
 {
 	StructRNA *srna;
@@ -762,6 +1010,19 @@ static void rna_def_pose(BlenderRNA *brna)
 	RNA_def_property_ui_text(prop, "Active Bone Group Index", "Active index in bone groups array.");
 	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_update");
 
+	prop= RNA_def_property(srna, "ik_solver", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "iksolver");
+	RNA_def_property_enum_funcs(prop, NULL, "rna_Pose_ik_solver_set", NULL);
+	RNA_def_property_enum_items(prop, prop_iksolver_items);
+	RNA_def_property_ui_text(prop, "IK Solver", "Selection of IK solver for IK chain, current choice is 0 for Legacy, 1 for iTaSC.");
+	RNA_def_property_update(prop, NC_OBJECT|ND_POSE, "rna_Pose_ik_solver_update");
+
+	prop= RNA_def_property(srna, "ik_param", PROP_POINTER, PROP_NONE);
+	RNA_def_property_struct_type(prop, "IKParam");
+	RNA_def_property_pointer_funcs(prop, "rna_Pose_ikparam_get", NULL, "rna_Pose_ikparam_typef");
+	RNA_def_property_clear_flag(prop, PROP_EDITABLE);
+	RNA_def_property_ui_text(prop, "IK Param", "Parameters for IK solver.");
+
 	/* RNA_api_pose(srna); */
 }
 
@@ -769,7 +1030,8 @@ void RNA_def_pose(BlenderRNA *brna)
 {
 	rna_def_pose(brna);
 	rna_def_pose_channel(brna);
-	
+	rna_def_pose_ikparam(brna);
+	rna_def_pose_itasc(brna);
 	rna_def_bone_group(brna);
 }
 
diff --git a/source/blender/makesrna/intern/rna_sensor.c b/source/blender/makesrna/intern/rna_sensor.c
index a5d76fdb039..1003af6d4d1 100644
--- a/source/blender/makesrna/intern/rna_sensor.c
+++ b/source/blender/makesrna/intern/rna_sensor.c
@@ -48,6 +48,8 @@ static StructRNA* rna_Sensor_refine(struct PointerRNA *ptr)
 			return &RNA_KeyboardSensor;
 		case SENS_PROPERTY:
 			return &RNA_PropertySensor;
+		case SENS_ARMATURE:
+			return &RNA_ArmatureSensor;
 		case SENS_MOUSE:
 			return &RNA_MouseSensor;
 		case SENS_COLLISION:
@@ -92,6 +94,7 @@ static void rna_def_sensor(BlenderRNA *brna)
 		{SENS_JOYSTICK, "JOYSTICK", 0, "joystick", ""},
 		{SENS_ACTUATOR, "ACTUATOR", 0, "Actuator", ""},
 		{SENS_DELAY, "DELAY", 0, "Delay", ""},
+		{SENS_ARMATURE, "ARMATURE", 0, "Armature", ""},
 		{0, NULL, 0, NULL, NULL}};
 
 	srna= RNA_def_struct(brna, "Sensor", NULL);
@@ -278,6 +281,40 @@ static void rna_def_property_sensor(BlenderRNA *brna)
 	RNA_def_property_ui_text(prop, "Maximum Value", "Specify maximum value in Interval type.");
 }
 
+static void rna_def_armature_sensor(BlenderRNA *brna)
+{
+	StructRNA *srna;
+	PropertyRNA *prop;
+	static EnumPropertyItem prop_type_items[] ={
+		{SENS_ARM_STATE_CHANGED, "STATECHG", 0, "State Changed", ""},
+		{SENS_ARM_LIN_ERROR_BELOW, "LINERRORBELOW", 0, "Lin error below", ""},
+		{SENS_ARM_LIN_ERROR_ABOVE, "LINERRORABOVE", 0, "Lin error above", ""},
+		{SENS_ARM_ROT_ERROR_BELOW, "ROTERRORBELOW", 0, "Rot error below", ""},
+		{SENS_ARM_ROT_ERROR_ABOVE, "ROTERRORBELOW", 0, "Rot error above", ""},
+		{0, NULL, 0, NULL, NULL}};
+
+	srna= RNA_def_struct(brna, "ArmatureSensor", "Sensor");
+	RNA_def_struct_ui_text(srna, "Armature Sensor", "Sensor to detect values and changes in values of IK solver.");
+	RNA_def_struct_sdna_from(srna, "bArmatureSensor", "data");
+
+	prop= RNA_def_property(srna, "type", PROP_ENUM, PROP_NONE);
+	RNA_def_property_enum_sdna(prop, NULL, "type");
+	RNA_def_property_enum_items(prop, prop_type_items);
+	RNA_def_property_ui_text(prop, "Test Type", "Type of value and test.");
+
+	prop= RNA_def_property(srna, "channel_name", PROP_STRING, PROP_NONE);
+	RNA_def_property_string_sdna(prop, NULL, "posechannel");
+	RNA_def_property_ui_text(prop, "Bone name", "Identify the bone to check value from");
+
+	prop= RNA_def_property(srna, "constraint_name", PROP_STRING, PROP_NONE);
+	RNA_def_property_string_sdna(prop, NULL, "constraint");
+	RNA_def_property_ui_text(prop, "Constraint name", "Identify the bone constraint to check value from.");
+
+	prop= RNA_def_property(srna, "value", PROP_FLOAT, PROP_NONE);
+	RNA_def_property_float_sdna(prop, NULL, "value");
+	RNA_def_property_ui_text(prop, "Compare Value", "Specify value to be used in comparison.");
+}
+
 static void rna_def_actuator_sensor(BlenderRNA *brna)
 {
 	StructRNA *srna;
@@ -531,6 +568,7 @@ void RNA_def_sensor(BlenderRNA *brna)
 	rna_def_touch_sensor(brna);
 	rna_def_keyboard_sensor(brna);
 	rna_def_property_sensor(brna);
+	rna_def_armature_sensor(brna);
 	rna_def_actuator_sensor(brna);
 	rna_def_delay_sensor(brna);
 	rna_def_collision_sensor(brna);
diff --git a/source/gameengine/Converter/BL_ActionActuator.cpp b/source/gameengine/Converter/BL_ActionActuator.cpp
index ca4290703e1..2f0e3e9fa65 100644
--- a/source/gameengine/Converter/BL_ActionActuator.cpp
+++ b/source/gameengine/Converter/BL_ActionActuator.cpp
@@ -526,12 +526,11 @@ KX_PYMETHODDEF_DOC(BL_ActionActuator, setChannel,
 		mat.setValue((const float *)matrix);
 		
 		BL_ArmatureObject *obj = (BL_ArmatureObject*)GetParent();
-		obj->GetPose(&m_pose); /* Get the underlying pose from the armature */
 		
 		if (!m_userpose) {
 			if(!m_pose)
 				obj->GetPose(&m_pose); /* Get the underlying pose from the armature */
-			game_copy_pose(&m_userpose, m_pose);
+			game_copy_pose(&m_userpose, m_pose, 0);
 		}
 		// pchan= verify_pose_channel(m_userpose, string); // adds the channel if its not there.
 		pchan= get_pose_channel(m_userpose, string); // adds the channel if its not there.
@@ -554,7 +553,7 @@ KX_PYMETHODDEF_DOC(BL_ActionActuator, setChannel,
 		if (!m_userpose) {
 			if(!m_pose)
 				obj->GetPose(&m_pose); /* Get the underlying pose from the armature */
-			game_copy_pose(&m_userpose, m_pose);
+			game_copy_pose(&m_userpose, m_pose, 0);
 		}
 		// pchan= verify_pose_channel(m_userpose, string);
 		pchan= get_pose_channel(m_userpose, string); // adds the channel if its not there.
@@ -572,7 +571,6 @@ KX_PYMETHODDEF_DOC(BL_ActionActuator, setChannel,
 		return NULL;
 	}
 	
-	pchan->flag |= POSE_ROT|POSE_LOC|POSE_SIZE;
 	Py_RETURN_NONE;
 }
 
diff --git a/source/gameengine/Converter/BL_ActionActuator.h b/source/gameengine/Converter/BL_ActionActuator.h
index a6b4c4a055d..2e0e38b4cc3 100644
--- a/source/gameengine/Converter/BL_ActionActuator.h
+++ b/source/gameengine/Converter/BL_ActionActuator.h
@@ -50,7 +50,7 @@ public:
 						short	priority,
 						short	end_reset,
 						float	stride) 
-		: SCA_IActuator(gameobj),
+		: SCA_IActuator(gameobj, KX_ACT_ACTION),
 		
 		m_lastpos(0, 0, 0),
 		m_blendframe(0),
diff --git a/source/gameengine/Converter/BL_ArmatureActuator.cpp b/source/gameengine/Converter/BL_ArmatureActuator.cpp
new file mode 100644
index 00000000000..741f1cf0553
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureActuator.cpp
@@ -0,0 +1,265 @@
+/**
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include "DNA_action_types.h"
+#include "DNA_constraint_types.h"
+#include "DNA_actuator_types.h"
+#include "BKE_constraint.h"
+#include "BLI_arithb.h"
+#include "BL_ArmatureActuator.h"
+#include "BL_ArmatureObject.h"
+
+/**
+ * This class is the conversion of the Pose channel constraint.
+ * It makes a link between the pose constraint and the KX scene.
+ * The main purpose is to give access to the constraint target 
+ * to link it to a game object. 
+ * It also allows to activate/deactivate constraints during the game.
+ * Later it will also be possible to create constraint on the fly
+ */
+
+BL_ArmatureActuator::BL_ArmatureActuator(SCA_IObject* obj,
+						int type,
+						const char *posechannel,
+						const char *constraintname,
+						KX_GameObject* targetobj,
+						KX_GameObject* subtargetobj,
+						float weight) :
+	SCA_IActuator(obj, KX_ACT_ARMATURE),
+	m_constraint(NULL),
+	m_gametarget(targetobj),
+	m_gamesubtarget(subtargetobj),
+	m_posechannel(posechannel),
+	m_constraintname(constraintname),
+	m_weight(weight),
+	m_type(type)
+{
+	if (m_gametarget)
+		m_gametarget->RegisterActuator(this);
+	if (m_gamesubtarget)
+		m_gamesubtarget->RegisterActuator(this);
+	FindConstraint();
+}
+
+BL_ArmatureActuator::~BL_ArmatureActuator()
+{
+	if (m_gametarget)
+		m_gametarget->UnregisterActuator(this);
+	if (m_gamesubtarget)
+		m_gamesubtarget->UnregisterActuator(this);
+}
+
+void BL_ArmatureActuator::ProcessReplica()
+{
+	// the replica is tracking the same object => register it (this may be changed in Relnk())
+	if (m_gametarget)
+		m_gametarget->RegisterActuator(this);
+	if (m_gamesubtarget)
+		m_gamesubtarget->UnregisterActuator(this);
+	SCA_IActuator::ProcessReplica();
+}
+
+void BL_ArmatureActuator::ReParent(SCA_IObject* parent)
+{
+	SCA_IActuator::ReParent(parent);
+	// must remap the constraint
+	FindConstraint();
+}
+
+bool BL_ArmatureActuator::UnlinkObject(SCA_IObject* clientobj)
+{
+	bool res=false;
+	if (clientobj == m_gametarget)
+	{
+		// this object is being deleted, we cannot continue to track it.
+		m_gametarget = NULL;
+		res = true;
+	}
+	if (clientobj == m_gamesubtarget)
+	{
+		// this object is being deleted, we cannot continue to track it.
+		m_gamesubtarget = NULL;
+		res = true;
+	}
+	return res;
+}
+
+void BL_ArmatureActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
+{
+	void **h_obj = (*obj_map)[m_gametarget];
+	if (h_obj) {
+		if (m_gametarget)
+			m_gametarget->UnregisterActuator(this);
+		m_gametarget = (KX_GameObject*)(*h_obj);
+		m_gametarget->RegisterActuator(this);
+	}
+	h_obj = (*obj_map)[m_gamesubtarget];
+	if (h_obj) {
+		if (m_gamesubtarget)
+			m_gamesubtarget->UnregisterActuator(this);
+		m_gamesubtarget = (KX_GameObject*)(*h_obj);
+		m_gamesubtarget->RegisterActuator(this);
+	}
+}
+
+void BL_ArmatureActuator::FindConstraint()
+{
+	m_constraint = NULL;
+
+	if (m_gameobj->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE) {
+		BL_ArmatureObject* armobj = (BL_ArmatureObject*)m_gameobj;
+		m_constraint = armobj->GetConstraint(m_posechannel, m_constraintname);
+	}
+}
+
+bool BL_ArmatureActuator::Update(double curtime, bool frame)
+{
+	// the only role of this actuator is to ensure that the armature pose will be evaluated
+	bool result = false;	
+	bool bNegativeEvent = IsNegativeEvent();
+	RemoveAllEvents();
+
+	if (!bNegativeEvent) {
+		BL_ArmatureObject *obj = (BL_ArmatureObject*)GetParent();
+		switch (m_type) {
+		case ACT_ARM_RUN:
+			result = true;
+			obj->SetActiveAction(NULL, 0, curtime);
+			break;
+		case ACT_ARM_ENABLE:
+			if (m_constraint)
+				m_constraint->ClrConstraintFlag(CONSTRAINT_OFF);
+			break;
+		case ACT_ARM_DISABLE:
+			if (m_constraint)
+				m_constraint->SetConstraintFlag(CONSTRAINT_OFF);
+			break;
+		case ACT_ARM_SETTARGET:
+			if (m_constraint) {
+				m_constraint->SetTarget(m_gametarget);
+				m_constraint->SetSubtarget(m_gamesubtarget);
+			}
+			break;
+		case ACT_ARM_SETWEIGHT:
+			if (m_constraint)
+				m_constraint->SetWeight(m_weight);
+			break;
+		}
+	}
+	return result;
+}
+
+/* ------------------------------------------------------------------------- */
+/* Python Integration Hooks					                                 */
+/* ------------------------------------------------------------------------- */
+
+PyTypeObject BL_ArmatureActuator::Type = {
+#if (PY_VERSION_HEX >= 0x02060000)
+	PyVarObject_HEAD_INIT(NULL, 0)
+#else
+	/* python 2.5 and below */
+	PyObject_HEAD_INIT( NULL )  /* required py macro */
+	0,                          /* ob_size */
+#endif
+		"BL_ArmatureActuator",
+		sizeof(PyObjectPlus_Proxy),
+		0,
+		py_base_dealloc,
+		0,
+		0,
+		0,
+		0,
+		py_base_repr,
+		0,0,0,0,0,0,0,0,0,
+		Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+		0,0,0,0,0,0,0,
+		Methods,
+		0,
+		0,
+		&SCA_IActuator::Type,
+		0,0,0,0,0,0,
+		py_base_new
+};
+
+
+PyMethodDef BL_ArmatureActuator::Methods[] = {
+	{NULL,NULL} //Sentinel
+};
+
+PyAttributeDef BL_ArmatureActuator::Attributes[] = {
+	KX_PYATTRIBUTE_RO_FUNCTION("constraint", BL_ArmatureActuator, pyattr_get_constraint),
+	KX_PYATTRIBUTE_RW_FUNCTION("target", BL_ArmatureActuator, pyattr_get_object, pyattr_set_object),
+	KX_PYATTRIBUTE_RW_FUNCTION("subtarget", BL_ArmatureActuator, pyattr_get_object, pyattr_set_object),
+	KX_PYATTRIBUTE_FLOAT_RW("weight",0.0f,1.0f,BL_ArmatureActuator,m_weight),
+	KX_PYATTRIBUTE_INT_RW("type",0,ACT_ARM_MAXTYPE,false,BL_ArmatureActuator,m_type),
+	{ NULL }	//Sentinel
+};
+
+PyObject* BL_ArmatureActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureActuator* actuator = static_cast<BL_ArmatureActuator*>(self);
+	KX_GameObject *target = (!strcmp(attrdef->m_name, "target")) ? actuator->m_gametarget : actuator->m_gamesubtarget;
+	if (!target)	
+		Py_RETURN_NONE;
+	else
+		return target->GetProxy();
+}
+
+int BL_ArmatureActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
+{
+	BL_ArmatureActuator* actuator = static_cast<BL_ArmatureActuator*>(self);
+	KX_GameObject* &target = (!strcmp(attrdef->m_name, "target")) ? actuator->m_gametarget : actuator->m_gamesubtarget;
+	KX_GameObject *gameobj;
+		
+	if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: BL_ArmatureActuator"))
+		return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
+		
+	if (target != NULL)
+		target->UnregisterActuator(actuator);	
+
+	target = gameobj;
+		
+	if (target)
+		target->RegisterActuator(actuator);
+		
+	return PY_SET_ATTR_SUCCESS;
+}
+
+PyObject* BL_ArmatureActuator::pyattr_get_constraint(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureActuator* actuator = static_cast<BL_ArmatureActuator*>(self);
+	BL_ArmatureConstraint* constraint = actuator->m_constraint;
+	if (!constraint)	
+		Py_RETURN_NONE;
+	else
+		return constraint->GetProxy();
+}
+
+
+
diff --git a/source/gameengine/Converter/BL_ArmatureActuator.h b/source/gameengine/Converter/BL_ArmatureActuator.h
new file mode 100644
index 00000000000..5a7752b8169
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureActuator.h
@@ -0,0 +1,89 @@
+/**
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+#ifndef BL_ARMATUREACTUATOR
+#define BL_ARMATUREACTUATOR
+
+#include "SCA_IActuator.h"
+#include "BL_ArmatureConstraint.h"
+
+/**
+ * This class is the conversion of the Pose channel constraint.
+ * It makes a link between the pose constraint and the KX scene.
+ * The main purpose is to give access to the constraint target 
+ * to link it to a game object. 
+ * It also allows to activate/deactivate constraints during the game.
+ * Later it will also be possible to create constraint on the fly
+ */
+
+class	BL_ArmatureActuator : public SCA_IActuator
+{
+	Py_Header;
+public:
+	BL_ArmatureActuator(SCA_IObject* gameobj,
+						int type,
+						const char *posechannel,
+						const char *constraintname,
+						KX_GameObject* targetobj,
+						KX_GameObject* subtargetobj,
+						float weight);
+
+	virtual ~BL_ArmatureActuator();
+
+	virtual CValue* GetReplica() {
+		BL_ArmatureActuator* replica = new BL_ArmatureActuator(*this);
+		replica->ProcessReplica();
+		return replica;
+	};
+	virtual void ProcessReplica();
+	virtual bool UnlinkObject(SCA_IObject* clientobj);
+	virtual void Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map);
+	virtual bool Update(double curtime, bool frame);
+	virtual void ReParent(SCA_IObject* parent);
+	
+	/* These are used to get and set m_target */
+	static PyObject* pyattr_get_constraint(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static PyObject* pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static int pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
+
+private:
+	// identify the constraint that this actuator controls
+	void FindConstraint();
+
+	BL_ArmatureConstraint*	m_constraint;
+	KX_GameObject*	m_gametarget;
+	KX_GameObject*	m_gamesubtarget;
+	STR_String		m_posechannel;
+	STR_String		m_constraintname;
+	float			m_weight;
+	int				m_type;
+};
+
+#endif //BL_ARMATUREACTUATOR
+
+
diff --git a/source/gameengine/Converter/BL_ArmatureChannel.cpp b/source/gameengine/Converter/BL_ArmatureChannel.cpp
new file mode 100644
index 00000000000..c0afeb008f6
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureChannel.cpp
@@ -0,0 +1,461 @@
+/**
+ * $Id$
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "DNA_armature_types.h"
+#include "BL_ArmatureChannel.h"
+#include "BL_ArmatureObject.h"
+#include "BL_ArmatureConstraint.h"
+#include "BLI_arithb.h"
+#include "BLI_string.h"
+
+PyTypeObject BL_ArmatureChannel::Type = {
+	PyVarObject_HEAD_INIT(NULL, 0)
+	"BL_ArmatureChannel",
+	sizeof(PyObjectPlus_Proxy),
+	0,
+	py_base_dealloc,
+	0,
+	0,
+	0,
+	0,
+	py_base_repr,
+	0,0,0,0,0,0,0,0,0,
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+	0,0,0,0,0,0,0,
+	Methods,
+	0,
+	0,
+	&CValue::Type,
+	0,0,0,0,0,0,
+	py_base_new
+};
+
+PyObject* BL_ArmatureChannel::py_repr(void)
+{
+	return PyUnicode_FromString(m_posechannel->name);
+}
+
+PyObject *BL_ArmatureChannel::GetProxy()
+{
+	return GetProxyPlus_Ext(this, &Type, m_posechannel);
+}
+
+PyObject *BL_ArmatureChannel::NewProxy(bool py_owns)
+{
+	return NewProxyPlus_Ext(this, &Type, m_posechannel, py_owns);
+}
+
+BL_ArmatureChannel::BL_ArmatureChannel(
+	BL_ArmatureObject *armature, 
+	bPoseChannel *posechannel)
+	: PyObjectPlus(), m_posechannel(posechannel), m_armature(armature)
+{
+}
+
+BL_ArmatureChannel::~BL_ArmatureChannel()
+{
+}
+
+// PYTHON
+
+PyMethodDef BL_ArmatureChannel::Methods[] = {
+  {NULL,NULL} //Sentinel
+};
+
+// order of definition of attributes, must match Attributes[] array
+#define BCA_BONE		0
+#define BCA_PARENT		1
+
+PyAttributeDef BL_ArmatureChannel::Attributes[] = {
+	// Keep these attributes in order of BCA_ defines!!! used by py_attr_getattr and py_attr_setattr
+	KX_PYATTRIBUTE_RO_FUNCTION("bone",BL_ArmatureChannel,py_attr_getattr),	
+	KX_PYATTRIBUTE_RO_FUNCTION("parent",BL_ArmatureChannel,py_attr_getattr),	
+	
+	{ NULL }	//Sentinel
+};
+
+/* attributes directly taken from bPoseChannel */
+PyAttributeDef BL_ArmatureChannel::AttributesPtr[] = {
+	KX_PYATTRIBUTE_CHAR_RO("name",bPoseChannel,name),
+	KX_PYATTRIBUTE_FLAG_RO("has_ik",bPoseChannel,flag, POSE_CHAIN),
+	KX_PYATTRIBUTE_FLAG_NEGATIVE_RO("ik_dof_x",bPoseChannel,ikflag, BONE_IK_NO_XDOF),
+	KX_PYATTRIBUTE_FLAG_NEGATIVE_RO("ik_dof_y",bPoseChannel,ikflag, BONE_IK_NO_YDOF),
+	KX_PYATTRIBUTE_FLAG_NEGATIVE_RO("ik_dof_z",bPoseChannel,ikflag, BONE_IK_NO_ZDOF),
+	KX_PYATTRIBUTE_FLAG_RO("ik_limit_x",bPoseChannel,ikflag, BONE_IK_XLIMIT),
+	KX_PYATTRIBUTE_FLAG_RO("ik_limit_y",bPoseChannel,ikflag, BONE_IK_YLIMIT),
+	KX_PYATTRIBUTE_FLAG_RO("ik_limit_z",bPoseChannel,ikflag, BONE_IK_ZLIMIT),
+	KX_PYATTRIBUTE_FLAG_RO("ik_rot_control",bPoseChannel,ikflag, BONE_IK_ROTCTL),
+	KX_PYATTRIBUTE_FLAG_RO("ik_lin_control",bPoseChannel,ikflag, BONE_IK_LINCTL),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RW("location",-FLT_MAX,FLT_MAX,bPoseChannel,loc,3),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RW("scale",-FLT_MAX,FLT_MAX,bPoseChannel,size,3),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RW("rotation",-1.0f,1.0f,bPoseChannel,quat,4),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RW("euler_rotation",-10.f,10.f,bPoseChannel,eul,3),
+	KX_PYATTRIBUTE_SHORT_RW("rotation_mode",0,PCHAN_ROT_MAX-1,false,bPoseChannel,rotmode),
+	KX_PYATTRIBUTE_FLOAT_MATRIX_RO("channel_matrix",bPoseChannel,chan_mat,4),
+	KX_PYATTRIBUTE_FLOAT_MATRIX_RO("pose_matrix",bPoseChannel,pose_mat,4),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("pose_head",bPoseChannel,pose_head,3),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("pose_tail",bPoseChannel,pose_tail,3),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_min_x",bPoseChannel,limitmin[0]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_max_x",bPoseChannel,limitmax[0]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_min_y",bPoseChannel,limitmin[1]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_max_y",bPoseChannel,limitmax[1]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_min_z",bPoseChannel,limitmin[2]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_max_z",bPoseChannel,limitmax[2]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_stiffness_x",bPoseChannel,stiffness[0]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_stiffness_y",bPoseChannel,stiffness[1]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_stiffness_z",bPoseChannel,stiffness[2]),
+	KX_PYATTRIBUTE_FLOAT_RO("ik_stretch",bPoseChannel,ikstretch),
+	KX_PYATTRIBUTE_FLOAT_RW("ik_rot_weight",0,1.0f,bPoseChannel,ikrotweight),
+	KX_PYATTRIBUTE_FLOAT_RW("ik_lin_weight",0,1.0f,bPoseChannel,iklinweight),
+	KX_PYATTRIBUTE_RW_FUNCTION("joint_rotation",BL_ArmatureChannel,py_attr_get_joint_rotation,py_attr_set_joint_rotation),
+	{ NULL }	//Sentinel
+};
+
+PyObject* BL_ArmatureChannel::py_attr_getattr(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureChannel* self= static_cast<BL_ArmatureChannel*>(self_v);
+	bPoseChannel* channel = self->m_posechannel;
+	int attr_order = attrdef-Attributes;
+
+	if (!channel) {
+		PyErr_SetString(PyExc_AttributeError, "channel is NULL");
+		return NULL;
+	}
+
+	switch (attr_order) {
+	case BCA_BONE:
+		// bones are standalone proxy
+		return NewProxyPlus_Ext(NULL,&BL_ArmatureBone::Type,channel->bone,false);
+	case BCA_PARENT:
+		{
+			BL_ArmatureChannel* parent = self->m_armature->GetChannel(channel->parent);
+			if (parent)
+				return parent->GetProxy();
+			else
+				Py_RETURN_NONE;
+		}
+	}
+	PyErr_SetString(PyExc_AttributeError, "channel unknown attribute");
+	return NULL;
+}
+
+int BL_ArmatureChannel::py_attr_setattr(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
+{
+	BL_ArmatureChannel* self= static_cast<BL_ArmatureChannel*>(self_v);
+	bPoseChannel* channel = self->m_posechannel;
+	int attr_order = attrdef-Attributes;
+
+	int ival;
+	double dval;
+	char* sval;
+	KX_GameObject *oval;
+
+	if (!channel) {
+		PyErr_SetString(PyExc_AttributeError, "channel is NULL");
+		return PY_SET_ATTR_FAIL;
+	}
+	
+	switch (attr_order) {
+	default:
+		break;
+	}
+
+	PyErr_SetString(PyExc_AttributeError, "channel unknown attribute");
+	return PY_SET_ATTR_FAIL;
+}
+
+PyObject* BL_ArmatureChannel::py_attr_get_joint_rotation(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureChannel* self= static_cast<BL_ArmatureChannel*>(self_v);
+	bPoseChannel* pchan = self->m_posechannel;
+	// decompose the pose matrix in euler rotation
+	float rest_mat[3][3];
+	float pose_mat[3][3];
+	float joint_mat[3][3];
+	float joints[3];
+	float norm;
+	double sa, ca;
+	// get rotation in armature space
+	Mat3CpyMat4(pose_mat, pchan->pose_mat);
+	Mat3Ortho(pose_mat);
+	if (pchan->parent) {
+		// bone has a parent, compute the rest pose of the bone taking actual pose of parent
+		Mat3IsMat3MulMat4(rest_mat, pchan->bone->bone_mat, pchan->parent->pose_mat);
+		Mat3Ortho(rest_mat);
+	} else {
+		// otherwise, the bone matrix in armature space is the rest pose
+		Mat3CpyMat4(rest_mat, pchan->bone->arm_mat);
+	}
+	// remove the rest pose to get the joint movement
+	Mat3Transp(rest_mat);
+	Mat3MulMat3(joint_mat, rest_mat, pose_mat);		
+	joints[0] = joints[1] = joints[2] = 0.f;
+	// returns a 3 element list that gives corresponding joint
+	int flag = 0;
+	if (!(pchan->ikflag & BONE_IK_NO_XDOF))
+		flag |= 1;
+	if (!(pchan->ikflag & BONE_IK_NO_YDOF))
+		flag |= 2;
+	if (!(pchan->ikflag & BONE_IK_NO_ZDOF))
+		flag |= 4;
+	switch (flag) {
+	case 0:	// fixed joint
+		break;
+	case 1:	// X only
+		Mat3ToEulO(joint_mat, joints, EULER_ORDER_XYZ);
+		joints[1] = joints[2] = 0.f;
+		break;
+	case 2:	// Y only
+		Mat3ToEulO(joint_mat, joints, EULER_ORDER_XYZ);
+		joints[0] = joints[2] = 0.f;
+		break;
+	case 3:	// X+Y
+		Mat3ToEulO(joint_mat, joints, EULER_ORDER_ZYX);
+		joints[2] = 0.f;
+		break;
+	case 4:	// Z only
+		Mat3ToEulO(joint_mat, joints, EULER_ORDER_XYZ);
+		joints[0] = joints[1] = 0.f;
+		break;
+	case 5:	// X+Z
+		// decompose this as an equivalent rotation vector in X/Z plane
+		joints[0] = joint_mat[1][2];
+		joints[2] = -joint_mat[1][0];
+		norm = Normalize(joints);
+		if (norm < FLT_EPSILON) {
+			norm = (joint_mat[1][1] < 0.f) ? M_PI : 0.f;
+		} else {
+			norm = acos(joint_mat[1][1]);
+		}
+		VecMulf(joints, norm);
+		break;
+	case 6:	// Y+Z
+		Mat3ToEulO(joint_mat, joints, EULER_ORDER_XYZ);
+		joints[0] = 0.f;
+		break;
+	case 7: // X+Y+Z
+		// equivalent axis
+		joints[0] = (joint_mat[1][2]-joint_mat[2][1])*0.5f;
+		joints[1] = (joint_mat[2][0]-joint_mat[0][2])*0.5f;
+		joints[2] = (joint_mat[0][1]-joint_mat[1][0])*0.5f;
+		sa = VecLength(joints);
+		ca = (joint_mat[0][0]+joint_mat[1][1]+joint_mat[1][1]-1.0f)*0.5f;
+		if (sa > FLT_EPSILON) {
+			norm = atan2(sa,ca)/sa;
+		} else {
+		   if (ca < 0.0) {
+			   norm = M_PI;
+			   VecMulf(joints,0.f);
+			   if (joint_mat[0][0] > 0.f) {
+				   joints[0] = 1.0f;
+			   } else if (joint_mat[1][1] > 0.f) {
+				   joints[1] = 1.0f;
+			   } else {
+				   joints[2] = 1.0f;
+			   }
+		   } else {
+			   norm = 0.0;
+		   }
+		}
+		VecMulf(joints,norm);
+		break;
+	}
+	return newVectorObject(joints, 3, Py_NEW, NULL);
+}
+
+int BL_ArmatureChannel::py_attr_set_joint_rotation(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
+{
+	BL_ArmatureChannel* self= static_cast<BL_ArmatureChannel*>(self_v);
+	bPoseChannel* pchan = self->m_posechannel;
+	PyObject *item;
+	float joints[3];
+	float quat[4];
+
+	if (!PySequence_Check(value) || PySequence_Size(value) != 3) {
+		PyErr_SetString(PyExc_AttributeError, "expected a sequence of [3] floats");
+		return PY_SET_ATTR_FAIL;
+	}
+	for (int i=0; i<3; i++) {
+		item = PySequence_GetItem(value, i); /* new ref */
+		joints[i] = PyFloat_AsDouble(item);
+		Py_DECREF(item);
+		if (joints[i] == -1.0f && PyErr_Occurred()) {
+			PyErr_SetString(PyExc_AttributeError, "expected a sequence of [3] floats");
+			return PY_SET_ATTR_FAIL;
+		}
+	}
+
+	int flag = 0;
+	if (!(pchan->ikflag & BONE_IK_NO_XDOF))
+		flag |= 1;
+	if (!(pchan->ikflag & BONE_IK_NO_YDOF))
+		flag |= 2;
+	if (!(pchan->ikflag & BONE_IK_NO_ZDOF))
+		flag |= 4;
+	QuatOne(quat);
+	switch (flag) {
+	case 0:	// fixed joint
+		break;
+	case 1:	// X only
+		joints[1] = joints[2] = 0.f;
+		EulOToQuat(joints, EULER_ORDER_XYZ, quat);
+		break;
+	case 2:	// Y only
+		joints[0] = joints[2] = 0.f;
+		EulOToQuat(joints, EULER_ORDER_XYZ, quat);
+		break;
+	case 3:	// X+Y
+		joints[2] = 0.f;
+		EulOToQuat(joints, EULER_ORDER_ZYX, quat);
+		break;
+	case 4:	// Z only
+		joints[0] = joints[1] = 0.f;
+		EulOToQuat(joints, EULER_ORDER_XYZ, quat);
+		break;
+	case 5:	// X+Z
+		// X and Z are components of an equivalent rotation axis
+		joints[1] = 0;
+		VecRotToQuat(joints, VecLength(joints), quat);
+		break;
+	case 6:	// Y+Z
+		joints[0] = 0.f;
+		EulOToQuat(joints, EULER_ORDER_XYZ, quat);
+		break;
+	case 7: // X+Y+Z
+		// equivalent axis
+		VecRotToQuat(joints, VecLength(joints), quat);
+		break;
+	}
+	if (pchan->rotmode > 0) {
+		QuatToEulO(quat, joints, pchan->rotmode);
+		VecCopyf(pchan->eul, joints);
+	} else
+		QuatCopy(pchan->quat, quat);
+	return PY_SET_ATTR_SUCCESS;
+}
+
+// *************************
+// BL_ArmatureBone
+//
+// Access to Bone structure
+PyTypeObject BL_ArmatureBone::Type = {
+	PyVarObject_HEAD_INIT(NULL, 0)
+	"BL_ArmatureBone",
+	sizeof(PyObjectPlus_Proxy),
+	0,
+	py_base_dealloc,
+	0,
+	0,
+	0,
+	0,
+	py_bone_repr,
+	0,0,0,0,0,0,0,0,0,
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+	0,0,0,0,0,0,0,
+	Methods,
+	0,
+	0,
+	&CValue::Type,
+	0,0,0,0,0,0,
+	py_base_new
+};
+
+// not used since this class is never instantiated
+PyObject *BL_ArmatureBone::GetProxy() 
+{ 
+	return NULL; 
+}
+PyObject *BL_ArmatureBone::NewProxy(bool py_owns) 
+{ 
+	return NULL; 
+}
+
+PyObject *BL_ArmatureBone::py_bone_repr(PyObject *self)
+{
+	Bone* bone = static_cast<Bone*>BGE_PROXY_PTR(self);
+	return PyUnicode_FromString(bone->name);
+}
+
+PyMethodDef BL_ArmatureBone::Methods[] = {
+	{NULL,NULL} //Sentinel
+};
+
+/* no attributes on C++ class since it is never instantiated */
+PyAttributeDef BL_ArmatureBone::Attributes[] = {
+	{ NULL }	//Sentinel
+};
+
+// attributes that work on proxy ptr (points to a Bone structure)
+PyAttributeDef BL_ArmatureBone::AttributesPtr[] = {
+	KX_PYATTRIBUTE_CHAR_RO("name",Bone,name),
+	KX_PYATTRIBUTE_FLAG_RO("connected",Bone,flag, BONE_CONNECTED),
+	KX_PYATTRIBUTE_FLAG_RO("hinge",Bone,flag, BONE_HINGE),
+	KX_PYATTRIBUTE_FLAG_NEGATIVE_RO("inherit_scale",Bone,flag, BONE_NO_SCALE),
+	KX_PYATTRIBUTE_SHORT_RO("bbone_segments",Bone,segments),
+	KX_PYATTRIBUTE_FLOAT_RO("roll",Bone,roll),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("head",Bone,head,3),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("tail",Bone,tail,3),
+	KX_PYATTRIBUTE_FLOAT_RO("length",Bone,length),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("arm_head",Bone,arm_head,3),
+	KX_PYATTRIBUTE_FLOAT_VECTOR_RO("arm_tail",Bone,arm_tail,3),
+	KX_PYATTRIBUTE_FLOAT_MATRIX_RO("arm_mat",Bone,arm_mat,4),
+	KX_PYATTRIBUTE_FLOAT_MATRIX_RO("bone_mat",Bone,bone_mat,4),
+	KX_PYATTRIBUTE_RO_FUNCTION("parent",BL_ArmatureBone,py_bone_get_parent),
+	KX_PYATTRIBUTE_RO_FUNCTION("children",BL_ArmatureBone,py_bone_get_parent),
+	{ NULL }	//Sentinel
+};
+
+PyObject *BL_ArmatureBone::py_bone_get_parent(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	Bone* bone = reinterpret_cast<Bone*>BGE_PROXY_PTR(self);
+	if (bone->parent) {
+		// create a proxy unconnected to any GE object
+		return NewProxyPlus_Ext(NULL,&Type,bone->parent,false);
+	}
+	Py_RETURN_NONE;
+}
+
+PyObject *BL_ArmatureBone::py_bone_get_children(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	Bone* bone = reinterpret_cast<Bone*>BGE_PROXY_PTR(self);
+	Bone* child;
+	int count = 0;
+	for (child=(Bone*)bone->childbase.first; child; child=(Bone*)child->next)
+		count++;
+
+	PyObject* childrenlist = PyList_New(count);
+
+	for (count = 0, child=(Bone*)bone->childbase.first; child; child=(Bone*)child->next, ++count)
+		PyList_SET_ITEM(childrenlist,count,NewProxyPlus_Ext(NULL,&Type,child,false));
+
+	return childrenlist;
+}
diff --git a/source/gameengine/Converter/BL_ArmatureChannel.h b/source/gameengine/Converter/BL_ArmatureChannel.h
new file mode 100644
index 00000000000..0f0f5a062e0
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureChannel.h
@@ -0,0 +1,91 @@
+/**
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+#ifndef __BL_ARMATURECHANNEL
+#define __BL_ARMATURECHANNEL
+
+#include "DNA_action_types.h"
+#include "GEN_HashedPtr.h"
+#include "GEN_Map.h"
+#include "PyObjectPlus.h"
+
+class SCA_IObject;
+class KX_GameObject;
+class BL_ArmatureObject;
+struct bConstraint;
+struct bPoseChannel;
+struct Object;
+struct bPose;
+
+class BL_ArmatureChannel : public PyObjectPlus
+{
+	// use Py_HeaderPtr since we use generic pointer in proxy
+	Py_HeaderPtr;
+
+private:
+	friend class BL_ArmatureObject;
+	struct bPoseChannel* m_posechannel;
+	BL_ArmatureObject* m_armature;
+
+public:
+	BL_ArmatureChannel(class BL_ArmatureObject *armature, 
+						struct bPoseChannel *posechannel);
+	virtual ~BL_ArmatureChannel();
+
+	virtual PyObject* py_repr(void);
+
+	// Python access
+	static PyObject* py_attr_getattr(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static int py_attr_setattr(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
+	static PyObject* py_attr_get_joint_rotation(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static int py_attr_set_joint_rotation(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
+
+};
+
+/* this is a factory class to access bBone data field in the GE.
+   It's not supposed to be instantiated, we only need it for the Attributes and Method array.
+   The actual proxy object will be manually created using NewProxyPtr */
+class BL_ArmatureBone : public PyObjectPlus
+{
+	// use Py_HeaderPtr since we use generic pointer in proxy
+	Py_HeaderPtr;
+private:
+	// make constructor private to make sure no one tries to instantiate this class
+	BL_ArmatureBone() {}
+	virtual ~BL_ArmatureBone() {}
+
+public:
+	static PyObject *py_bone_repr(PyObject *self);
+	static PyObject *py_bone_get_parent(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static PyObject *py_bone_get_children(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+
+};
+
+
+#endif //__BL_ARMATURECHANNEL
+
diff --git a/source/gameengine/Converter/BL_ArmatureConstraint.cpp b/source/gameengine/Converter/BL_ArmatureConstraint.cpp
new file mode 100644
index 00000000000..15e12611483
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureConstraint.cpp
@@ -0,0 +1,447 @@
+/**
+ * $Id$
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "DNA_constraint_types.h"
+#include "DNA_action_types.h"
+#include "BL_ArmatureConstraint.h"
+#include "BL_ArmatureObject.h"
+#include "BLI_arithb.h"
+#include "BLI_string.h"
+
+PyTypeObject BL_ArmatureConstraint::Type = {
+	PyVarObject_HEAD_INIT(NULL, 0)
+	"BL_ArmatureConstraint",
+	sizeof(PyObjectPlus_Proxy),
+	0,
+	py_base_dealloc,
+	0,
+	0,
+	0,
+	0,
+	py_base_repr,
+	0,0,0,0,0,0,0,0,0,
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+	0,0,0,0,0,0,0,
+	Methods,
+	0,
+	0,
+	&CValue::Type,
+	0,0,0,0,0,0,
+	py_base_new
+};
+
+PyObject* BL_ArmatureConstraint::py_repr(void)
+{
+	return PyUnicode_FromString(m_name);
+}
+
+BL_ArmatureConstraint::BL_ArmatureConstraint(
+	BL_ArmatureObject *armature, 
+	bPoseChannel *posechannel,
+	bConstraint *constraint, 
+	KX_GameObject* target,
+	KX_GameObject* subtarget)
+	: PyObjectPlus(), m_armature(armature), m_constraint(constraint), m_posechannel(posechannel)
+{
+	m_target = target;
+	m_blendtarget = (target) ? target->GetBlenderObject() : NULL;
+	m_subtarget = subtarget;
+	m_blendsubtarget = (subtarget) ? subtarget->GetBlenderObject() : NULL;
+	m_pose = m_subpose = NULL;
+	if (m_blendtarget) {
+		Mat4CpyMat4(m_blendmat, m_blendtarget->obmat);
+		if (m_blendtarget->type == OB_ARMATURE)
+			m_pose = m_blendtarget->pose;
+	}
+	if (m_blendsubtarget) {
+		Mat4CpyMat4(m_blendsubmat, m_blendsubtarget->obmat);
+		if (m_blendsubtarget->type == OB_ARMATURE)
+			m_subpose = m_blendsubtarget->pose;
+	}
+	if (m_target)
+		m_target->RegisterObject(m_armature);
+	if (m_subtarget)
+		m_subtarget->RegisterObject(m_armature);
+	BLI_snprintf(m_name, sizeof(m_name), "%s:%s", m_posechannel->name, m_constraint->name);
+}
+
+BL_ArmatureConstraint::~BL_ArmatureConstraint()
+{
+	if (m_target)
+		m_target->UnregisterObject(m_armature);
+	if (m_subtarget)
+		m_subtarget->UnregisterObject(m_armature);
+}
+
+BL_ArmatureConstraint* BL_ArmatureConstraint::GetReplica() const
+{
+	BL_ArmatureConstraint* replica = new BL_ArmatureConstraint(*this);
+	replica->ProcessReplica();
+	return replica;
+}
+
+void BL_ArmatureConstraint::ReParent(BL_ArmatureObject* armature)
+{
+	m_armature = armature;
+	if (m_target)
+		m_target->RegisterObject(armature);
+	if (m_subtarget)
+		m_subtarget->RegisterObject(armature);
+	// find the corresponding constraint in the new armature object
+	if (m_constraint) {
+		bPose* newpose = armature->GetOrigPose();
+		char* constraint = m_constraint->name;
+		char* posechannel = m_posechannel->name;
+		bPoseChannel* pchan;
+		bConstraint* pcon;
+		m_constraint = NULL;
+		m_posechannel = NULL;
+		// and locate the constraint
+		for (pchan = (bPoseChannel*)newpose->chanbase.first; pchan; pchan=(bPoseChannel*)pchan->next) {
+			if (!strcmp(pchan->name, posechannel)) {
+				// now locate the constraint
+				for (pcon = (bConstraint*)pchan->constraints.first; pcon; pcon=(bConstraint*)pcon->next) {
+					if (!strcmp(pcon->name, constraint)) {
+						m_constraint = pcon;
+						m_posechannel = pchan;
+						break;
+					}
+				}
+				break;
+			}
+		}
+	}
+}
+
+void BL_ArmatureConstraint::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
+{
+	void **h_obj = (*obj_map)[m_target];
+	if (h_obj) {
+		m_target->UnregisterObject(m_armature);
+		m_target = (KX_GameObject*)(*h_obj);
+		m_target->RegisterObject(m_armature);
+	}
+	h_obj = (*obj_map)[m_subtarget];
+	if (h_obj) {
+		m_subtarget->UnregisterObject(m_armature);
+		m_subtarget = (KX_GameObject*)(*h_obj);
+		m_subtarget->RegisterObject(m_armature);
+	}
+}
+
+bool BL_ArmatureConstraint::UnlinkObject(SCA_IObject* clientobj)
+{
+	bool res=false;
+	if (clientobj == m_target) {
+		m_target = NULL;
+		res = true;
+	}
+	if (clientobj == m_subtarget) {
+		m_subtarget = NULL;
+		res = true;
+	}
+	return res;
+}
+
+void BL_ArmatureConstraint::UpdateTarget()
+{
+	if (m_constraint && !(m_constraint->flag&CONSTRAINT_OFF) && (!m_blendtarget || m_target)) {
+		if (m_blendtarget) {
+			// external target, must be updated
+			m_target->UpdateBlenderObjectMatrix(m_blendtarget);
+			if (m_pose && m_target->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE)
+				// update the pose in case a bone is specified in the constraint target
+				m_blendtarget->pose = ((BL_ArmatureObject*)m_target)->GetOrigPose();
+		}
+		if (m_blendsubtarget && m_subtarget) {
+			m_subtarget->UpdateBlenderObjectMatrix(m_blendsubtarget);
+			if (m_subpose && m_subtarget->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE)
+				m_blendsubtarget->pose = ((BL_ArmatureObject*)m_target)->GetOrigPose();
+		}
+	}
+}
+
+void BL_ArmatureConstraint::RestoreTarget()
+{
+	if (m_constraint && !(m_constraint->flag&CONSTRAINT_OFF) && (!m_blendtarget || m_target)) {
+		if (m_blendtarget) {
+			Mat4CpyMat4(m_blendtarget->obmat, m_blendmat);
+			if (m_pose)
+				m_blendtarget->pose = m_pose;
+		}
+		if (m_blendsubtarget && m_subtarget) {
+			Mat4CpyMat4(m_blendsubtarget->obmat, m_blendsubmat);
+			if (m_subpose)
+				m_blendsubtarget->pose = m_subpose;
+		}
+	}
+}
+
+bool BL_ArmatureConstraint::Match(const char* posechannel, const char* constraint)
+{
+	return (!strcmp(m_posechannel->name, posechannel) && !strcmp(m_constraint->name, constraint));
+}
+
+void BL_ArmatureConstraint::SetTarget(KX_GameObject* target)
+{
+	if (m_blendtarget) {
+		if (target != m_target) {
+			m_target->UnregisterObject(m_armature);
+			m_target = target;
+			if (m_target)
+				m_target->RegisterObject(m_armature);
+		}
+	}
+
+}
+
+void BL_ArmatureConstraint::SetSubtarget(KX_GameObject* subtarget)
+{
+	if (m_blendsubtarget) {
+		if (subtarget != m_subtarget) {
+			m_subtarget->UnregisterObject(m_armature);
+			m_subtarget = subtarget;
+			if (m_subtarget)
+				m_subtarget->RegisterObject(m_armature);
+		}
+	}
+
+}
+
+// PYTHON
+
+PyMethodDef BL_ArmatureConstraint::Methods[] = {
+  {NULL,NULL} //Sentinel
+};
+
+// order of definition of attributes, must match Attributes[] array
+#define BCA_TYPE		0
+#define BCA_NAME		1
+#define BCA_ENFORCE		2
+#define BCA_HEADTAIL	3
+#define BCA_LINERROR	4
+#define BCA_ROTERROR	5
+#define BCA_TARGET		6
+#define BCA_SUBTARGET	7
+#define BCA_ACTIVE		8
+#define BCA_IKWEIGHT	9
+#define BCA_IKTYPE		10
+#define BCA_IKFLAG		11
+#define BCA_IKDIST		12
+#define BCA_IKMODE		13
+
+PyAttributeDef BL_ArmatureConstraint::Attributes[] = {
+	// Keep these attributes in order of BCA_ defines!!! used by py_attr_getattr and py_attr_setattr
+	KX_PYATTRIBUTE_RO_FUNCTION("type",BL_ArmatureConstraint,py_attr_getattr),	
+	KX_PYATTRIBUTE_RO_FUNCTION("name",BL_ArmatureConstraint,py_attr_getattr),	
+	KX_PYATTRIBUTE_RW_FUNCTION("enforce",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("headtail",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RO_FUNCTION("lin_error",BL_ArmatureConstraint,py_attr_getattr),
+	KX_PYATTRIBUTE_RO_FUNCTION("rot_error",BL_ArmatureConstraint,py_attr_getattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("target",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("subtarget",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("active",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("ik_weight",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RO_FUNCTION("ik_type",BL_ArmatureConstraint,py_attr_getattr),
+	KX_PYATTRIBUTE_RO_FUNCTION("ik_flag",BL_ArmatureConstraint,py_attr_getattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("ik_dist",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	KX_PYATTRIBUTE_RW_FUNCTION("ik_mode",BL_ArmatureConstraint,py_attr_getattr,py_attr_setattr),
+	
+	{ NULL }	//Sentinel
+};
+
+
+PyObject* BL_ArmatureConstraint::py_attr_getattr(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureConstraint* self= static_cast<BL_ArmatureConstraint*>(self_v);
+	bConstraint* constraint = self->m_constraint;
+	bKinematicConstraint* ikconstraint = (constraint && constraint->type == CONSTRAINT_TYPE_KINEMATIC) ? (bKinematicConstraint*)constraint->data : NULL;
+	int attr_order = attrdef-Attributes;
+
+	if (!constraint) {
+		PyErr_SetString(PyExc_AttributeError, "constraint is NULL");
+		return NULL;
+	}
+
+	switch (attr_order) {
+	case BCA_TYPE:
+		return PyLong_FromLong(constraint->type);
+	case BCA_NAME:
+		return PyUnicode_FromString(constraint->name);
+	case BCA_ENFORCE:
+		return PyFloat_FromDouble(constraint->enforce);
+	case BCA_HEADTAIL:
+		return PyFloat_FromDouble(constraint->headtail);
+	case BCA_LINERROR:
+		return PyFloat_FromDouble(constraint->lin_error);
+	case BCA_ROTERROR:
+		return PyFloat_FromDouble(constraint->rot_error);
+	case BCA_TARGET:
+		if (!self->m_target)	
+			Py_RETURN_NONE;
+		else
+			return self->m_target->GetProxy();
+	case BCA_SUBTARGET:
+		if (!self->m_subtarget)	
+			Py_RETURN_NONE;
+		else
+			return self->m_subtarget->GetProxy();
+	case BCA_ACTIVE:
+		return PyBool_FromLong(constraint->flag & CONSTRAINT_OFF);
+	case BCA_IKWEIGHT:
+	case BCA_IKTYPE:
+	case BCA_IKFLAG:
+	case BCA_IKDIST:
+	case BCA_IKMODE:
+		if (!ikconstraint) {
+			PyErr_SetString(PyExc_AttributeError, "constraint is not of IK type");
+			return NULL;
+		}
+		switch (attr_order) {
+		case BCA_IKWEIGHT:
+			return PyFloat_FromDouble((ikconstraint)?ikconstraint->weight:0.0);
+		case BCA_IKTYPE:
+			return PyLong_FromLong(ikconstraint->type);
+		case BCA_IKFLAG:
+			return PyLong_FromLong(ikconstraint->flag);
+		case BCA_IKDIST:
+			return PyFloat_FromDouble(ikconstraint->dist);
+		case BCA_IKMODE:
+			return PyLong_FromLong(ikconstraint->mode);
+		}
+		// should not come here
+		break;
+	}
+	PyErr_SetString(PyExc_AttributeError, "constraint unknown attribute");
+	return NULL;
+}
+
+int BL_ArmatureConstraint::py_attr_setattr(void *self_v, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
+{
+	BL_ArmatureConstraint* self= static_cast<BL_ArmatureConstraint*>(self_v);
+	bConstraint* constraint = self->m_constraint;
+	bKinematicConstraint* ikconstraint = (constraint && constraint->type == CONSTRAINT_TYPE_KINEMATIC) ? (bKinematicConstraint*)constraint->data : NULL;
+	int attr_order = attrdef-Attributes;
+	int ival;
+	double dval;
+	char* sval;
+	KX_GameObject *oval;
+
+	if (!constraint) {
+		PyErr_SetString(PyExc_AttributeError, "constraint is NULL");
+		return PY_SET_ATTR_FAIL;
+	}
+	
+	switch (attr_order) {
+	case BCA_ENFORCE:
+		dval = PyFloat_AsDouble(value);
+		if (dval < 0.0f || dval > 1.0f) { /* also accounts for non float */
+			PyErr_SetString(PyExc_AttributeError, "constraint.enforce = float: BL_ArmatureConstraint, expected a float between 0 and 1");
+			return PY_SET_ATTR_FAIL;
+		}
+		constraint->enforce = dval;
+		return PY_SET_ATTR_SUCCESS;
+
+	case BCA_HEADTAIL:
+		dval = PyFloat_AsDouble(value);
+		if (dval < 0.0f || dval > 1.0f) { /* also accounts for non float */
+			PyErr_SetString(PyExc_AttributeError, "constraint.headtail = float: BL_ArmatureConstraint, expected a float between 0 and 1");
+			return PY_SET_ATTR_FAIL;
+		}
+		constraint->headtail = dval;
+		return PY_SET_ATTR_SUCCESS;
+
+	case BCA_TARGET:
+		if (!ConvertPythonToGameObject(value, &oval, true, "constraint.target = value: BL_ArmatureConstraint"))
+			return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
+		self->SetTarget(oval);
+		return PY_SET_ATTR_SUCCESS;
+
+	case BCA_SUBTARGET:
+		if (!ConvertPythonToGameObject(value, &oval, true, "constraint.subtarget = value: BL_ArmatureConstraint"))
+			return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
+		self->SetSubtarget(oval);
+		return PY_SET_ATTR_SUCCESS;
+
+	case BCA_ACTIVE:
+		ival = PyObject_IsTrue( value );
+		if (ival == -1) {
+			PyErr_SetString(PyExc_AttributeError, "constraint.active = bool: BL_ArmatureConstraint, expected True or False");
+			return PY_SET_ATTR_FAIL;
+		}
+		self->m_constraint->flag = (self->m_constraint->flag & ~CONSTRAINT_OFF) | ((ival)?0:CONSTRAINT_OFF);
+		return PY_SET_ATTR_SUCCESS;
+
+	case BCA_IKWEIGHT:
+	case BCA_IKDIST:
+	case BCA_IKMODE:
+		if (!ikconstraint) {
+			PyErr_SetString(PyExc_AttributeError, "constraint is not of IK type");
+			return PY_SET_ATTR_FAIL;
+		}
+		switch (attr_order) {
+		case BCA_IKWEIGHT:
+			dval = PyFloat_AsDouble(value);
+			if (dval < 0.0f || dval > 1.0f) { /* also accounts for non float */
+				PyErr_SetString(PyExc_AttributeError, "constraint.weight = float: BL_ArmatureConstraint, expected a float between 0 and 1");
+				return PY_SET_ATTR_FAIL;
+			}
+			ikconstraint->weight = dval;
+			return PY_SET_ATTR_SUCCESS;
+
+		case BCA_IKDIST:
+			dval = PyFloat_AsDouble(value);
+			if (dval < 0.0f) { /* also accounts for non float */
+				PyErr_SetString(PyExc_AttributeError, "constraint.ik_dist = float: BL_ArmatureConstraint, expected a positive float");
+				return PY_SET_ATTR_FAIL;
+			}
+			ikconstraint->dist = dval;
+			return PY_SET_ATTR_SUCCESS;
+
+		case BCA_IKMODE:
+			ival = PyLong_AsLong(value);
+			if (ival < 0) {
+				PyErr_SetString(PyExc_AttributeError, "constraint.ik_mode = integer: BL_ArmatureConstraint, expected a positive integer");
+				return PY_SET_ATTR_FAIL;
+			}
+			ikconstraint->mode = ival;
+			return PY_SET_ATTR_SUCCESS;
+		}
+		// should not come here
+		break;
+
+	}
+
+	PyErr_SetString(PyExc_AttributeError, "constraint unknown attribute");
+	return PY_SET_ATTR_FAIL;
+}
+
diff --git a/source/gameengine/Converter/BL_ArmatureConstraint.h b/source/gameengine/Converter/BL_ArmatureConstraint.h
new file mode 100644
index 00000000000..f10849ea05b
--- /dev/null
+++ b/source/gameengine/Converter/BL_ArmatureConstraint.h
@@ -0,0 +1,117 @@
+/**
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+#ifndef __BL_ARMATURECONSTRAINT
+#define __BL_ARMATURECONSTRAINT
+
+#include "DNA_constraint_types.h"
+#include "GEN_HashedPtr.h"
+#include "GEN_Map.h"
+#include "PyObjectPlus.h"
+
+class SCA_IObject;
+class KX_GameObject;
+class BL_ArmatureObject;
+struct bConstraint;
+struct bPoseChannel;
+struct Object;
+struct bPose;
+
+/**
+ * SG_DList : element of controlled constraint list 
+ *            head = BL_ArmatureObject::m_controlledConstraints
+ * SG_QList : not used
+ */
+class BL_ArmatureConstraint	: public PyObjectPlus
+{
+	Py_Header;
+
+private:
+	struct bConstraint* m_constraint;
+	struct bPoseChannel* m_posechannel;
+	class BL_ArmatureObject* m_armature;
+	char m_name[64];
+	KX_GameObject*  m_target;
+	KX_GameObject*  m_subtarget;
+	struct Object*	m_blendtarget;
+	struct Object*	m_blendsubtarget;
+	float		m_blendmat[4][4];
+	float		m_blendsubmat[4][4];
+	struct bPose*	m_pose;
+	struct bPose*	m_subpose;
+
+public:
+	BL_ArmatureConstraint(class BL_ArmatureObject *armature, 
+						struct bPoseChannel *posechannel, 
+						struct bConstraint *constraint, 
+						KX_GameObject* target,
+						KX_GameObject* subtarget);
+	virtual ~BL_ArmatureConstraint();
+
+
+	virtual PyObject* py_repr(void);
+
+	BL_ArmatureConstraint* GetReplica() const;
+	void ReParent(BL_ArmatureObject* armature);
+	void Relink(GEN_Map<GEN_HashedPtr, void*> *map);
+	bool UnlinkObject(SCA_IObject* clientobj);
+
+	void UpdateTarget();
+	void RestoreTarget();
+
+	bool Match(const char* posechannel, const char* constraint);
+	const char* GetName() { return m_name; }
+
+	void SetConstraintFlag(int flag)
+	{
+		if (m_constraint)
+			m_constraint->flag |= flag;
+	}
+	void ClrConstraintFlag(int flag)
+	{
+		if (m_constraint)
+			m_constraint->flag &= ~flag;
+	}
+	void SetWeight(float weight)
+	{
+		if (m_constraint && m_constraint->type == CONSTRAINT_TYPE_KINEMATIC && m_constraint->data) {
+			bKinematicConstraint* con = (bKinematicConstraint*)m_constraint->data;
+			con->weight = weight;
+		}
+	}
+	void SetTarget(KX_GameObject* target);
+	void SetSubtarget(KX_GameObject* subtarget);
+
+	// Python access
+	static PyObject* py_attr_getattr(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+	static int py_attr_setattr(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value);
+
+};
+
+#endif //__BL_ARMATURECONSTRAINT
+
diff --git a/source/gameengine/Converter/BL_ArmatureObject.cpp b/source/gameengine/Converter/BL_ArmatureObject.cpp
index cfd90813a16..d5660d9ef84 100644
--- a/source/gameengine/Converter/BL_ArmatureObject.cpp
+++ b/source/gameengine/Converter/BL_ArmatureObject.cpp
@@ -29,9 +29,15 @@
 
 #include "BL_ArmatureObject.h"
 #include "BL_ActionActuator.h"
+#include "KX_BlenderSceneConverter.h"
 #include "BLI_blenlib.h"
+#include "BLI_ghash.h"
+#include "BLI_arithb.h"
+#include "BIK_api.h"
 #include "BKE_action.h"
 #include "BKE_armature.h"
+#include "BKE_utildefines.h"
+#include "BKE_constraint.h"
 #include "GEN_Map.h"
 #include "GEN_HashedPtr.h"
 #include "MEM_guardedalloc.h"
@@ -39,6 +45,11 @@
 #include "DNA_armature_types.h"
 #include "DNA_object_types.h"
 #include "DNA_scene_types.h"
+#include "DNA_nla_types.h"
+#include "DNA_constraint_types.h"
+#include "KX_PythonSeq.h"
+#include "KX_PythonInit.h"
+#include "KX_KetsjiEngine.h"
 
 #include "MT_Matrix4x4.h"
 
@@ -46,6 +57,153 @@
 #include <config.h>
 #endif
 
+/** 
+ * Move here pose function for game engine so that we can mix with GE objects
+ * Principle is as follow:
+ * Use Blender structures so that where_is_pose can be used unchanged
+ * Copy the constraint so that they can be enabled/disabled/added/removed at runtime
+ * Don't copy the constraints for the pose used by the Action actuator, it does not need them.
+ * Scan the constraint structures so that the KX equivalent of target objects are identified and 
+ * stored in separate list.
+ * When it is about to evaluate the pose, set the KX object position in the obmat of the corresponding
+ * Blender objects and restore after the evaluation.
+ */
+void game_copy_pose(bPose **dst, bPose *src, int copy_constraint)
+{
+	bPose *out;
+	bPoseChannel *pchan, *outpchan;
+	GHash *ghash;
+	
+	/* the game engine copies the current armature pose and then swaps
+	 * the object pose pointer. this makes it possible to change poses
+	 * without affecting the original blender data. */
+
+	if (!src) {
+		*dst=NULL;
+		return;
+	}
+	else if (*dst==src) {
+		printf("copy_pose source and target are the same\n");
+		*dst=NULL;
+		return;
+	}
+	
+	out= (bPose*)MEM_dupallocN(src);
+	out->agroups.first= out->agroups.last= NULL;
+	out->ikdata = NULL;
+	out->ikparam = MEM_dupallocN(out->ikparam);
+	out->flag |= POSE_GAME_ENGINE;
+	BLI_duplicatelist(&out->chanbase, &src->chanbase);
+
+	/* remap pointers */
+	ghash= BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp);
+
+	pchan= (bPoseChannel*)src->chanbase.first;
+	outpchan= (bPoseChannel*)out->chanbase.first;
+	for (; pchan; pchan=pchan->next, outpchan=outpchan->next)
+		BLI_ghash_insert(ghash, pchan, outpchan);
+
+	for (pchan=(bPoseChannel*)out->chanbase.first; pchan; pchan=(bPoseChannel*)pchan->next) {
+		pchan->parent= (bPoseChannel*)BLI_ghash_lookup(ghash, pchan->parent);
+		pchan->child= (bPoseChannel*)BLI_ghash_lookup(ghash, pchan->child);
+		pchan->path= NULL;
+
+		if (copy_constraint) {
+			ListBase listb;
+			// copy all constraint for backward compatibility
+			copy_constraints(&listb, &pchan->constraints);  // copy_constraints NULLs listb
+			pchan->constraints= listb;
+		} else {
+			pchan->constraints.first = NULL;
+			pchan->constraints.last = NULL;
+		}
+	}
+
+	BLI_ghash_free(ghash, NULL, NULL);
+	
+	*dst=out;
+}
+
+
+
+/* Only allowed for Poses with identical channels */
+void game_blend_poses(bPose *dst, bPose *src, float srcweight/*, short mode*/)
+{
+	short mode= ACTSTRIPMODE_BLEND;
+	
+	bPoseChannel *dchan;
+	const bPoseChannel *schan;
+	bConstraint *dcon, *scon;
+	float dstweight;
+	int i;
+
+	switch (mode){
+	case ACTSTRIPMODE_BLEND:
+		dstweight = 1.0F - srcweight;
+		break;
+	case ACTSTRIPMODE_ADD:
+		dstweight = 1.0F;
+		break;
+	default :
+		dstweight = 1.0F;
+	}
+	
+	schan= (bPoseChannel*)src->chanbase.first;
+	for (dchan = (bPoseChannel*)dst->chanbase.first; dchan; dchan=(bPoseChannel*)dchan->next, schan= (bPoseChannel*)schan->next){
+		// always blend on all channels since we don't know which one has been set
+		/* quat interpolation done separate */
+		if (schan->rotmode == PCHAN_ROT_QUAT) {
+			float dquat[4], squat[4];
+			
+			QUATCOPY(dquat, dchan->quat);
+			QUATCOPY(squat, schan->quat);
+			if (mode==ACTSTRIPMODE_BLEND)
+				QuatInterpol(dchan->quat, dquat, squat, srcweight);
+			else {
+				QuatMulFac(squat, srcweight);
+				QuatMul(dchan->quat, dquat, squat);
+			}
+			
+			NormalQuat(dchan->quat);
+		}
+
+		for (i=0; i<3; i++) {
+			/* blending for loc and scale are pretty self-explanatory... */
+			dchan->loc[i] = (dchan->loc[i]*dstweight) + (schan->loc[i]*srcweight);
+			dchan->size[i] = 1.0f + ((dchan->size[i]-1.0f)*dstweight) + ((schan->size[i]-1.0f)*srcweight);
+			
+			/* euler-rotation interpolation done here instead... */
+			// FIXME: are these results decent?
+			if (schan->rotmode)
+				dchan->eul[i] = (dchan->eul[i]*dstweight) + (schan->eul[i]*srcweight);
+		}
+		for(dcon= (bConstraint*)dchan->constraints.first, scon= (bConstraint*)schan->constraints.first; dcon && scon; dcon= (bConstraint*)dcon->next, scon= (bConstraint*)scon->next) {
+			/* no 'add' option for constraint blending */
+			dcon->enforce= dcon->enforce*(1.0f-srcweight) + scon->enforce*srcweight;
+		}
+	}
+	
+	/* this pose is now in src time */
+	dst->ctime= src->ctime;
+}
+
+void game_free_pose(bPose *pose)
+{
+	if (pose) {
+		/* free pose-channels and constraints */
+		free_pose_channels(pose);
+		
+		/* free IK solver state */
+		BIK_clear_data(pose);
+
+		/* free IK solver param */
+		if (pose->ikparam)
+			MEM_freeN(pose->ikparam);
+
+		MEM_freeN(pose);
+	}
+}
+
 BL_ArmatureObject::BL_ArmatureObject(
 				void* sgReplicationInfo, 
 				SG_Callbacks callbacks, 
@@ -53,12 +211,17 @@ BL_ArmatureObject::BL_ArmatureObject(
 				Scene *scene)
 
 :	KX_GameObject(sgReplicationInfo,callbacks),
+	m_controlledConstraints(),
+	m_poseChannels(),
 	m_objArma(armature),
 	m_framePose(NULL),
 	m_scene(scene), // maybe remove later. needed for where_is_pose
 	m_lastframe(0.0),
+	m_timestep(0.040),
 	m_activeAct(NULL),
 	m_activePriority(999),
+	m_constraintNumber(0),
+	m_channelNumber(0),
 	m_lastapplyframe(0.0)
 {
 	m_armature = (bArmature *)armature->data;
@@ -67,7 +230,177 @@ BL_ArmatureObject::BL_ArmatureObject(
 	 * the original pose before calling into blender functions, to deal with
 	 * replica's or other objects using the same blender object */
 	m_pose = NULL;
-	game_copy_pose(&m_pose, m_objArma->pose);
+	game_copy_pose(&m_pose, m_objArma->pose, 1);
+	// store the original armature object matrix
+	memcpy(m_obmat, m_objArma->obmat, sizeof(m_obmat));
+}
+
+BL_ArmatureObject::~BL_ArmatureObject()
+{
+	BL_ArmatureConstraint* constraint;
+	while ((constraint = m_controlledConstraints.Remove()) != NULL) {
+		delete constraint;
+	}
+	BL_ArmatureChannel* channel;
+	while ((channel = static_cast<BL_ArmatureChannel*>(m_poseChannels.Remove())) != NULL) {
+		delete channel;
+	}
+	if (m_pose)
+		game_free_pose(m_pose);
+	if (m_framePose)
+		game_free_pose(m_framePose);
+}
+
+
+void BL_ArmatureObject::LoadConstraints(KX_BlenderSceneConverter* converter)
+{
+	// first delete any existing constraint (should not have any)
+	while (!m_controlledConstraints.Empty()) {
+		BL_ArmatureConstraint* constraint = m_controlledConstraints.Remove();
+		delete constraint;
+	}
+	m_constraintNumber = 0;
+
+	// list all the constraint and convert them to BL_ArmatureConstraint
+	// get the persistent pose structure
+	bPoseChannel* pchan;
+	bConstraint* pcon;
+	bConstraintTypeInfo* cti;
+	Object* blendtarget;
+	KX_GameObject* gametarget;
+	KX_GameObject* gamesubtarget;
+
+	// and locate the constraint
+	for (pchan = (bPoseChannel*)m_pose->chanbase.first; pchan; pchan=(bPoseChannel*)pchan->next) {
+		for (pcon = (bConstraint*)pchan->constraints.first; pcon; pcon=(bConstraint*)pcon->next) {
+			if (pcon->flag & CONSTRAINT_DISABLE)
+				continue;
+			// which constraint should we support?
+			switch (pcon->type) {
+			case CONSTRAINT_TYPE_TRACKTO:
+			case CONSTRAINT_TYPE_KINEMATIC:
+			case CONSTRAINT_TYPE_ROTLIKE:
+			case CONSTRAINT_TYPE_LOCLIKE:
+			case CONSTRAINT_TYPE_MINMAX:
+			case CONSTRAINT_TYPE_SIZELIKE:
+			case CONSTRAINT_TYPE_LOCKTRACK:
+			case CONSTRAINT_TYPE_STRETCHTO:
+			case CONSTRAINT_TYPE_CLAMPTO:
+			case CONSTRAINT_TYPE_TRANSFORM:
+			case CONSTRAINT_TYPE_DISTLIMIT:
+				cti = constraint_get_typeinfo(pcon);
+				gametarget = gamesubtarget = NULL;
+				if (cti && cti->get_constraint_targets) {
+					ListBase listb = { NULL, NULL };
+					cti->get_constraint_targets(pcon, &listb);
+					if (listb.first) {
+						bConstraintTarget* target = (bConstraintTarget*)listb.first;
+						if (target->tar && target->tar != m_objArma) {
+							// only remember external objects, self target is handled automatically
+							blendtarget = target->tar;
+							gametarget = converter->FindGameObject(blendtarget);
+						}
+						if (target->next != NULL) {
+							// secondary target
+							target = (bConstraintTarget*)target->next;
+							if (target->tar && target->tar != m_objArma) {
+								// only track external object
+								blendtarget = target->tar;
+								gamesubtarget = converter->FindGameObject(blendtarget);
+							}
+						}
+					}
+					if (cti->flush_constraint_targets)
+						cti->flush_constraint_targets(pcon, &listb, 1);
+				}
+				BL_ArmatureConstraint* constraint = new BL_ArmatureConstraint(this, pchan, pcon, gametarget, gamesubtarget);
+				m_controlledConstraints.AddBack(constraint);
+				m_constraintNumber++;
+			}
+		}
+	}
+}
+
+BL_ArmatureConstraint* BL_ArmatureObject::GetConstraint(const char* posechannel, const char* constraintname)
+{
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end(); ++cit) {
+		BL_ArmatureConstraint* constraint = *cit;
+		if (constraint->Match(posechannel, constraintname))
+			return constraint;
+	}
+	return NULL;
+}
+
+BL_ArmatureConstraint* BL_ArmatureObject::GetConstraint(const char* posechannelconstraint)
+{
+	// performance: use hash string instead of plain string compare
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end(); ++cit) {
+		BL_ArmatureConstraint* constraint = *cit;
+		if (!strcmp(constraint->GetName(), posechannelconstraint))
+			return constraint;
+	}
+	return NULL;
+}
+
+BL_ArmatureConstraint* BL_ArmatureObject::GetConstraint(int index)
+{
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end() && index; ++cit, --index);
+	return (cit.end()) ? NULL : *cit;
+}
+
+/* this function is called to populate the m_poseChannels list */
+void BL_ArmatureObject::LoadChannels()
+{
+	if (m_poseChannels.Empty()) {
+		bPoseChannel* pchan;
+		BL_ArmatureChannel* proxy;
+	
+		m_channelNumber = 0;
+		for (pchan = (bPoseChannel*)m_pose->chanbase.first; pchan; pchan=(bPoseChannel*)pchan->next) {
+			proxy = new BL_ArmatureChannel(this, pchan);
+			m_poseChannels.AddBack(proxy);
+			m_channelNumber++;
+		}
+	}
+}
+
+BL_ArmatureChannel* BL_ArmatureObject::GetChannel(bPoseChannel* pchan)
+{
+	LoadChannels();
+	SG_DList::iterator<BL_ArmatureChannel> cit(m_poseChannels);
+	for (cit.begin(); !cit.end(); ++cit) 
+	{
+		BL_ArmatureChannel* channel = *cit;
+		if (channel->m_posechannel == pchan)
+			return channel;
+	}
+	return NULL;
+}
+
+BL_ArmatureChannel* BL_ArmatureObject::GetChannel(const char* str)
+{
+	LoadChannels();
+	SG_DList::iterator<BL_ArmatureChannel> cit(m_poseChannels);
+	for (cit.begin(); !cit.end(); ++cit) 
+	{
+		BL_ArmatureChannel* channel = *cit;
+		if (!strcmp(channel->m_posechannel->name, str))
+			return channel;
+	}
+	return NULL;
+}
+
+BL_ArmatureChannel* BL_ArmatureObject::GetChannel(int index)
+{
+	LoadChannels();
+	if (index < 0 || index >= m_channelNumber)
+		return NULL;
+	SG_DList::iterator<BL_ArmatureChannel> cit(m_poseChannels);
+	for (cit.begin(); !cit.end() && index; ++cit, --index);
+	return (cit.end()) ? NULL : *cit;
 }
 
 CValue* BL_ArmatureObject::GetReplica()
@@ -83,22 +416,61 @@ void BL_ArmatureObject::ProcessReplica()
 	KX_GameObject::ProcessReplica();
 
 	m_pose = NULL;
-	game_copy_pose(&m_pose, pose);
+	m_framePose = NULL;
+	game_copy_pose(&m_pose, pose, 1);	
 }
 
-BL_ArmatureObject::~BL_ArmatureObject()
+void BL_ArmatureObject::ReParentLogic()
 {
-	if (m_pose)
-		game_free_pose(m_pose);
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end(); ++cit) {
+		(*cit)->ReParent(this);
+	}
+	KX_GameObject::ReParentLogic();
+}
+
+void BL_ArmatureObject::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
+{
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end(); ++cit) {
+		(*cit)->Relink(obj_map);
+	}
+	KX_GameObject::Relink(obj_map);
+}
+
+bool BL_ArmatureObject::UnlinkObject(SCA_IObject* clientobj)
+{
+	// clientobj is being deleted, make sure we don't hold any reference to it
+	bool res = false;
+	SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+	for (cit.begin(); !cit.end(); ++cit) {
+		res |= (*cit)->UnlinkObject(clientobj);
+	}
+	return res;
 }
 
 void BL_ArmatureObject::ApplyPose()
 {
 	m_armpose = m_objArma->pose;
 	m_objArma->pose = m_pose;
+	// in the GE, we use ctime to store the timestep
+	m_pose->ctime = (float)m_timestep;
 	//m_scene->r.cfra++;
 	if(m_lastapplyframe != m_lastframe) {
+		// update the constraint if any, first put them all off so that only the active ones will be updated
+		SG_DList::iterator<BL_ArmatureConstraint> cit(m_controlledConstraints);
+		for (cit.begin(); !cit.end(); ++cit) {
+			(*cit)->UpdateTarget();
+		}
+		// update ourself
+		UpdateBlenderObjectMatrix(m_objArma);
 		where_is_pose(m_scene, m_objArma); // XXX
+		// restore ourself
+		memcpy(m_objArma->obmat, m_obmat, sizeof(m_obmat));
+		// restore active targets
+		for (cit.begin(); !cit.end(); ++cit) {
+			(*cit)->RestoreTarget();
+		}
 		m_lastapplyframe = m_lastframe;
 	}
 }
@@ -119,30 +491,37 @@ bool BL_ArmatureObject::SetActiveAction(BL_ActionActuator *act, short priority,
 {
 	if (curtime != m_lastframe){
 		m_activePriority = 9999;
+		// compute the timestep for the underlying IK algorithm
+		m_timestep = curtime-m_lastframe;
 		m_lastframe= curtime;
 		m_activeAct = NULL;
 		// remember the pose at the start of the frame
-		m_framePose = m_pose;
+		GetPose(&m_framePose);
 	}
 
-	if (priority<=m_activePriority)
+	if (act) 
 	{
-		if (priority<m_activePriority)
-			// this action overwrites the previous ones, start from initial pose to cancel their effects
-			m_pose = m_framePose;
-		if (m_activeAct && (m_activeAct!=act))
-			m_activeAct->SetBlendTime(0.0);	/* Reset the blend timer */
-		m_activeAct = act;
-		m_activePriority = priority;
-		m_lastframe = curtime;
-	
-		return true;
-	}
-	else{
-		act->SetBlendTime(0.0);
-		return false;
+		if (priority<=m_activePriority)
+		{
+			if (priority<m_activePriority) {
+				// this action overwrites the previous ones, start from initial pose to cancel their effects
+				SetPose(m_framePose);
+				if (m_activeAct && (m_activeAct!=act))
+					/* Reset the blend timer since this new action cancels the old one */
+					m_activeAct->SetBlendTime(0.0);	
+			}
+			m_activeAct = act;
+			m_activePriority = priority;
+			m_lastframe = curtime;
+		
+			return true;
+		}
+		else{
+			act->SetBlendTime(0.0);
+			return false;
+		}
 	}
-	
+	return false;
 }
 
 BL_ActionActuator * BL_ArmatureObject::GetActiveAction()
@@ -161,7 +540,7 @@ void BL_ArmatureObject::GetPose(bPose **pose)
 			a crash and memory leakage when 
 			&BL_ActionActuator::m_pose is freed
 		*/
-		game_copy_pose(pose, m_pose);
+		game_copy_pose(pose, m_pose, 0);
 	}
 	else {
 		if (*pose == m_pose)
@@ -178,7 +557,7 @@ void BL_ArmatureObject::GetMRDPose(bPose **pose)
 	/* Otherwise, copy the armature's pose channels into the caller-supplied pose */
 
 	if (!*pose)
-		game_copy_pose(pose, m_pose);
+		game_copy_pose(pose, m_pose, 0);
 	else
 		extract_pose_from_pose(*pose, m_pose);
 }
@@ -210,3 +589,68 @@ float BL_ArmatureObject::GetBoneLength(Bone* bone) const
 {
 	return (float)(MT_Point3(bone->head) - MT_Point3(bone->tail)).length();
 }
+
+// PYTHON
+
+PyTypeObject BL_ArmatureObject::Type = {
+	PyVarObject_HEAD_INIT(NULL, 0)
+	"BL_ArmatureObject",
+	sizeof(PyObjectPlus_Proxy),
+	0,
+	py_base_dealloc,
+	0,
+	0,
+	0,
+	0,
+	py_base_repr,
+	0,
+	&KX_GameObject::Sequence,
+	&KX_GameObject::Mapping,
+	0,0,0,
+	NULL,
+	NULL,
+	0,
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+	0,0,0,0,0,0,0,
+	Methods,
+	0,
+	0,
+	&KX_GameObject::Type,
+	0,0,0,0,0,0,
+	py_base_new
+};
+
+PyMethodDef BL_ArmatureObject::Methods[] = {
+
+	KX_PYMETHODTABLE_NOARGS(BL_ArmatureObject, update),
+	{NULL,NULL} //Sentinel
+};
+
+PyAttributeDef BL_ArmatureObject::Attributes[] = {
+
+	KX_PYATTRIBUTE_RO_FUNCTION("constraints",		BL_ArmatureObject, pyattr_get_constraints),
+	KX_PYATTRIBUTE_RO_FUNCTION("channels",		BL_ArmatureObject, pyattr_get_channels),
+	{NULL} //Sentinel
+};
+
+PyObject* BL_ArmatureObject::pyattr_get_constraints(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef)
+{
+	return KX_PythonSeq_CreatePyObject((static_cast<BL_ArmatureObject*>(self_v))->m_proxy, KX_PYGENSEQ_OB_TYPE_CONSTRAINTS);
+}
+
+PyObject* BL_ArmatureObject::pyattr_get_channels(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef)
+{
+	BL_ArmatureObject* self = static_cast<BL_ArmatureObject*>(self_v);
+	self->LoadChannels(); // make sure we have the channels
+	return KX_PythonSeq_CreatePyObject((static_cast<BL_ArmatureObject*>(self_v))->m_proxy, KX_PYGENSEQ_OB_TYPE_CHANNELS);
+}
+
+KX_PYMETHODDEF_DOC_NOARGS(BL_ArmatureObject, update, 
+						  "update()\n"
+						  "Make sure that the armature will be updated on next graphic frame.\n"
+						  "This is automatically done if a KX_ArmatureActuator with mode run is active\n"
+						  "or if an action is playing. This function is usefull in other cases.\n")
+{
+	SetActiveAction(NULL, 0, KX_GetActiveEngine()->GetFrameTime());
+	Py_RETURN_NONE;
+}
diff --git a/source/gameengine/Converter/BL_ArmatureObject.h b/source/gameengine/Converter/BL_ArmatureObject.h
index af0b7dc201c..3e917e08001 100644
--- a/source/gameengine/Converter/BL_ArmatureObject.h
+++ b/source/gameengine/Converter/BL_ArmatureObject.h
@@ -31,21 +31,34 @@
 #define BL_ARMATUREOBJECT
 
 #include "KX_GameObject.h"
+#include "BL_ArmatureConstraint.h"
+#include "BL_ArmatureChannel.h"
 
 #include "SG_IObject.h"
+#include <vector>
+#include <algorithm>
 
 struct bArmature;
 struct Bone;
+struct bConstraint;
 class BL_ActionActuator;
+class BL_ArmatureActuator;
 class MT_Matrix4x4;
 struct Object;
+class KX_BlenderSceneConverter;
 
 class BL_ArmatureObject : public KX_GameObject  
 {
+	Py_Header;
 public:
+
 	double GetLastFrame ();
 	short GetActivePriority();
 	virtual void ProcessReplica();
+	virtual void ReParentLogic();
+	virtual void Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map);
+	virtual bool UnlinkObject(SCA_IObject* clientobj);
+
 	class BL_ActionActuator * GetActiveAction();
 	
 	BL_ArmatureObject(
@@ -73,6 +86,19 @@ public:
 	
 	Object* GetArmatureObject() {return m_objArma;}
 
+	// for constraint python API
+	void LoadConstraints(KX_BlenderSceneConverter* converter);
+	size_t GetConstraintNumber() const { return m_constraintNumber; }
+	BL_ArmatureConstraint* GetConstraint(const char* posechannel, const char* constraint);
+	BL_ArmatureConstraint* GetConstraint(const char* posechannelconstraint);
+	BL_ArmatureConstraint* GetConstraint(int index);
+	// for pose channel python API
+	void LoadChannels();
+	size_t GetChannelNumber() const { return m_constraintNumber; }
+	BL_ArmatureChannel* GetChannel(bPoseChannel* channel);
+	BL_ArmatureChannel* GetChannel(const char* channel);
+	BL_ArmatureChannel* GetChannel(int index);
+
 	/// Retrieve the pose matrix for the specified bone.
 	/// Returns true on success.
 	bool GetBoneMatrix(Bone* bone, MT_Matrix4x4& matrix);
@@ -81,7 +107,17 @@ public:
 	float GetBoneLength(Bone* bone) const;
 
 	virtual int GetGameObjectType() { return OBJ_ARMATURE; }
+
+	// PYTHON
+	static PyObject* pyattr_get_constraints(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef);
+	static PyObject* pyattr_get_channels(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef);
+	KX_PYMETHOD_DOC_NOARGS(BL_ArmatureObject, update);
+
 protected:
+	/* list element: BL_ArmatureConstraint. Use SG_DListHead to have automatic list replication */
+	SG_DListHead<BL_ArmatureConstraint>	 m_controlledConstraints;
+	/* list element: BL_ArmatureChannel. Use SG_DList to avoid list replication */
+	SG_DList			m_poseChannels;
 	Object				*m_objArma;
 	struct bArmature	*m_armature;
 	struct bPose		*m_pose;
@@ -89,8 +125,13 @@ protected:
 	struct bPose		*m_framePose;
 	struct Scene		*m_scene; // need for where_is_pose 
 	double	m_lastframe;
+	double  m_timestep;		// delta since last pose evaluation.
 	class BL_ActionActuator *m_activeAct;
 	short	m_activePriority;
+	size_t  m_constraintNumber;
+	size_t  m_channelNumber;
+	// store the original armature object matrix
+	float m_obmat[4][4];
 
 	double			m_lastapplyframe;
 
@@ -102,5 +143,12 @@ public:
 #endif
 };
 
+/* Pose function specific to the game engine */
+void game_blend_poses(struct bPose *dst, struct bPose *src, float srcweight/*, short mode*/); /* was blend_poses */
+//void extract_pose_from_pose(struct bPose *pose, const struct bPose *src);
+void game_copy_pose(struct bPose **dst, struct bPose *src, int copy_con);
+void game_free_pose(struct bPose *pose);
+
+
 #endif
 
diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
index 230820719aa..8feae48f12e 100644
--- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp
+++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
@@ -2501,6 +2501,14 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 			gameobj->GetDeformer()->UpdateBuckets();
 	}
 
+	// Set up armature constraints
+	for (i=0;i<sumolist->GetCount();++i)
+	{
+		KX_GameObject* gameobj = (KX_GameObject*) sumolist->GetValue(i);
+		if (gameobj->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE)
+			((BL_ArmatureObject*)gameobj)->LoadConstraints(converter);
+	}
+
 	bool processCompoundChildren = false;
 
 	// create physics information
diff --git a/source/gameengine/Converter/BL_ShapeActionActuator.h b/source/gameengine/Converter/BL_ShapeActionActuator.h
index 28a6d90abdf..80157630e27 100644
--- a/source/gameengine/Converter/BL_ShapeActionActuator.h
+++ b/source/gameengine/Converter/BL_ShapeActionActuator.h
@@ -51,7 +51,7 @@ public:
 						short	blendin,
 						short	priority,
 						float	stride) 
-		: SCA_IActuator(gameobj),
+		: SCA_IActuator(gameobj, KX_ACT_SHAPEACTION),
 		
 		m_lastpos(0, 0, 0),
 		m_blendframe(0),
diff --git a/source/gameengine/Converter/CMakeLists.txt b/source/gameengine/Converter/CMakeLists.txt
index 257ca856b2c..d617934ec01 100644
--- a/source/gameengine/Converter/CMakeLists.txt
+++ b/source/gameengine/Converter/CMakeLists.txt
@@ -59,6 +59,7 @@ SET(INC
 	../../../source/blender/misc
 	../../../source/blender/blenloader
 	../../../source/blender/gpu
+	../../../source/blender/ikplugin
 	../../../extern/bullet2/src
 	${PYTHON_INC}
 )
diff --git a/source/gameengine/Converter/KX_ConvertActuators.cpp b/source/gameengine/Converter/KX_ConvertActuators.cpp
index 91a39bd7686..1cb16acf148 100644
--- a/source/gameengine/Converter/KX_ConvertActuators.cpp
+++ b/source/gameengine/Converter/KX_ConvertActuators.cpp
@@ -88,6 +88,7 @@
 #include "DNA_packedFile_types.h"
 #include "BL_ActionActuator.h"
 #include "BL_ShapeActionActuator.h"
+#include "BL_ArmatureActuator.h"
 /* end of blender include block */
 
 #include "BL_BlenderDataConversion.h"
@@ -1021,6 +1022,15 @@ void BL_ConvertActuators(char* maggiename,
 				break;
 			}
 		
+		case ACT_ARMATURE:
+			{
+				bArmatureActuator* armAct = (bArmatureActuator*) bact->data;
+				KX_GameObject *tmpgob = converter->FindGameObject(armAct->target);
+				KX_GameObject *subgob = converter->FindGameObject(armAct->subtarget);
+				BL_ArmatureActuator* tmparmact = new BL_ArmatureActuator(gameobj, armAct->type, armAct->posechannel, armAct->constraint, tmpgob, subgob, armAct->weight);
+				baseact = tmparmact;
+				break;
+			}
 		default:
 			; /* generate some error */
 		}
diff --git a/source/gameengine/Converter/KX_ConvertSensors.cpp b/source/gameengine/Converter/KX_ConvertSensors.cpp
index 09027f18844..99a2ec3ca64 100644
--- a/source/gameengine/Converter/KX_ConvertSensors.cpp
+++ b/source/gameengine/Converter/KX_ConvertSensors.cpp
@@ -64,6 +64,7 @@ probably misplaced */
 #include "KX_NearSensor.h"
 #include "KX_RadarSensor.h"
 #include "KX_MouseFocusSensor.h"
+#include "KX_ArmatureSensor.h"
 #include "SCA_JoystickSensor.h"
 #include "KX_NetworkMessageSensor.h"
 #include "SCA_ActuatorSensor.h"
@@ -302,7 +303,7 @@ void BL_ConvertSensors(struct Object* blenderobject,
 		case  SENS_ALWAYS:
 			{
 				
-				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::ALWAYS_EVENTMGR);
+				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
 				if (eventmgr)
 				{
 					gamesensor = new SCA_AlwaysSensor(eventmgr, gameobj);
@@ -314,7 +315,7 @@ void BL_ConvertSensors(struct Object* blenderobject,
 		case  SENS_DELAY:
 			{
 				// we can reuse the Always event manager for the delay sensor
-				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::ALWAYS_EVENTMGR);
+				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
 				if (eventmgr)
 				{
 					bDelaySensor* delaysensor = (bDelaySensor*)sens->data;
@@ -550,7 +551,7 @@ void BL_ConvertSensors(struct Object* blenderobject,
 			{
 				bPropertySensor* blenderpropsensor = (bPropertySensor*) sens->data;
 				SCA_EventManager* eventmgr 
-					= logicmgr->FindEventManager(SCA_EventManager::PROPERTY_EVENTMGR);
+					= logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
 				if (eventmgr)
 				{
 					STR_String propname=blenderpropsensor->name;
@@ -601,6 +602,21 @@ void BL_ConvertSensors(struct Object* blenderobject,
 				break;
 			}
 			
+		case SENS_ARMATURE:
+			{
+				bArmatureSensor* blenderarmsensor = (bArmatureSensor*) sens->data;
+				// we will reuse the property event manager, there is nothing special with this sensor
+				SCA_EventManager* eventmgr 
+					= logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
+				if (eventmgr)
+				{
+					STR_String bonename=blenderarmsensor->posechannel;
+					STR_String constraintname=blenderarmsensor->constraint;
+					gamesensor = new KX_ArmatureSensor(eventmgr,gameobj,bonename,constraintname, blenderarmsensor->type, blenderarmsensor->value);
+				}
+				break;
+			}
+
 		case SENS_RADAR:
 			{
 				
@@ -659,7 +675,7 @@ void BL_ConvertSensors(struct Object* blenderobject,
 				bRaySensor* blenderraysensor = (bRaySensor*) sens->data;
 				
 				//blenderradarsensor->angle;
-				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::RAY_EVENTMGR);
+				SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
 				if (eventmgr)
 				{
 					bool bFindMaterial = (blenderraysensor->mode & SENS_COLLISION_MATERIAL);
@@ -691,7 +707,7 @@ void BL_ConvertSensors(struct Object* blenderobject,
 				// some files didn't write randomsensor, avoid crash now for NULL ptr's
 				if (blenderrndsensor)
 				{
-					SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::RANDOM_EVENTMGR);
+					SCA_EventManager* eventmgr = logicmgr->FindEventManager(SCA_EventManager::BASIC_EVENTMGR);
 					if (eventmgr)
 					{
 						int randomSeed = blenderrndsensor->seed;
diff --git a/source/gameengine/Converter/Makefile b/source/gameengine/Converter/Makefile
index e261f9350e9..d41cc705087 100644
--- a/source/gameengine/Converter/Makefile
+++ b/source/gameengine/Converter/Makefile
@@ -53,6 +53,7 @@ CPPFLAGS += -I../../blender/blenlib
 CPPFLAGS += -I../../blender/blenkernel
 CPPFLAGS += -I../../blender/render/extern/include
 CPPFLAGS += -I../../blender/gpu
+CPPFLAGS += -I../../blender/ikplugin
 CPPFLAGS += -I$(NAN_GUARDEDALLOC)/include
 CPPFLAGS += -I../Expressions -I../Rasterizer -I../GameLogic
 CPPFLAGS += -I../Ketsji -I../BlenderRoutines -I../SceneGraph
diff --git a/source/gameengine/Converter/SConscript b/source/gameengine/Converter/SConscript
index 2d126310475..5f6e9df521b 100644
--- a/source/gameengine/Converter/SConscript
+++ b/source/gameengine/Converter/SConscript
@@ -19,6 +19,7 @@ incs += ' #source/gameengine/Network/LoopBackNetwork'
 incs += ' #source/blender/misc #source/blender/blenloader #source/blender/gpu'
 incs += ' #source/blender/windowmanager'
 incs += ' #source/blender/makesrna'
+incs += ' #source/blender/ikplugin'
 
 incs += ' ' + env['BF_PYTHON_INC']
 incs += ' ' + env['BF_BULLET_INC']
diff --git a/source/gameengine/Expressions/CMakeLists.txt b/source/gameengine/Expressions/CMakeLists.txt
index 439a50852a7..ec3738d1fe8 100644
--- a/source/gameengine/Expressions/CMakeLists.txt
+++ b/source/gameengine/Expressions/CMakeLists.txt
@@ -30,6 +30,7 @@ SET(INC
 	.
 	../../../source/kernel/gen_system
 	../../../intern/string
+	../../../intern/guardedalloc
 	../../../intern/moto/include
 	../../../source/gameengine/SceneGraph
 	../../../source/blender/blenloader
diff --git a/source/gameengine/Expressions/PyObjectPlus.cpp b/source/gameengine/Expressions/PyObjectPlus.cpp
index 1d1d9e6103b..7adbf5ac651 100644
--- a/source/gameengine/Expressions/PyObjectPlus.cpp
+++ b/source/gameengine/Expressions/PyObjectPlus.cpp
@@ -51,6 +51,7 @@
 #include "PyObjectPlus.h"
 #include "STR_String.h"
 #include "MT_Vector3.h"
+#include "MEM_guardedalloc.h"
 /*------------------------------
  * PyObjectPlus Type		-- Every class, even the abstract one should have a Type
 ------------------------------*/
@@ -95,7 +96,6 @@ PyObject *PyObjectPlus::py_base_repr(PyObject *self)			// This should be the ent
 		PyErr_SetString(PyExc_SystemError, BGE_PROXY_ERROR_MSG);
 		return NULL;
 	}
-	
 	return self_plus->py_repr();  
 }
 
@@ -145,42 +145,55 @@ PyObject * PyObjectPlus::py_base_new(PyTypeObject *type, PyObject *args, PyObjec
 
 	PyObjectPlus_Proxy *ret = (PyObjectPlus_Proxy *) type->tp_alloc(type, 0); /* starts with 1 ref, used for the return ref' */
 	ret->ref= base->ref;
-	base->ref= NULL;		/* invalidate! disallow further access */
-
+	ret->ptr= base->ptr;
 	ret->py_owns= base->py_owns;
+	ret->py_ref = base->py_ref;
 
-	ret->ref->m_proxy= NULL;
-
-	/* 'base' may be free'd after this func finished but not necessarily
-	 * there is no reference to the BGE data now so it will throw an error on access */
-	Py_DECREF(base);
-
-	ret->ref->m_proxy= (PyObject *)ret; /* no need to add a ref because one is added when creating. */
-	Py_INCREF(ret); /* we return a new ref but m_proxy holds a ref so we need to add one */
-
-
-	/* 'ret' will have 2 references.
-	 * - One ref is needed because ret->ref->m_proxy holds a refcount to the current proxy.
-	 * - Another is needed for returning the value.
-	 *
-	 * So we should be ok with 2 refs, but for some reason this crashes. so adding a new ref...
-	 * */
+	if (ret->py_ref) {
+		base->ref= NULL;		/* invalidate! disallow further access */
+		base->ptr = NULL;
+		if (ret->ref)
+			ret->ref->m_proxy= NULL;
+		/* 'base' may be free'd after this func finished but not necessarily
+		 * there is no reference to the BGE data now so it will throw an error on access */
+		Py_DECREF(base);
+		if (ret->ref) {
+			ret->ref->m_proxy= (PyObject *)ret; /* no need to add a ref because one is added when creating. */
+			Py_INCREF(ret); /* we return a new ref but m_proxy holds a ref so we need to add one */
+		}
+	} else {
+		// generic structures don't hold a reference to this proxy, so don't increment ref count
+		if (ret->py_owns)
+			// but if the proxy owns the structure, there can be only one owner
+			base->ptr= NULL;
+	}
 
 	return (PyObject *)ret;
 }
 
 void PyObjectPlus::py_base_dealloc(PyObject *self)				// python wrapper
 {
-	PyObjectPlus *self_plus= BGE_PROXY_REF(self);
-	if(self_plus) {
-		if(BGE_PROXY_PYOWNS(self)) { /* Does python own this?, then delete it  */
-			self_plus->m_proxy = NULL; /* Need this to stop ~PyObjectPlus from decrefing m_proxy otherwise its decref'd twice and py-debug crashes */
-			delete self_plus;
+	if (BGE_PROXY_PYREF(self)) {
+		PyObjectPlus *self_plus= BGE_PROXY_REF(self);
+		if(self_plus) {
+			if(BGE_PROXY_PYOWNS(self)) { /* Does python own this?, then delete it  */
+				self_plus->m_proxy = NULL; /* Need this to stop ~PyObjectPlus from decrefing m_proxy otherwise its decref'd twice and py-debug crashes */
+				delete self_plus;
+			}
+			BGE_PROXY_REF(self)= NULL; // not really needed
+		}
+		// the generic pointer is not deleted directly, only through self_plus
+		BGE_PROXY_PTR(self)= NULL; // not really needed
+	} else {
+		void *ptr= BGE_PROXY_PTR(self);
+		if(ptr) {
+			if(BGE_PROXY_PYOWNS(self)) { /* Does python own this?, then delete it  */
+				// generic structure owned by python MUST be created though MEM_alloc
+				MEM_freeN(ptr);
+			}
+			BGE_PROXY_PTR(self)= NULL; // not really needed
 		}
-
-		BGE_PROXY_REF(self)= NULL; // not really needed
 	}
-
 #if 0
 	/* is ok normally but not for subtyping, use tp_free instead. */
 	PyObject_DEL( self );
@@ -217,8 +230,9 @@ PyObject* PyObjectPlus::pyattr_get_invalid(void *self_v, const KX_PYATTRIBUTE_DE
 /* note, this is called as a python 'getset, where the PyAttributeDef is the closure */
 PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *attrdef)
 {
-	void *self= (void *)(BGE_PROXY_REF(self_py));
-	if(self==NULL) {
+	PyObjectPlus *ref= (BGE_PROXY_REF(self_py));
+	char* ptr = (attrdef->m_usePtr) ? (char*)BGE_PROXY_PTR(self_py) : (char*)ref;
+	if(ptr == NULL || (BGE_PROXY_PYREF(self_py) && (ref==NULL || !ref->py_is_valid()))) {
 		if(attrdef == attr_invalid)
 			Py_RETURN_TRUE; // dont bother running the function
 
@@ -226,7 +240,6 @@ PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *
 		return NULL;
 	}
 
-
 	if (attrdef->m_type == KX_PYATTRIBUTE_TYPE_DUMMY)
 	{
 		// fake attribute, ignore
@@ -237,9 +250,9 @@ PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *
 		// the attribute has no field correspondance, handover processing to function.
 		if (attrdef->m_getFunction == NULL)
 			return NULL;
-		return (*attrdef->m_getFunction)(self, attrdef);
+		return (*attrdef->m_getFunction)(ref, attrdef);
 	}
-	char *ptr = reinterpret_cast<char*>(self)+attrdef->m_offset;
+	ptr += attrdef->m_offset;
 	if (attrdef->m_length > 1)
 	{
 		PyObject* resultlist = PyList_New(attrdef->m_length);
@@ -293,6 +306,35 @@ PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *
 	else
 	{
 		switch (attrdef->m_type) {
+		case KX_PYATTRIBUTE_TYPE_FLAG:
+			{
+				bool bval;
+				switch (attrdef->m_size) {
+				case 1:
+					{
+						unsigned char *val = reinterpret_cast<unsigned char*>(ptr);
+						bval = (*val & attrdef->m_imin);
+						break;
+					}
+				case 2:
+					{
+						unsigned short *val = reinterpret_cast<unsigned short*>(ptr);
+						bval = (*val & attrdef->m_imin);
+						break;
+					}
+				case 4:
+					{
+						unsigned int *val = reinterpret_cast<unsigned int*>(ptr);
+						bval = (*val & attrdef->m_imin);
+						break;
+					}
+				default:
+					return NULL;
+				}
+				if (attrdef->m_imax)
+					bval = !bval;
+				return PyLong_FromSsize_t(bval);
+			}
 		case KX_PYATTRIBUTE_TYPE_BOOL:
 			{
 				bool *val = reinterpret_cast<bool*>(ptr);
@@ -318,7 +360,49 @@ PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *
 		case KX_PYATTRIBUTE_TYPE_FLOAT:
 			{
 				float *val = reinterpret_cast<float*>(ptr);
-				return PyFloat_FromDouble(*val);
+				if (attrdef->m_imin == 0) {
+					if (attrdef->m_imax == 0) {
+						return PyFloat_FromDouble(*val);
+					} else {
+						// vector, verify size
+						if (attrdef->m_size != attrdef->m_imax*sizeof(float)) 
+						{
+							return NULL;
+						}
+#ifdef USE_MATHUTILS
+						return newVectorObject(val, attrdef->m_imax, Py_NEW, NULL);
+#else
+						PyObject* resultlist = PyList_New(attrdef->m_imax);
+						for (unsigned int i=0; i<attrdef->m_imax; i++)
+						{
+							PyList_SET_ITEM(resultlist,i,PyFloat_FromDouble(val[i]));
+						}
+						return resultlist;
+#endif
+					}
+				} else {
+					// matrix case
+					if (attrdef->m_size != attrdef->m_imax*attrdef->m_imin*sizeof(float)) 
+					{
+						return NULL;
+					}
+#ifdef USE_MATHUTILS
+					return newMatrixObject(val, attrdef->m_imin, attrdef->m_imax, Py_WRAP, NULL);
+#else
+					PyObject* rowlist = PyList_New(attrdef->m_imin);
+					for (unsigned int i=0; i<attrdef->m_imin; i++)
+					{
+						PyObject* collist = PyList_New(attrdef->m_imax);
+						for (unsigned int j=0; j<attrdef->m_imax; j++)
+						{
+							PyList_SET_ITEM(collist,j,PyFloat_FromDouble(val[j]));
+						}
+						PyList_SET_ITEM(rowlist,i,collist);
+						val += attrdef->m_imax;
+					}
+					return rowlist;
+#endif
+				}
 			}
 		case KX_PYATTRIBUTE_TYPE_VECTOR:
 			{
@@ -340,17 +424,47 @@ PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *
 				STR_String *val = reinterpret_cast<STR_String*>(ptr);
 				return PyUnicode_FromString(*val);
 			}
+		case KX_PYATTRIBUTE_TYPE_CHAR:
+			{
+				return PyUnicode_FromString(ptr);
+			}
 		default:
 			return NULL;
 		}
 	}
 }
 
+
+static bool py_check_attr_float(float *var, PyObject *value, const PyAttributeDef *attrdef)
+{
+	double val = PyFloat_AsDouble(value);
+	if (val == -1.0 && PyErr_Occurred())
+	{
+		PyErr_Format(PyExc_TypeError, "expected float value for attribute \"%s\"", attrdef->m_name);
+		return false;
+	}
+	if (attrdef->m_clamp)
+	{
+		if (val < attrdef->m_fmin)
+			val = attrdef->m_fmin;
+		else if (val > attrdef->m_fmax)
+			val = attrdef->m_fmax;
+	}
+	else if (val < attrdef->m_fmin || val > attrdef->m_fmax)
+	{
+		PyErr_Format(PyExc_ValueError, "value out of range for attribute \"%s\"", attrdef->m_name);
+		return false;
+	}
+	*var = (float)val;
+	return true;
+}
+
 /* note, this is called as a python getset */
 int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAttributeDef *attrdef)
 {
-	void *self= (void *)(BGE_PROXY_REF(self_py));
-	if(self==NULL) {
+	PyObjectPlus *ref= (BGE_PROXY_REF(self_py));
+	char* ptr = (attrdef->m_usePtr) ? (char*)BGE_PROXY_PTR(self_py) : (char*)ref;
+	if(ref==NULL || !ref->py_is_valid() || ptr==NULL) {
 		PyErr_SetString(PyExc_SystemError, BGE_PROXY_ERROR_MSG);
 		return PY_SET_ATTR_FAIL;
 	}
@@ -358,8 +472,10 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 	void *undoBuffer = NULL;
 	void *sourceBuffer = NULL;
 	size_t bufferSize = 0;
+	PyObject *item = NULL;	// to store object that must be dereferenced in case of error
+	PyObject *list = NULL;	// to store object that must be dereferenced in case of error
 	
-	char *ptr = reinterpret_cast<char*>(self)+attrdef->m_offset;
+	ptr += attrdef->m_offset;
 	if (attrdef->m_length > 1)
 	{
 		if (!PySequence_Check(value)) 
@@ -380,7 +496,7 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				PyErr_Format(PyExc_AttributeError, "function attribute without function for attribute \"%s\", report to blender.org", attrdef->m_name);
 				return PY_SET_ATTR_FAIL;
 			}
-			return (*attrdef->m_setFunction)(self, attrdef, value);
+			return (*attrdef->m_setFunction)(ref, attrdef, value);
 		case KX_PYATTRIBUTE_TYPE_BOOL:
 			bufferSize = sizeof(bool);
 			break;
@@ -409,10 +525,7 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 		}
 		for (int i=0; i<attrdef->m_length; i++)
 		{
-			PyObject *item = PySequence_GetItem(value, i); /* new ref */
-			// we can decrement the reference immediately, the reference count
-			// is at least 1 because the item is part of an array
-			Py_DECREF(item);
+			item = PySequence_GetItem(value, i); /* new ref */
 			switch (attrdef->m_type) 
 			{
 			case KX_PYATTRIBUTE_TYPE_BOOL:
@@ -528,11 +641,14 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				PyErr_Format(PyExc_AttributeError, "type check error for attribute \"%s\", report to blender.org", attrdef->m_name);
 				goto UNDO_AND_ERROR;
 			}
+			// finished using item, release
+			Py_DECREF(item);
+			item = NULL;
 		}
 		// no error, call check function if any
 		if (attrdef->m_checkFunction != NULL)
 		{
-			if ((*attrdef->m_checkFunction)(self, attrdef) != 0)
+			if ((*attrdef->m_checkFunction)(ref, attrdef) != 0)
 			{
 				// if the checing function didnt set an error then set a generic one here so we dont set an error with no exception
 				if (PyErr_Occurred()==0)
@@ -545,6 +661,8 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 					memcpy(sourceBuffer, undoBuffer, bufferSize);
 					free(undoBuffer);
 				}
+				if (item)
+					Py_DECREF(item);
 				return PY_SET_ATTR_FAIL;
 			}
 		}
@@ -561,7 +679,7 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				PyErr_Format(PyExc_AttributeError, "function attribute without function \"%s\", report to blender.org", attrdef->m_name);
 				return PY_SET_ATTR_FAIL;
 			}
-			return (*attrdef->m_setFunction)(self, attrdef, value);
+			return (*attrdef->m_setFunction)(ref, attrdef, value);
 		}
 		if (attrdef->m_checkFunction != NULL || attrdef->m_type == KX_PYATTRIBUTE_TYPE_VECTOR)
 		{
@@ -576,11 +694,19 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				bufferSize = sizeof(short);
 				break;
 			case KX_PYATTRIBUTE_TYPE_ENUM:
+			case KX_PYATTRIBUTE_TYPE_FLAG:
+			case KX_PYATTRIBUTE_TYPE_CHAR:
+				bufferSize = attrdef->m_size;
+				break;
 			case KX_PYATTRIBUTE_TYPE_INT:
 				bufferSize = sizeof(int);
 				break;
 			case KX_PYATTRIBUTE_TYPE_FLOAT:
 				bufferSize = sizeof(float);
+				if (attrdef->m_imax)
+					bufferSize *= attrdef->m_imax;
+				if (attrdef->m_imin)
+					bufferSize *= attrdef->m_imin;
 				break;
 			case KX_PYATTRIBUTE_TYPE_STRING:
 				sourceBuffer = reinterpret_cast<STR_String*>(ptr)->Ptr();
@@ -624,6 +750,49 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				}
 				break;
 			}
+		case KX_PYATTRIBUTE_TYPE_FLAG:
+			{
+				bool bval;
+				if (PyLong_Check(value)) 
+				{
+					bval = (PyLong_AsSsize_t(value) != 0);
+				} 
+				else if (PyBool_Check(value))
+				{
+					bval = (value == Py_True);
+				}
+				else
+				{
+					PyErr_Format(PyExc_TypeError, "expected an integer or a bool for attribute \"%s\"", attrdef->m_name);
+					goto FREE_AND_ERROR;
+				}
+				if (attrdef->m_imax)
+					bval = !bval;
+				switch (attrdef->m_size) {
+				case 1:
+					{
+						unsigned char *val = reinterpret_cast<unsigned char*>(ptr);
+						*val = (*val & ~attrdef->m_imin) | ((bval)?attrdef->m_imin:0);
+						break;
+					}
+				case 2:
+					{
+						unsigned short *val = reinterpret_cast<unsigned short*>(ptr);
+						*val = (*val & ~attrdef->m_imin) | ((bval)?attrdef->m_imin:0);
+						break;
+					}
+				case 4:
+					{
+						unsigned int *val = reinterpret_cast<unsigned int*>(ptr);
+						*val = (*val & ~attrdef->m_imin) | ((bval)?attrdef->m_imin:0);
+						break;
+					}
+				default:
+					PyErr_Format(PyExc_TypeError, "internal error: unsupported flag field \"%s\"", attrdef->m_name);
+					goto FREE_AND_ERROR;
+				}
+				break;
+			}
 		case KX_PYATTRIBUTE_TYPE_SHORT:
 			{
 				short int *var = reinterpret_cast<short int*>(ptr);
@@ -689,25 +858,71 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 		case KX_PYATTRIBUTE_TYPE_FLOAT:
 			{
 				float *var = reinterpret_cast<float*>(ptr);
-				double val = PyFloat_AsDouble(value);
-				if (val == -1.0 && PyErr_Occurred())
+				if (attrdef->m_imin != 0) 
 				{
-					PyErr_Format(PyExc_TypeError, "expected a float for attribute \"%s\"", attrdef->m_name);
-					goto FREE_AND_ERROR;
-				}
-				else if (attrdef->m_clamp)
+					if (attrdef->m_size != attrdef->m_imin*attrdef->m_imax*sizeof(float)) 
+					{
+						PyErr_Format(PyExc_TypeError, "internal error: incorrect field size for attribute \"%s\"", attrdef->m_name);
+						goto FREE_AND_ERROR;
+					}
+					if (!PySequence_Check(value) || PySequence_Size(value) != attrdef->m_imin) 
+					{
+						PyErr_Format(PyExc_TypeError, "expected a sequence of [%d][%d] floats for attribute \"%s\"", attrdef->m_imin, attrdef->m_imax, attrdef->m_name);
+						goto FREE_AND_ERROR;
+					}
+					for (int i=0; i<attrdef->m_imin; i++)
+					{
+						PyObject *list = PySequence_GetItem(value, i); /* new ref */
+						if (!PySequence_Check(list) || PySequence_Size(list) != attrdef->m_imax) 
+						{
+							PyErr_Format(PyExc_TypeError, "expected a sequence of [%d][%d] floats for attribute \"%s\"", attrdef->m_imin, attrdef->m_imax, attrdef->m_name);
+							goto RESTORE_AND_ERROR;
+						}
+						for (int j=0; j<attrdef->m_imax; j++)
+						{
+							item = PySequence_GetItem(list, j); /* new ref */
+							if (!py_check_attr_float(var, item, attrdef))
+							{
+								PyErr_Format(PyExc_TypeError, "expected a sequence of [%d][%d] floats for attribute \"%s\"", attrdef->m_imin, attrdef->m_imax, attrdef->m_name);
+								goto RESTORE_AND_ERROR;
+							}
+							Py_DECREF(item);
+							item = NULL;
+							++var;
+						}
+						Py_DECREF(list);
+						list = NULL;
+					}
+				} 
+				else if (attrdef->m_imax != 0) 
 				{
-					if (val < attrdef->m_fmin)
-						val = attrdef->m_fmin;
-					else if (val > attrdef->m_fmax)
-						val = attrdef->m_fmax;
-				}
-				else if (val < attrdef->m_fmin || val > attrdef->m_fmax)
+					if (attrdef->m_size != attrdef->m_imax*sizeof(float)) 
+					{
+						PyErr_Format(PyExc_TypeError, "internal error: incorrect field size for attribute \"%s\"", attrdef->m_name);
+						goto FREE_AND_ERROR;
+					}
+					if (!PySequence_Check(value) || PySequence_Size(value) != attrdef->m_imax) 
+					{
+						PyErr_Format(PyExc_TypeError, "expected a sequence of [%d] floats for attribute \"%s\"", attrdef->m_imax, attrdef->m_name);
+						goto FREE_AND_ERROR;
+					}
+					for (int i=0; i<attrdef->m_imax; i++)
+					{
+						item = PySequence_GetItem(value, i); /* new ref */
+						if (!py_check_attr_float(var, item, attrdef))
+						{
+							goto RESTORE_AND_ERROR;
+						}
+						Py_DECREF(item);
+						item = NULL;
+						++var;
+					}
+				} 
+				else
 				{
-					PyErr_Format(PyExc_ValueError, "value out of range for attribute \"%s\"", attrdef->m_name);
-					goto FREE_AND_ERROR;
+					if (!py_check_attr_float(var, value, attrdef))
+						goto FREE_AND_ERROR;
 				}
-				*var = (float)val;
 				break;
 			}
 		case KX_PYATTRIBUTE_TYPE_VECTOR:
@@ -715,16 +930,15 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				if (!PySequence_Check(value) || PySequence_Size(value) != 3) 
 				{
 					PyErr_Format(PyExc_TypeError, "expected a sequence of 3 floats for attribute \"%s\"", attrdef->m_name);
-					return PY_SET_ATTR_FAIL;
+					goto FREE_AND_ERROR;
 				}
 				MT_Vector3 *var = reinterpret_cast<MT_Vector3*>(ptr);
 				for (int i=0; i<3; i++)
 				{
-					PyObject *item = PySequence_GetItem(value, i); /* new ref */
-					// we can decrement the reference immediately, the reference count
-					// is at least 1 because the item is part of an array
-					Py_DECREF(item);
+					item = PySequence_GetItem(value, i); /* new ref */
 					double val = PyFloat_AsDouble(item);
+					Py_DECREF(item);
+					item = NULL;
 					if (val == -1.0 && PyErr_Occurred())
 					{
 						PyErr_Format(PyExc_TypeError, "expected a sequence of 3 floats for attribute \"%s\"", attrdef->m_name);
@@ -746,6 +960,22 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 				}
 				break;
 			}
+		case KX_PYATTRIBUTE_TYPE_CHAR:
+			{
+				if (PyUnicode_Check(value)) 
+				{
+					Py_ssize_t val_len;
+					char *val = _PyUnicode_AsStringAndSize(value, &val_len);
+					strncpy(ptr, val, attrdef->m_size);
+					ptr[attrdef->m_size-1] = 0;
+				}
+				else
+				{
+					PyErr_Format(PyExc_TypeError, "expected a string for attribute \"%s\"", attrdef->m_name);
+					goto FREE_AND_ERROR;
+				}
+				break;
+			}
 		case KX_PYATTRIBUTE_TYPE_STRING:
 			{
 				STR_String *var = reinterpret_cast<STR_String*>(ptr);
@@ -793,7 +1023,7 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 	// check if post processing is needed
 	if (attrdef->m_checkFunction != NULL)
 	{
-		if ((*attrdef->m_checkFunction)(self, attrdef) != 0)
+		if ((*attrdef->m_checkFunction)(ref, attrdef) != 0)
 		{
 			// restore value
 		RESTORE_AND_ERROR:
@@ -814,6 +1044,10 @@ int PyObjectPlus::py_set_attrdef(PyObject *self_py, PyObject *value, const PyAtt
 		FREE_AND_ERROR:
 			if (undoBuffer)
 				free(undoBuffer);
+			if (list)
+				Py_DECREF(list);
+			if (item)
+				Py_DECREF(item);
 			return 1;
 		}
 	}
@@ -857,23 +1091,35 @@ void PyObjectPlus::InvalidateProxy()		// check typename of each parent
 	}
 }
 
-PyObject *PyObjectPlus::GetProxy_Ext(PyObjectPlus *self, PyTypeObject *tp)
+PyObject *PyObjectPlus::GetProxyPlus_Ext(PyObjectPlus *self, PyTypeObject *tp, void *ptr)
 {
 	if (self->m_proxy==NULL)
 	{
 		self->m_proxy = reinterpret_cast<PyObject *>PyObject_NEW( PyObjectPlus_Proxy, tp);
 		BGE_PROXY_PYOWNS(self->m_proxy) = false;
+		BGE_PROXY_PYREF(self->m_proxy) = true;
 	}
 	//PyObject_Print(self->m_proxy, stdout, 0);
 	//printf("ref %d\n", self->m_proxy->ob_refcnt);
 	
 	BGE_PROXY_REF(self->m_proxy) = self; /* Its possible this was set to NULL, so set it back here */
+	BGE_PROXY_PTR(self->m_proxy) = ptr;
 	Py_INCREF(self->m_proxy); /* we own one, thos ones fore the return */
 	return self->m_proxy;
 }
 
-PyObject *PyObjectPlus::NewProxy_Ext(PyObjectPlus *self, PyTypeObject *tp, bool py_owns)
+PyObject *PyObjectPlus::NewProxyPlus_Ext(PyObjectPlus *self, PyTypeObject *tp, void *ptr, bool py_owns)
 {
+	if (!self) 
+	{
+		// in case of proxy without reference to game object
+		PyObject* proxy = reinterpret_cast<PyObject *>PyObject_NEW( PyObjectPlus_Proxy, tp);
+		BGE_PROXY_PYREF(proxy) = false;
+		BGE_PROXY_PYOWNS(proxy) = py_owns;
+		BGE_PROXY_REF(proxy) = NULL; 
+		BGE_PROXY_PTR(proxy) = ptr;
+		return proxy;
+	}
 	if (self->m_proxy)
 	{
 		if(py_owns)
@@ -889,7 +1135,7 @@ PyObject *PyObjectPlus::NewProxy_Ext(PyObjectPlus *self, PyTypeObject *tp, bool
 		
 	}
 	
-	GetProxy_Ext(self, tp);
+	GetProxyPlus_Ext(self, tp, ptr);
 	if(py_owns) {
 		BGE_PROXY_PYOWNS(self->m_proxy) = py_owns;
 		Py_DECREF(self->m_proxy); /* could avoid thrashing here but for now its ok */
diff --git a/source/gameengine/Expressions/PyObjectPlus.h b/source/gameengine/Expressions/PyObjectPlus.h
index d8ab007dde9..237f01f5a3a 100644
--- a/source/gameengine/Expressions/PyObjectPlus.h
+++ b/source/gameengine/Expressions/PyObjectPlus.h
@@ -88,13 +88,17 @@ typedef struct {
 
 typedef struct PyObjectPlus_Proxy {
 	PyObject_HEAD		/* required python macro   */
-	class PyObjectPlus *ref;
-	bool py_owns;
+	class PyObjectPlus *ref;	// pointer to GE object, it holds a reference to this proxy
+	void *ptr;					// optional pointer to generic structure, the structure holds no reference to this proxy
+	bool py_owns;		// true if the object pointed by ref should be deleted when the proxy is deleted
+	bool py_ref;		// true if proxy is connected to a GE object (ref is used)
 } PyObjectPlus_Proxy;
 
 #define BGE_PROXY_ERROR_MSG "Blender Game Engine data has been freed, cannot use this python variable"
 #define BGE_PROXY_REF(_self) (((PyObjectPlus_Proxy *)_self)->ref)
+#define BGE_PROXY_PTR(_self) (((PyObjectPlus_Proxy *)_self)->ptr)
 #define BGE_PROXY_PYOWNS(_self) (((PyObjectPlus_Proxy *)_self)->py_owns)
+#define BGE_PROXY_PYREF(_self) (((PyObjectPlus_Proxy *)_self)->py_ref)
 
 /* Note, sometimes we dont care what BGE type this is as long as its a proxy */
 #define BGE_PROXY_CHECK_TYPE(_type) ((_type)->tp_dealloc == PyObjectPlus::py_base_dealloc)
@@ -103,27 +107,51 @@ typedef struct PyObjectPlus_Proxy {
 #define BGE_PROXY_FROM_REF(_self) (((PyObjectPlus *)_self)->GetProxy())
 
 
-								// This must be the first line of each 
-								// PyC++ class
+// This must be the first line of each 
+// PyC++ class
+// AttributesPtr correspond to attributes of proxy generic pointer 
+// each PyC++ class must be registered in KX_PythonInitTypes.cpp
 #define __Py_Header \
  public: \
   static PyTypeObject   Type; \
   static PyMethodDef    Methods[]; \
   static PyAttributeDef Attributes[]; \
   virtual PyTypeObject *GetType(void) {return &Type;}; \
-  virtual PyObject *GetProxy() {return GetProxy_Ext(this, &Type);}; \
-  virtual PyObject *NewProxy(bool py_owns) {return NewProxy_Ext(this, &Type, py_owns);}; 
+  virtual PyObject *GetProxy() {return GetProxyPlus_Ext(this, &Type, NULL);}; \
+  virtual PyObject *NewProxy(bool py_owns) {return NewProxyPlus_Ext(this, &Type, NULL, py_owns);}; \
 
+// leave above line empty (macro)!
+// use this macro for class that use generic pointer in proxy
+// GetProxy() and NewProxy() must be defined to set the correct pointer in the proxy
+#define __Py_HeaderPtr \
+ public: \
+  static PyTypeObject   Type; \
+  static PyMethodDef    Methods[]; \
+  static PyAttributeDef Attributes[]; \
+  static PyAttributeDef AttributesPtr[]; \
+  virtual PyTypeObject *GetType(void) {return &Type;}; \
+  virtual PyObject *GetProxy(); \
+  virtual PyObject *NewProxy(bool py_owns); \
 
+// leave above line empty (macro)!
 #ifdef WITH_CXX_GUARDEDALLOC
 #define Py_Header __Py_Header \
   void *operator new( unsigned int num_bytes) { return MEM_mallocN(num_bytes, Type.tp_name); } \
-  void operator delete( void *mem ) { MEM_freeN(mem); } 
+  void operator delete( void *mem ) { MEM_freeN(mem); } \
 
 #else
 #define Py_Header __Py_Header
 #endif
 
+#ifdef WITH_CXX_GUARDEDALLOC
+#define Py_HeaderPtr __Py_HeaderPtr \
+  void *operator new( unsigned int num_bytes) { return MEM_mallocN(num_bytes, Type.tp_name); } \
+  void operator delete( void *mem ) { MEM_freeN(mem); } \
+
+#else
+#define Py_HeaderPtr __Py_HeaderPtr
+#endif
+
 /*
  * nonzero values are an error for setattr
  * however because of the nested lookups we need to know if the errors
@@ -245,6 +273,8 @@ enum KX_PYATTRIBUTE_TYPE {
 	KX_PYATTRIBUTE_TYPE_DUMMY,
 	KX_PYATTRIBUTE_TYPE_FUNCTION,
 	KX_PYATTRIBUTE_TYPE_VECTOR,
+	KX_PYATTRIBUTE_TYPE_FLAG,
+	KX_PYATTRIBUTE_TYPE_CHAR,
 };
 
 enum KX_PYATTRIBUTE_ACCESS {
@@ -261,11 +291,14 @@ typedef struct KX_PYATTRIBUTE_DEF {
 	const char *m_name;				// name of the python attribute
 	KX_PYATTRIBUTE_TYPE m_type;		// type of value
 	KX_PYATTRIBUTE_ACCESS m_access;	// read/write access or read-only
-	int m_imin;						// minimum value in case of integer attributes (for string: minimum string length)
-	int m_imax;						// maximum value in case of integer attributes (for string: maximum string length)
+	int m_imin;						// minimum value in case of integer attributes 
+									// (for string: minimum string length, for flag: mask value, for float: matrix row size)
+	int m_imax;						// maximum value in case of integer attributes 
+									// (for string: maximum string length, for flag: 1 if flag is negative, float: vector/matrix col size)
 	float m_fmin;					// minimum value in case of float attributes
 	float m_fmax;					// maximum value in case of float attributes
 	bool   m_clamp;					// enforce min/max value by clamping
+	bool   m_usePtr;				// the attribute uses the proxy generic pointer, set at runtime
 	size_t m_offset;				// position of field in structure
 	size_t m_size;					// size of field for runtime verification (enum only)
 	size_t m_length;				// length of array, 1=simple attribute
@@ -283,104 +316,146 @@ typedef struct KX_PYATTRIBUTE_DEF {
 		float *m_floatPtr;
 		STR_String *m_stringPtr;
 		MT_Vector3 *m_vectorPtr;
+		char *m_charPtr;
 	} m_typeCheck;
 } PyAttributeDef;
 
 #define KX_PYATTRIBUTE_DUMMY(name) \
-	{ name, KX_PYATTRIBUTE_TYPE_DUMMY, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, 0, 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_DUMMY, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, 0, 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 
 #define KX_PYATTRIBUTE_BOOL_RW(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RW, 0, 1, 0.f, 0.f, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RW, 0, 1, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_BOOL_RW_CHECK(name,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RW, 0, 1, 0.f, 0.f, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RW, 0, 1, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_BOOL_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RO, 0, 1, 0.f, 0.f, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_BOOL, KX_PYATTRIBUTE_RO, 0, 1, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {&((object *)0)->field, NULL, NULL, NULL, NULL, NULL, NULL} }
+
+/* attribute points to a single bit of an integer field, attribute=true if bit is set */
+#define KX_PYATTRIBUTE_FLAG_RW(name,object,field,bit) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RW, bit, 0, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLAG_RW_CHECK(name,object,field,bit,function) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RW, bit, 0, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLAG_RO(name,object,field,bit) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RO, bit, 0, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
+
+/* attribute points to a single bit of an integer field, attribute=true if bit is set*/
+#define KX_PYATTRIBUTE_FLAG_NEGATIVE_RW(name,object,field,bit) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RW, bit, 1, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLAG_NEGATIVE_RW_CHECK(name,object,field,bit,function) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RW, bit, 1, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLAG_NEGATIVE_RO(name,object,field,bit) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLAG, KX_PYATTRIBUTE_RO, bit, 1, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 
 // enum field cannot be mapped to pointer (because we would need a pointer for each enum)
 // use field size to verify mapping at runtime only, assuming enum size is equal to int size.
 #define KX_PYATTRIBUTE_ENUM_RW(name,min,max,clamp,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_ENUM_RW_CHECK(name,min,max,clamp,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_ENUM_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_ENUM, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 
 #define KX_PYATTRIBUTE_SHORT_RW(name,min,max,clamp,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_RW_CHECK(name,min,max,clamp,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_ARRAY_RW(name,min,max,clamp,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_ARRAY_RW_CHECK(name,min,max,clamp,object,field,length,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_ARRAY_RO(name,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, ((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 // SHORT_LIST
 #define KX_PYATTRIBUTE_SHORT_LIST_RW(name,min,max,clamp,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_LIST_RW_CHECK(name,min,max,clamp,object,field,length,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_SHORT_LIST_RO(name,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_SHORT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, &((object *)0)->field, NULL, NULL, NULL, NULL, NULL} }
 
 #define KX_PYATTRIBUTE_INT_RW(name,min,max,clamp,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_RW_CHECK(name,min,max,clamp,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_ARRAY_RW(name,min,max,clamp,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_ARRAY_RW_CHECK(name,min,max,clamp,object,field,length,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_ARRAY_RO(name,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, ((object *)0)->field, NULL, NULL, NULL, NULL} }
 // INT_LIST
 #define KX_PYATTRIBUTE_INT_LIST_RW(name,min,max,clamp,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_LIST_RW_CHECK(name,min,max,clamp,object,field,length,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_INT_LIST_RO(name,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_INT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, &((object *)0)->field, NULL, NULL, NULL, NULL} }
 
 // always clamp for float
 #define KX_PYATTRIBUTE_FLOAT_RW(name,min,max,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_FLOAT_RW_CHECK(name,min,max,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_FLOAT_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, &((object *)0)->field, NULL, NULL, NULL} }
+// field must be float[n], returns a sequence
 #define KX_PYATTRIBUTE_FLOAT_ARRAY_RW(name,min,max,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_FLOAT_ARRAY_RW_CHECK(name,min,max,object,field,length,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, length, &object::function, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_FLOAT_ARRAY_RO(name,object,field,length) \
-	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL} }
-
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, length, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+// field must be float[n], returns a vector
+#define KX_PYATTRIBUTE_FLOAT_VECTOR_RW(name,min,max,object,field,length) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, length, min, max, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLOAT_VECTOR_RW_CHECK(name,min,max,object,field,length,function) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, 0, length, min, max, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLOAT_VECTOR_RO(name,object,field,length) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, 0, length, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field, NULL, NULL, NULL} }
+// field must be float[n][n], returns a matrix
+#define KX_PYATTRIBUTE_FLOAT_MATRIX_RW(name,min,max,object,field,length) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, length, length, min, max, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field[0], NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLOAT_MATRIX_RW_CHECK(name,min,max,object,field,length,function) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RW, length, length, min, max, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field[0], NULL, NULL, NULL} }
+#define KX_PYATTRIBUTE_FLOAT_MATRIX_RO(name,object,field,length) \
+	{ name, KX_PYATTRIBUTE_TYPE_FLOAT, KX_PYATTRIBUTE_RO, length, length, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, ((object *)0)->field[0], NULL, NULL, NULL} }
+
+// only for STR_String member
 #define KX_PYATTRIBUTE_STRING_RW(name,min,max,clamp,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
 #define KX_PYATTRIBUTE_STRING_RW_CHECK(name,min,max,clamp,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RW, min, max, 0.f, 0.f, clamp, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
 #define KX_PYATTRIBUTE_STRING_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, 1 , NULL, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_STRING, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, 1 , NULL, NULL, NULL, {NULL, NULL, NULL, NULL, &((object *)0)->field, NULL, NULL} }
+
+// only for char [] array 
+#define KX_PYATTRIBUTE_CHAR_RW(name,object,field) \
+	{ name, KX_PYATTRIBUTE_TYPE_CHAR, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0.f, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, ((object *)0)->field} }
+#define KX_PYATTRIBUTE_CHAR_RW_CHECK(name,object,field,function) \
+	{ name, KX_PYATTRIBUTE_TYPE_CHAR, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0.f, true, false, offsetof(object,field), sizeof(((object *)0)->field), 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, ((object *)0)->field} }
+#define KX_PYATTRIBUTE_CHAR_RO(name,object,field) \
+	{ name, KX_PYATTRIBUTE_TYPE_CHAR, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), sizeof(((object *)0)->field), 1 , NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, NULL, ((object *)0)->field} }
 
+// for MT_Vector3 member
 #define KX_PYATTRIBUTE_VECTOR_RW(name,min,max,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field} }
+	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, 1, NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
 #define KX_PYATTRIBUTE_VECTOR_RW_CHECK(name,min,max,clamp,object,field,function) \
-	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field} }
+	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RW, 0, 0, min, max, true, false, offsetof(object,field), 0, 1, &object::function, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
 #define KX_PYATTRIBUTE_VECTOR_RO(name,object,field) \
-	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, offsetof(object,field), 0, 1 , NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field} }
+	{ name, KX_PYATTRIBUTE_TYPE_VECTOR, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, offsetof(object,field), 0, 1 , NULL, NULL, NULL, {NULL, NULL, NULL, NULL, NULL, &((object *)0)->field, NULL} }
 
 #define KX_PYATTRIBUTE_RW_FUNCTION(name,object,getfunction,setfunction) \
-	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0.f, false, 0, 0, 1, NULL, &object::setfunction, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0.f, false, false, 0, 0, 1, NULL, &object::setfunction, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_RO_FUNCTION(name,object,getfunction) \
-	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, 0, 0, 1, NULL, NULL, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0.f, false, false, 0, 0, 1, NULL, NULL, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_ARRAY_RW_FUNCTION(name,object,length,getfunction,setfunction) \
-	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0,f, false, 0, 0, length, NULL, &object::setfunction, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RW, 0, 0, 0.f, 0,f, false, false, 0, 0, length, NULL, &object::setfunction, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 #define KX_PYATTRIBUTE_ARRAY_RO_FUNCTION(name,object,length,getfunction) \
-	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0,f, false, 0, 0, length, NULL, NULL, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL} }
+	{ name, KX_PYATTRIBUTE_TYPE_FUNCTION, KX_PYATTRIBUTE_RO, 0, 0, 0.f, 0,f, false, false, 0, 0, length, NULL, NULL, &object::getfunction, {NULL, NULL, NULL, NULL, NULL, NULL, NULL} }
 
 
 /*------------------------------
@@ -422,16 +497,23 @@ public:
 	/* These are all virtual python methods that are defined in each class
 	 * Our own fake subclassing calls these on each class, then calls the parent */
 	virtual PyObject*		py_repr(void);
+	/* subclass may overwrite this function to implement more sophisticated method of validating a proxy */
+	virtual bool			py_is_valid(void) { return true; }
 
 	static PyObject*		py_get_attrdef(PyObject *self_py, const PyAttributeDef *attrdef);
 	static int				py_set_attrdef(PyObject *self_py, PyObject *value, const PyAttributeDef *attrdef);
 	
 	/* Kindof dumb, always returns True, the false case is checked for, before this function gets accessed */
 	static PyObject*	pyattr_get_invalid(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef);
-	
-	static PyObject *GetProxy_Ext(PyObjectPlus *self, PyTypeObject *tp);
-	static PyObject *NewProxy_Ext(PyObjectPlus *self, PyTypeObject *tp, bool py_owns);
-	
+
+	static PyObject *GetProxyPlus_Ext(PyObjectPlus *self, PyTypeObject *tp, void *ptr);
+	/* self=NULL => proxy to generic pointer detached from GE object
+	                if py_owns is true, the memory pointed by ptr will be deleted automatially with MEM_freeN 
+	   self!=NULL=> proxy attached to GE object, ptr is optional and point to a struct from which attributes can be defined
+	                if py_owns is true, the object will be deleted automatically, ptr will NOT be deleted 
+					(assume object destructor takes care of it) */
+	static PyObject *NewProxyPlus_Ext(PyObjectPlus *self, PyTypeObject *tp, void *ptr, bool py_owns);
+
 	void	InvalidateProxy();
 	
 	/**
diff --git a/source/gameengine/Expressions/SConscript b/source/gameengine/Expressions/SConscript
index c819bfb0d3e..26cafb004b2 100644
--- a/source/gameengine/Expressions/SConscript
+++ b/source/gameengine/Expressions/SConscript
@@ -3,7 +3,7 @@ Import ('env')
 
 sources = env.Glob('*.cpp')
 
-incs ='. #source/kernel/gen_system #intern/string #intern/moto/include #source/gameengine/SceneGraph #source/blender/blenloader'
+incs ='. #source/kernel/gen_system #intern/guardedalloc #intern/string #intern/moto/include #source/gameengine/SceneGraph #source/blender/blenloader'
 incs += ' ' + env['BF_PYTHON_INC']
 
 env.BlenderLib ( 'bf_expressions', sources, Split(incs), [], libtype=['core','player'], priority = [360,80], cxx_compileflags=env['BGE_CXXFLAGS'])
diff --git a/source/gameengine/GameLogic/SCA_2DFilterActuator.cpp b/source/gameengine/GameLogic/SCA_2DFilterActuator.cpp
index 9dfb5ddc38f..9b04e263350 100644
--- a/source/gameengine/GameLogic/SCA_2DFilterActuator.cpp
+++ b/source/gameengine/GameLogic/SCA_2DFilterActuator.cpp
@@ -43,7 +43,7 @@ SCA_2DFilterActuator::SCA_2DFilterActuator(
 		int int_arg,
 		RAS_IRasterizer* rasterizer,
 		RAS_IRenderTools* rendertools)
-    : SCA_IActuator(gameobj),
+    : SCA_IActuator(gameobj, KX_ACT_2DFILTER),
      m_type(type),
 	 m_disableMotionBlur(flag),
 	 m_float_arg(float_arg),
diff --git a/source/gameengine/GameLogic/SCA_BasicEventManager.cpp b/source/gameengine/GameLogic/SCA_BasicEventManager.cpp
new file mode 100644
index 00000000000..24cae439fb7
--- /dev/null
+++ b/source/gameengine/GameLogic/SCA_BasicEventManager.cpp
@@ -0,0 +1,62 @@
+/**
+ * Manager for 'always' events. Since always sensors can operate in pulse
+ * mode, they need to be activated.
+ *
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include "SCA_BasicEventManager.h"
+#include "SCA_LogicManager.h"
+#include <vector>
+#include "SCA_ISensor.h"
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+using namespace std;
+
+SCA_BasicEventManager::SCA_BasicEventManager(class SCA_LogicManager* logicmgr)
+	: SCA_EventManager(BASIC_EVENTMGR),
+	m_logicmgr(logicmgr)
+{
+}
+
+SCA_BasicEventManager::~SCA_BasicEventManager()
+{
+}
+
+void SCA_BasicEventManager::NextFrame()
+{
+	SG_DList::iterator<SCA_ISensor> it(m_sensors);
+	for (it.begin();!it.end();++it)
+	{
+		(*it)->Activate(m_logicmgr);
+	}
+}
+
diff --git a/source/gameengine/GameLogic/SCA_BasicEventManager.h b/source/gameengine/GameLogic/SCA_BasicEventManager.h
new file mode 100644
index 00000000000..1aae5f9be5c
--- /dev/null
+++ b/source/gameengine/GameLogic/SCA_BasicEventManager.h
@@ -0,0 +1,59 @@
+/**
+ * Manager for sensor that only need to call Update
+ *
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef __SCA_BASICEVENTMGR
+#define __SCA_BASICEVENTMGR
+
+#include "SCA_EventManager.h"
+#include <vector>
+
+using namespace std;
+
+class SCA_BasicEventManager : public SCA_EventManager
+{
+	class SCA_LogicManager* m_logicmgr;
+
+public:
+	SCA_BasicEventManager(class SCA_LogicManager* logicmgr);
+	~SCA_BasicEventManager();
+
+	virtual void NextFrame();
+
+
+#ifdef WITH_CXX_GUARDEDALLOC
+public:
+	void *operator new( unsigned int num_bytes) { return MEM_mallocN(num_bytes, "GE:SCA_BasicEventManager"); }
+	void operator delete( void *mem ) { MEM_freeN(mem); }
+#endif
+};
+
+#endif //__SCA_BASICEVENTMGR
+
diff --git a/source/gameengine/GameLogic/SCA_EventManager.h b/source/gameengine/GameLogic/SCA_EventManager.h
index 424150ffa63..f77b115ee03 100644
--- a/source/gameengine/GameLogic/SCA_EventManager.h
+++ b/source/gameengine/GameLogic/SCA_EventManager.h
@@ -52,10 +52,10 @@ public:
 		TIME_EVENTMGR,
 		RANDOM_EVENTMGR,
 		RAY_EVENTMGR,
-		RADAR_EVENTMGR,
 		NETWORK_EVENTMGR,
 		JOY_EVENTMGR,
-		ACTUATOR_EVENTMGR
+		ACTUATOR_EVENTMGR,
+		BASIC_EVENTMGR
 	};
 
 	SCA_EventManager(EVENT_MANAGER_TYPE mgrtype);
diff --git a/source/gameengine/GameLogic/SCA_IActuator.cpp b/source/gameengine/GameLogic/SCA_IActuator.cpp
index 0fda75590c1..0338213b3cf 100644
--- a/source/gameengine/GameLogic/SCA_IActuator.cpp
+++ b/source/gameengine/GameLogic/SCA_IActuator.cpp
@@ -34,8 +34,9 @@
 
 using namespace std;
 
-SCA_IActuator::SCA_IActuator(SCA_IObject* gameobj) :
+SCA_IActuator::SCA_IActuator(SCA_IObject* gameobj, KX_ACTUATOR_TYPE type) :
 	SCA_ILogicBrick(gameobj),
+	m_type(type),
 	m_links(0),
 	m_posevent(false),
 	m_negevent(false)
diff --git a/source/gameengine/GameLogic/SCA_IActuator.h b/source/gameengine/GameLogic/SCA_IActuator.h
index 00ba8c9ce4e..2f1c04192ab 100644
--- a/source/gameengine/GameLogic/SCA_IActuator.h
+++ b/source/gameengine/GameLogic/SCA_IActuator.h
@@ -41,6 +41,7 @@ class SCA_IActuator : public SCA_ILogicBrick
 {
 	friend class SCA_LogicManager;
 protected:
+	int					 m_type;
 	int					 m_links;	// number of active links to controllers
 									// when 0, the actuator is automatically stopped
 	//std::vector<CValue*> m_events;
@@ -60,8 +61,33 @@ public:
 	/**
 	 * This class also inherits the default copy constructors
 	 */
-
-	SCA_IActuator(SCA_IObject* gameobj); 
+	enum KX_ACTUATOR_TYPE {
+		KX_ACT_OBJECT,
+		KX_ACT_IPO,
+		KX_ACT_CAMERA,
+		KX_ACT_SOUND,
+		KX_ACT_PROPERTY,
+		KX_ACT_ADD_OBJECT,
+		KX_ACT_END_OBJECT,
+		KX_ACT_DYNAMIC,
+		KX_ACT_REPLACE_MESH,
+		KX_ACT_TRACKTO,
+		KX_ACT_CONSTRAINT,
+		KX_ACT_SCENE,
+		KX_ACT_RANDOM,
+		KX_ACT_MESSAGE,
+		KX_ACT_ACTION,
+		KX_ACT_CD,
+		KX_ACT_GAME,
+		KX_ACT_VISIBILITY,
+		KX_ACT_2DFILTER,
+		KX_ACT_PARENT,
+		KX_ACT_SHAPEACTION,
+		KX_ACT_STATE,
+		KX_ACT_ARMATURE,
+	};
+
+	SCA_IActuator(SCA_IObject* gameobj, KX_ACTUATOR_TYPE type); 
 
 	/**
 	 * UnlinkObject(...)
@@ -127,7 +153,7 @@ public:
 	void IncLink() { m_links++; }
 	void DecLink();
 	bool IsNoLink() const { return !m_links; }
-	
+	bool IsType(KX_ACTUATOR_TYPE type) { return m_type == type; }
 	
 #ifdef WITH_CXX_GUARDEDALLOC
 public:
diff --git a/source/gameengine/GameLogic/SCA_IObject.cpp b/source/gameengine/GameLogic/SCA_IObject.cpp
index 2bffd029bd4..fbf66b64d08 100644
--- a/source/gameengine/GameLogic/SCA_IObject.cpp
+++ b/source/gameengine/GameLogic/SCA_IObject.cpp
@@ -26,6 +26,7 @@
  * ***** END GPL LICENSE BLOCK *****
  */
 #include <iostream>
+#include <algorithm>
 
 #include "SCA_IObject.h"
 #include "SCA_ISensor.h"
@@ -76,6 +77,12 @@ SCA_IObject::~SCA_IObject()
 		(*ita)->Delete();
 	}
 
+	SCA_ObjectList::iterator ito;
+	for (ito = m_registeredObjects.begin(); !(ito==m_registeredObjects.end()); ++ito)
+	{
+		(*ito)->UnlinkObject(this);
+	}
+
 	//T_InterpolatorList::iterator i;
 	//for (i = m_interpolators.begin(); !(i == m_interpolators.end()); ++i) {
 	//	delete *i;
@@ -123,6 +130,26 @@ void SCA_IObject::UnregisterActuator(SCA_IActuator* act)
 	}
 }
 
+void SCA_IObject::RegisterObject(SCA_IObject* obj)
+{
+	// one object may be registered multiple times via constraint target
+	// store multiple reference, this will serve as registration counter
+	m_registeredObjects.push_back(obj);
+}
+
+void SCA_IObject::UnregisterObject(SCA_IObject* obj)
+{
+	SCA_ObjectList::iterator ito;
+	for (ito = m_registeredObjects.begin(); ito != m_registeredObjects.end(); ++ito)
+	{
+		if ((*ito) == obj) {
+			(*ito) = m_registeredObjects.back();
+			m_registeredObjects.pop_back();
+			break;
+		}
+	}
+}
+
 void SCA_IObject::ReParentLogic()
 {
 	SCA_ActuatorList& oldactuators  = GetActuators();
@@ -165,7 +192,7 @@ void SCA_IObject::ReParentLogic()
 
 	// a new object cannot be client of any actuator
 	m_registeredActuators.clear();
-		
+	m_registeredObjects.clear();
 }
 
 
diff --git a/source/gameengine/GameLogic/SCA_IObject.h b/source/gameengine/GameLogic/SCA_IObject.h
index 3060410dc6b..64ea0a76af1 100644
--- a/source/gameengine/GameLogic/SCA_IObject.h
+++ b/source/gameengine/GameLogic/SCA_IObject.h
@@ -36,6 +36,7 @@
 #include "Value.h"
 #include <vector>
 
+class SCA_IObject;
 class SCA_ISensor;
 class SCA_IController;
 class SCA_IActuator;
@@ -45,6 +46,7 @@ template<class T> T PyVecTo(PyObject*);
 typedef std::vector<SCA_ISensor *>       SCA_SensorList;
 typedef std::vector<SCA_IController *>   SCA_ControllerList;
 typedef std::vector<SCA_IActuator *>     SCA_ActuatorList;
+typedef std::vector<SCA_IObject *>		 SCA_ObjectList;
 
 class SCA_IObject :	public CValue
 {
@@ -59,6 +61,7 @@ protected:
 	SCA_ControllerList     m_controllers;
 	SCA_ActuatorList       m_actuators;
 	SCA_ActuatorList       m_registeredActuators;	// actuators that use a pointer to this object
+	SCA_ObjectList		   m_registeredObjects;		// objects that hold reference to this object
 
 	// SG_Dlist: element of objects with active actuators
 	//           Head: SCA_LogicManager::m_activeActuators
@@ -143,13 +146,22 @@ public:
 	void RegisterActuator(SCA_IActuator* act);
 	void UnregisterActuator(SCA_IActuator* act);
 	
+	void RegisterObject(SCA_IObject* objs);
+	void UnregisterObject(SCA_IObject* objs);
+	/**
+	 * UnlinkObject(...)
+	 * this object is informed that one of the object to which it holds a reference is deleted
+	 * returns true if there was indeed a reference.
+	 */
+	virtual bool UnlinkObject(SCA_IObject* clientobj) { return false; }
+
 	SCA_ISensor* FindSensor(const STR_String& sensorname);
 	SCA_IActuator* FindActuator(const STR_String& actuatorname);
 	SCA_IController* FindController(const STR_String& controllername);
 
 	void SetCurrentTime(float currentTime) {}
 
-	void ReParentLogic();
+	virtual void ReParentLogic();
 	
 	/**
 	 * Set whether or not to ignore activity culling requests
diff --git a/source/gameengine/GameLogic/SCA_PropertyActuator.cpp b/source/gameengine/GameLogic/SCA_PropertyActuator.cpp
index 9446487cdb7..2a3e600a653 100644
--- a/source/gameengine/GameLogic/SCA_PropertyActuator.cpp
+++ b/source/gameengine/GameLogic/SCA_PropertyActuator.cpp
@@ -43,7 +43,7 @@
 /* ------------------------------------------------------------------------- */
 
 SCA_PropertyActuator::SCA_PropertyActuator(SCA_IObject* gameobj,SCA_IObject* sourceObj,const STR_String& propname,const STR_String& expr,int acttype)
-   :	SCA_IActuator(gameobj),
+   :	SCA_IActuator(gameobj, KX_ACT_PROPERTY),
 	m_type(acttype),
 	m_propname(propname),
 	m_exprtxt(expr),
diff --git a/source/gameengine/GameLogic/SCA_RandomActuator.cpp b/source/gameengine/GameLogic/SCA_RandomActuator.cpp
index a3a5cc2303e..8f9482b7826 100644
--- a/source/gameengine/GameLogic/SCA_RandomActuator.cpp
+++ b/source/gameengine/GameLogic/SCA_RandomActuator.cpp
@@ -51,7 +51,7 @@ SCA_RandomActuator::SCA_RandomActuator(SCA_IObject *gameobj,
 									 float para1,
 									 float para2,
 									 const STR_String &propName)
-	: SCA_IActuator(gameobj),
+	: SCA_IActuator(gameobj, KX_ACT_RANDOM),
 	  m_propname(propName),
 	  m_parameter1(para1),
 	  m_parameter2(para2),
diff --git a/source/gameengine/Ketsji/KXNetwork/KX_NetworkMessageActuator.cpp b/source/gameengine/Ketsji/KXNetwork/KX_NetworkMessageActuator.cpp
index 3af8f765251..1cfab87d78b 100644
--- a/source/gameengine/Ketsji/KXNetwork/KX_NetworkMessageActuator.cpp
+++ b/source/gameengine/Ketsji/KXNetwork/KX_NetworkMessageActuator.cpp
@@ -42,7 +42,7 @@ KX_NetworkMessageActuator::KX_NetworkMessageActuator(
 	const STR_String &subject,
 	int bodyType,
 	const STR_String &body) :
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_MESSAGE),
 	m_networkscene(networkscene),
 	m_toPropName(toPropName),
 	m_subject(subject),
diff --git a/source/gameengine/Ketsji/KX_ArmatureSensor.cpp b/source/gameengine/Ketsji/KX_ArmatureSensor.cpp
new file mode 100644
index 00000000000..1b759bda28e
--- /dev/null
+++ b/source/gameengine/Ketsji/KX_ArmatureSensor.cpp
@@ -0,0 +1,205 @@
+/**
+ * Armature sensor
+ *
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include "DNA_action_types.h"
+#include "DNA_constraint_types.h"
+#include "BKE_constraint.h"
+#include "DNA_sensor_types.h"
+
+#include "BL_ArmatureObject.h"
+#include "KX_ArmatureSensor.h"
+#include "SCA_EventManager.h"
+#include "SCA_LogicManager.h"
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+KX_ArmatureSensor::KX_ArmatureSensor(class SCA_EventManager* eventmgr,
+					SCA_IObject* gameobj,
+					const char *posechannel,
+					const char *constraintname,
+					int type,
+					float value)
+	: SCA_ISensor(gameobj,eventmgr),
+	m_constraint(NULL),
+	m_posechannel(posechannel),
+	m_constraintname(constraintname),
+	m_type(type),
+	m_value(value)
+{
+	FindConstraint();
+}
+
+void KX_ArmatureSensor::Init()
+{
+	m_lastresult = m_invert?true:false;
+	m_result = false;
+	m_reset = true;
+}
+
+void KX_ArmatureSensor::FindConstraint()
+{
+	m_constraint = NULL;
+
+	if (m_gameobj->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE) {
+		BL_ArmatureObject* armobj = (BL_ArmatureObject*)m_gameobj;
+		// get the persistent pose structure
+		bPose* pose = armobj->GetOrigPose();
+		bPoseChannel* pchan;
+		bConstraint* pcon;
+		// and locate the constraint
+		for (pchan = (bPoseChannel*)pose->chanbase.first; pchan; pchan=(bPoseChannel*)pchan->next) {
+			if (!strcmp(pchan->name, m_posechannel)) {
+				// now locate the constraint
+				for (pcon = (bConstraint*)pchan->constraints.first; pcon; pcon=(bConstraint*)pcon->next) {
+					if (!strcmp(pcon->name, m_constraintname)) {
+						if (pcon->flag & CONSTRAINT_DISABLE)
+							/* this constraint is not valid, can't use it */
+							break;
+						m_constraint = pcon;
+						break;	
+					}
+				}
+				break;
+			}
+		}
+	}
+}
+
+
+CValue* KX_ArmatureSensor::GetReplica()
+{
+	KX_ArmatureSensor* replica = new KX_ArmatureSensor(*this);
+	// m_range_expr must be recalculated on replica!
+	replica->ProcessReplica();
+	return replica;
+}
+
+void KX_ArmatureSensor::ReParent(SCA_IObject* parent)
+{
+	SCA_ISensor::ReParent(parent);
+	// must remap the constraint
+	FindConstraint();
+}
+
+bool KX_ArmatureSensor::IsPositiveTrigger()
+{
+	return (m_invert) ? !m_result : m_result;
+}
+
+
+KX_ArmatureSensor::~KX_ArmatureSensor()
+{
+}
+
+bool KX_ArmatureSensor::Evaluate()
+{
+	bool reset = m_reset && m_level;
+
+	m_reset = false;
+	if (!m_constraint)
+		return false;
+	switch (m_type) {
+	case SENS_ARM_STATE_CHANGED:
+		m_result = !(m_constraint->flag & CONSTRAINT_OFF);
+		break;
+	case SENS_ARM_LIN_ERROR_BELOW:
+		m_result = (m_constraint->lin_error < m_value);
+		break;
+	case SENS_ARM_LIN_ERROR_ABOVE:
+		m_result = (m_constraint->lin_error > m_value);
+		break;
+	case SENS_ARM_ROT_ERROR_BELOW:
+		m_result = (m_constraint->rot_error < m_value);
+		break;
+	case SENS_ARM_ROT_ERROR_ABOVE:
+		m_result = (m_constraint->rot_error > m_value);
+		break;
+	}
+	if (m_lastresult!=m_result)
+	{
+		m_lastresult = m_result;
+		return true;
+	}
+	return (reset) ? true : false;
+}
+
+
+/* ------------------------------------------------------------------------- */
+/* Python functions                                                          */
+/* ------------------------------------------------------------------------- */
+
+/* Integration hooks ------------------------------------------------------- */
+PyTypeObject KX_ArmatureSensor::Type = {
+	PyVarObject_HEAD_INIT(NULL, 0)
+	"KX_ArmatureSensor",
+	sizeof(PyObjectPlus_Proxy),
+	0,
+	py_base_dealloc,
+	0,
+	0,
+	0,
+	0,
+	py_base_repr,
+	0,0,0,0,0,0,0,0,0,
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
+	0,0,0,0,0,0,0,
+	Methods,
+	0,
+	0,
+	&SCA_ISensor::Type,
+	0,0,0,0,0,0,
+	py_base_new
+};
+
+PyMethodDef KX_ArmatureSensor::Methods[] = {
+	{NULL,NULL} //Sentinel
+};
+
+PyAttributeDef KX_ArmatureSensor::Attributes[] = {
+	KX_PYATTRIBUTE_RO_FUNCTION("constraint", KX_ArmatureSensor, pyattr_get_constraint),
+	KX_PYATTRIBUTE_FLOAT_RW("value",-FLT_MAX,FLT_MAX,KX_ArmatureSensor,m_value),
+	KX_PYATTRIBUTE_INT_RW("type",0,SENS_ARM_MAXTYPE,false,KX_ArmatureSensor,m_type),
+	{ NULL }	//Sentinel
+};
+
+PyObject* KX_ArmatureSensor::pyattr_get_constraint(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
+{
+	KX_ArmatureSensor* sensor = static_cast<KX_ArmatureSensor*>(self);
+	if (sensor->m_gameobj->GetGameObjectType() == SCA_IObject::OBJ_ARMATURE) {
+		BL_ArmatureObject* armobj = (BL_ArmatureObject*)sensor->m_gameobj;
+		BL_ArmatureConstraint* constraint = armobj->GetConstraint(sensor->m_posechannel, sensor->m_constraintname);
+		if (constraint)	
+			return constraint->GetProxy();
+	}
+	Py_RETURN_NONE;
+}
diff --git a/source/gameengine/Ketsji/KX_ArmatureSensor.h b/source/gameengine/Ketsji/KX_ArmatureSensor.h
new file mode 100644
index 00000000000..2af002afff9
--- /dev/null
+++ b/source/gameengine/Ketsji/KX_ArmatureSensor.h
@@ -0,0 +1,85 @@
+/**
+ * Property sensor
+ *
+ * $Id$
+ *
+ * ***** 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): none yet.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#ifndef __KX_ARMATURESENSOR
+#define __KX_ARMATURESENSOR
+
+struct bConstraint;
+
+#include "SCA_ISensor.h"
+#include "DNA_sensor_types.h"
+
+class KX_ArmatureSensor : public SCA_ISensor
+{
+	Py_Header;
+	//class CExpression*	m_rightexpr;
+
+protected:
+
+public:
+	KX_ArmatureSensor(class SCA_EventManager* eventmgr,
+					SCA_IObject* gameobj,
+					const char *posechannel,
+					const char *constraintname,
+					int type,
+					float value);
+	
+	/** 
+	 *  For property sensor, it is used to release the pre-calculated expression
+	 *  so that self references are removed before the sensor itself is released
+	 */
+	virtual ~KX_ArmatureSensor();
+	virtual CValue* GetReplica();
+	virtual void ReParent(SCA_IObject* parent);
+	virtual void Init();
+	virtual bool Evaluate();
+	virtual bool IsPositiveTrigger();
+
+	// identify the constraint that this actuator controls
+	void FindConstraint();
+
+	/* --------------------------------------------------------------------- */
+	/* Python interface ---------------------------------------------------- */
+	/* --------------------------------------------------------------------- */
+	static PyObject* pyattr_get_constraint(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef);
+
+private:
+	struct bConstraint*	m_constraint;
+	STR_String		m_posechannel;
+	STR_String		m_constraintname;
+	int				m_type;
+	float			m_value;
+	bool			m_result;
+	bool			m_lastresult;
+};
+
+#endif
+
diff --git a/source/gameengine/Ketsji/KX_CameraActuator.cpp b/source/gameengine/Ketsji/KX_CameraActuator.cpp
index 9c00b5991af..99618fab8e3 100644
--- a/source/gameengine/Ketsji/KX_CameraActuator.cpp
+++ b/source/gameengine/Ketsji/KX_CameraActuator.cpp
@@ -55,7 +55,7 @@ KX_CameraActuator::KX_CameraActuator(
 	float maxhght,
 	bool  xytog
 ): 
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_CAMERA),
 	m_ob (obj),
 	m_height (hght),
 	m_minHeight (minhght),
diff --git a/source/gameengine/Ketsji/KX_ConstraintActuator.cpp b/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
index 7f1d2c7d53c..ea0b9f4f1e2 100644
--- a/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
@@ -56,7 +56,7 @@ KX_ConstraintActuator::KX_ConstraintActuator(SCA_IObject *gameobj,
 											 int time,
 											 int option,
 											 char *property) :
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_CONSTRAINT),
 	m_refDirVector(refDir),
 	m_currentTime(0)
 {
diff --git a/source/gameengine/Ketsji/KX_GameActuator.cpp b/source/gameengine/Ketsji/KX_GameActuator.cpp
index 42dc4d8fd24..71980a3347e 100644
--- a/source/gameengine/Ketsji/KX_GameActuator.cpp
+++ b/source/gameengine/Ketsji/KX_GameActuator.cpp
@@ -50,7 +50,7 @@ KX_GameActuator::KX_GameActuator(SCA_IObject *gameobj,
 								   const STR_String& loadinganimationname,
 								   KX_Scene* scene,
 								   KX_KetsjiEngine* ketsjiengine)
-								   : SCA_IActuator(gameobj)
+								   : SCA_IActuator(gameobj, KX_ACT_GAME)
 {
 	m_mode = mode;
 	m_filename = filename;
diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index 8193aa8c37b..a02fd9cec1b 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -46,6 +46,7 @@ typedef unsigned long uint_ptr;
 
 
 #define KX_INERTIA_INFINITE 10000
+#include "BLI_arithb.h"
 #include "RAS_IPolygonMaterial.h"
 #include "KX_BlenderMaterial.h"
 #include "KX_GameObject.h"
@@ -454,6 +455,22 @@ double*	KX_GameObject::GetOpenGLMatrix()
 	return fl;
 }
 
+void KX_GameObject::UpdateBlenderObjectMatrix(Object* blendobj)
+{
+	if (!blendobj)
+		blendobj = m_pBlenderObject;
+	if (blendobj) {
+		const MT_Matrix3x3& rot = NodeGetWorldOrientation();
+		const MT_Vector3& scale = NodeGetWorldScaling();
+		const MT_Vector3& pos = NodeGetWorldPosition();
+		rot.getValue(blendobj->obmat[0]);
+		pos.getValue(blendobj->obmat[3]);
+		VecMulf(blendobj->obmat[0], scale[0]);
+		VecMulf(blendobj->obmat[1], scale[1]);
+		VecMulf(blendobj->obmat[2], scale[2]);
+	}
+}
+
 void KX_GameObject::AddMeshUser()
 {
 	for (size_t i=0;i<m_meshes.size();i++)
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index 845cead1cdb..4c4eed9ca71 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -158,6 +158,15 @@ public:
 		return &m_OpenGL_4x4Matrix;
 	};
 
+	/**
+	 * Update the blender object obmat field from the object world position
+	 * if blendobj is NULL, update the object pointed by m_pBlenderObject
+	 * The user must take action to restore the matrix before leaving the GE.
+	 * Used in Armature evaluation
+	 */
+		void
+	UpdateBlenderObjectMatrix(Object* blendobj=NULL);
+
 	/** 
 	 * Get a pointer to the game object that is the parent of 
 	 * this object. Or NULL if there is no parent. The returned
diff --git a/source/gameengine/Ketsji/KX_IpoActuator.cpp b/source/gameengine/Ketsji/KX_IpoActuator.cpp
index b71907be961..d7f800fe5bd 100644
--- a/source/gameengine/Ketsji/KX_IpoActuator.cpp
+++ b/source/gameengine/Ketsji/KX_IpoActuator.cpp
@@ -71,7 +71,7 @@ KX_IpoActuator::KX_IpoActuator(SCA_IObject* gameobj,
 							   bool ipo_as_force,
 							   bool ipo_add,
 							   bool ipo_local)
-	: SCA_IActuator(gameobj),
+	: SCA_IActuator(gameobj, KX_ACT_IPO),
 	m_bNegativeEvent(false),
 	m_startframe (starttime),
 	m_endframe(endtime),
diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
index 4117e493322..e95676ff9d2 100644
--- a/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
+++ b/source/gameengine/Ketsji/KX_KetsjiEngine.cpp
@@ -1783,6 +1783,11 @@ double KX_KetsjiEngine::GetClockTime(void) const
 	return m_clockTime;
 }
 
+double KX_KetsjiEngine::GetFrameTime(void) const
+{
+	return m_frameTime;
+}
+
 double KX_KetsjiEngine::GetRealTime(void) const
 {
 	return m_kxsystem->GetTimeInSeconds();
diff --git a/source/gameengine/Ketsji/KX_KetsjiEngine.h b/source/gameengine/Ketsji/KX_KetsjiEngine.h
index acb9e53df8a..373e8bf218c 100644
--- a/source/gameengine/Ketsji/KX_KetsjiEngine.h
+++ b/source/gameengine/Ketsji/KX_KetsjiEngine.h
@@ -269,6 +269,10 @@ public:
 	 * Returns current render frame clock time
 	 */
 	double GetClockTime(void) const;
+	/**
+	 * Returns current logic frame clock time
+	 */
+	double GetFrameTime(void) const;
 
 	double GetRealTime(void) const;
 	/**
diff --git a/source/gameengine/Ketsji/KX_ObjectActuator.cpp b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
index 2601ced9c38..924e4a47008 100644
--- a/source/gameengine/Ketsji/KX_ObjectActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ObjectActuator.cpp
@@ -55,7 +55,7 @@ KX_ObjectActuator(
 	const short damping,
 	const KX_LocalFlags& flag
 ) : 
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_OBJECT),
 	m_force(force),
 	m_torque(torque),
 	m_dloc(dloc),
diff --git a/source/gameengine/Ketsji/KX_ParentActuator.cpp b/source/gameengine/Ketsji/KX_ParentActuator.cpp
index 20e982f03e0..a4a6b67ad10 100644
--- a/source/gameengine/Ketsji/KX_ParentActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ParentActuator.cpp
@@ -51,7 +51,7 @@ KX_ParentActuator::KX_ParentActuator(SCA_IObject *gameobj,
 									 bool addToCompound,
 									 bool ghost,
 									 SCA_IObject *ob)
-	: SCA_IActuator(gameobj),
+	: SCA_IActuator(gameobj, KX_ACT_PARENT),
 	  m_mode(mode),
 	  m_addToCompound(addToCompound),
 	  m_ghost(ghost),
diff --git a/source/gameengine/Ketsji/KX_PythonInit.cpp b/source/gameengine/Ketsji/KX_PythonInit.cpp
index 298d485aaaf..bef00fb7a06 100644
--- a/source/gameengine/Ketsji/KX_PythonInit.cpp
+++ b/source/gameengine/Ketsji/KX_PythonInit.cpp
@@ -51,6 +51,7 @@ extern "C" {
 #include "KX_KetsjiEngine.h"
 #include "KX_RadarSensor.h"
 #include "KX_RaySensor.h"
+#include "KX_ArmatureSensor.h"
 #include "KX_SceneActuator.h"
 #include "KX_GameActuator.h"
 #include "KX_ParentActuator.h"
@@ -65,6 +66,7 @@ extern "C" {
 #include "KX_SoundActuator.h"
 #include "KX_StateActuator.h"
 #include "BL_ActionActuator.h"
+#include "BL_ArmatureObject.h"
 #include "RAS_IRasterizer.h"
 #include "RAS_ICanvas.h"
 #include "RAS_BucketManager.h"
@@ -1218,6 +1220,53 @@ PyObject* initGameLogic(KX_KetsjiEngine *engine, KX_Scene* scene) // quick hack
 	KX_MACRO_addTypesToDict(d, KX_PARENT_SET, KX_ParentActuator::KX_PARENT_SET);
 	KX_MACRO_addTypesToDict(d, KX_PARENT_REMOVE, KX_ParentActuator::KX_PARENT_REMOVE);
 
+	/* BL_ArmatureConstraint type */
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_TRACKTO, CONSTRAINT_TYPE_TRACKTO);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_KINEMATIC, CONSTRAINT_TYPE_KINEMATIC);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_ROTLIKE, CONSTRAINT_TYPE_ROTLIKE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_LOCLIKE, CONSTRAINT_TYPE_LOCLIKE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_MINMAX, CONSTRAINT_TYPE_MINMAX);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_SIZELIKE, CONSTRAINT_TYPE_SIZELIKE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_LOCKTRACK, CONSTRAINT_TYPE_LOCKTRACK);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_STRETCHTO, CONSTRAINT_TYPE_STRETCHTO);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_CLAMPTO, CONSTRAINT_TYPE_CLAMPTO);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_TRANSFORM, CONSTRAINT_TYPE_TRANSFORM);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_TYPE_DISTLIMIT, CONSTRAINT_TYPE_DISTLIMIT);
+	/* BL_ArmatureConstraint ik_type */
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_COPYPOSE, CONSTRAINT_IK_COPYPOSE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_DISTANCE, CONSTRAINT_IK_DISTANCE);
+	/* BL_ArmatureConstraint ik_mode */
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_MODE_INSIDE, LIMITDIST_INSIDE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_MODE_OUTSIDE, LIMITDIST_OUTSIDE);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_MODE_ONSURFACE, LIMITDIST_ONSURFACE);
+	/* BL_ArmatureConstraint ik_flag */
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_FLAG_TIP, CONSTRAINT_IK_TIP);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_FLAG_ROT, CONSTRAINT_IK_ROT);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_FLAG_STRETCH, CONSTRAINT_IK_STRETCH);
+	KX_MACRO_addTypesToDict(d, CONSTRAINT_IK_FLAG_POS, CONSTRAINT_IK_POS);
+	/* KX_ArmatureSensor type */
+	KX_MACRO_addTypesToDict(d, KX_ARMSENSOR_STATE_CHANGED, SENS_ARM_STATE_CHANGED);
+	KX_MACRO_addTypesToDict(d, KX_ARMSENSOR_LIN_ERROR_BELOW, SENS_ARM_LIN_ERROR_BELOW);
+	KX_MACRO_addTypesToDict(d, KX_ARMSENSOR_LIN_ERROR_ABOVE, SENS_ARM_LIN_ERROR_ABOVE);
+	KX_MACRO_addTypesToDict(d, KX_ARMSENSOR_ROT_ERROR_BELOW, SENS_ARM_ROT_ERROR_BELOW);
+	KX_MACRO_addTypesToDict(d, KX_ARMSENSOR_ROT_ERROR_ABOVE, SENS_ARM_ROT_ERROR_ABOVE);
+
+	/* BL_ArmatureActuator type */
+	KX_MACRO_addTypesToDict(d, KX_ACT_ARMATURE_RUN, ACT_ARM_RUN);
+	KX_MACRO_addTypesToDict(d, KX_ACT_ARMATURE_ENABLE, ACT_ARM_ENABLE);
+	KX_MACRO_addTypesToDict(d, KX_ACT_ARMATURE_DISABLE, ACT_ARM_DISABLE);
+	KX_MACRO_addTypesToDict(d, KX_ACT_ARMATURE_SETTARGET, ACT_ARM_SETTARGET);
+	KX_MACRO_addTypesToDict(d, KX_ACT_ARMATURE_SETWEIGHT, ACT_ARM_SETWEIGHT);
+
+	/* BL_Armature Channel rotation_mode */
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_QUAT, PCHAN_ROT_QUAT);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_XYZ, PCHAN_ROT_XYZ);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_XZY, PCHAN_ROT_XZY);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_YXZ, PCHAN_ROT_YXZ);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_YZX, PCHAN_ROT_YZX);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_ZXY, PCHAN_ROT_ZXY);
+	KX_MACRO_addTypesToDict(d, PCHAN_ROT_ZYX, PCHAN_ROT_ZYX);
+
 	// Check for errors
 	if (PyErr_Occurred())
     {
diff --git a/source/gameengine/Ketsji/KX_PythonInitTypes.cpp b/source/gameengine/Ketsji/KX_PythonInitTypes.cpp
index 61e563791c3..c2c33918172 100644
--- a/source/gameengine/Ketsji/KX_PythonInitTypes.cpp
+++ b/source/gameengine/Ketsji/KX_PythonInitTypes.cpp
@@ -35,6 +35,10 @@
 /* Only for Class::Parents */
 #include "BL_BlenderShader.h"
 #include "BL_ShapeActionActuator.h"
+#include "BL_ArmatureActuator.h"
+#include "BL_ArmatureConstraint.h"
+#include "BL_ArmatureObject.h"
+#include "BL_ArmatureChannel.h"
 #include "KX_BlenderMaterial.h"
 #include "KX_CameraActuator.h"
 #include "KX_ConstraintActuator.h"
@@ -86,7 +90,22 @@
 #include "SCA_RandomActuator.h"
 #include "SCA_IController.h"
 
-static void PyType_Ready_ADD(PyObject *dict, PyTypeObject *tp, PyAttributeDef *attributes, int init_getset)
+static void PyType_Attr_Set(PyGetSetDef *attr_getset, PyAttributeDef *attr)
+{
+	attr_getset->name= (char *)attr->m_name;
+	attr_getset->doc= NULL;
+
+	attr_getset->get= reinterpret_cast<getter>(PyObjectPlus::py_get_attrdef);
+
+	if(attr->m_access==KX_PYATTRIBUTE_RO)
+		attr_getset->set= NULL;
+	else
+		attr_getset->set= reinterpret_cast<setter>(PyObjectPlus::py_set_attrdef);
+
+	attr_getset->closure= reinterpret_cast<void *>(attr);
+}
+
+static void PyType_Ready_ADD(PyObject *dict, PyTypeObject *tp, PyAttributeDef *attributes, PyAttributeDef *attributesPtr, int init_getset)
 {
 	PyAttributeDef *attr;
 
@@ -97,29 +116,31 @@ static void PyType_Ready_ADD(PyObject *dict, PyTypeObject *tp, PyAttributeDef *a
 		//if(tp->tp_base==NULL)
 		//	printf("Debug: No Parents - '%s'\n" , tp->tp_name);
 
-		if(tp->tp_getset==NULL && attributes->m_name) {
+		if(tp->tp_getset==NULL && ((attributes && attributes->m_name) || (attributesPtr && attributesPtr->m_name))) {
 			PyGetSetDef *attr_getset;
 			int attr_tot= 0;
 
-			for(attr= attributes; attr->m_name; attr++, attr_tot++) {};
+			if (attributes) {
+				for(attr= attributes; attr->m_name; attr++, attr_tot++)
+					attr->m_usePtr = false;
+			}
+			if (attributesPtr) {
+				for(attr= attributesPtr; attr->m_name; attr++, attr_tot++)
+					attr->m_usePtr = true;
+			}
 
 			tp->tp_getset = attr_getset = reinterpret_cast<PyGetSetDef *>(PyMem_Malloc((attr_tot+1) * sizeof(PyGetSetDef))); // XXX - Todo, free
 
-
-			for(attr= attributes; attr->m_name; attr++, attr_getset++) {
-				attr_getset->name= (char *)attr->m_name;
-				attr_getset->doc= NULL;
-
-				attr_getset->get= reinterpret_cast<getter>(PyObjectPlus::py_get_attrdef);
-
-				if(attr->m_access==KX_PYATTRIBUTE_RO)
-					attr_getset->set= NULL;
-				else
-					attr_getset->set= reinterpret_cast<setter>(PyObjectPlus::py_set_attrdef);
-
-				attr_getset->closure= reinterpret_cast<void *>(attr);
+			if (attributes) {
+				for(attr= attributes; attr->m_name; attr++, attr_getset++) {
+					PyType_Attr_Set(attr_getset, attr);
+				}
+			}
+			if (attributesPtr) {
+				for(attr= attributesPtr; attr->m_name; attr++, attr_getset++) {
+					PyType_Attr_Set(attr_getset, attr);
+				}
 			}
-
 			memset(attr_getset, 0, sizeof(PyGetSetDef));
 		}
 	} else {
@@ -130,7 +151,8 @@ static void PyType_Ready_ADD(PyObject *dict, PyTypeObject *tp, PyAttributeDef *a
 }
 
 
-#define PyType_Ready_Attr(d, n, i)   PyType_Ready_ADD(d, &n::Type, n::Attributes, i)
+#define PyType_Ready_Attr(d, n, i)   PyType_Ready_ADD(d, &n::Type, n::Attributes, NULL, i)
+#define PyType_Ready_AttrPtr(d, n, i)   PyType_Ready_ADD(d, &n::Type, n::Attributes, n::AttributesPtr, i)
 
 void initPyTypes(void)
 {
@@ -151,6 +173,11 @@ void initPyTypes(void)
 		PyType_Ready_Attr(dict, BL_ActionActuator, init_getset);
 		PyType_Ready_Attr(dict, BL_Shader, init_getset);
 		PyType_Ready_Attr(dict, BL_ShapeActionActuator, init_getset);
+		PyType_Ready_Attr(dict, BL_ArmatureObject, init_getset);
+		PyType_Ready_Attr(dict, BL_ArmatureActuator, init_getset);
+		PyType_Ready_Attr(dict, BL_ArmatureConstraint, init_getset);
+		PyType_Ready_AttrPtr(dict, BL_ArmatureBone, init_getset);
+		PyType_Ready_AttrPtr(dict, BL_ArmatureChannel, init_getset);
 		PyType_Ready_Attr(dict, CListValue, init_getset);
 		PyType_Ready_Attr(dict, CValue, init_getset);
 		PyType_Ready_Attr(dict, KX_BlenderMaterial, init_getset);
diff --git a/source/gameengine/Ketsji/KX_PythonSeq.cpp b/source/gameengine/Ketsji/KX_PythonSeq.cpp
index 75a7c9b8aeb..f7ad7acb252 100644
--- a/source/gameengine/Ketsji/KX_PythonSeq.cpp
+++ b/source/gameengine/Ketsji/KX_PythonSeq.cpp
@@ -31,6 +31,7 @@
 
 #include "KX_PythonSeq.h"
 #include "KX_GameObject.h"
+#include "BL_ArmatureObject.h"
 #include "SCA_ISensor.h"
 #include "SCA_IController.h"
 #include "SCA_IActuator.h"
@@ -72,6 +73,10 @@ static Py_ssize_t KX_PythonSeq_len( PyObject * self )
 		return ((KX_GameObject *)self_plus)->GetControllers().size();
 	case KX_PYGENSEQ_OB_TYPE_ACTUATORS:
 		return ((KX_GameObject *)self_plus)->GetActuators().size();
+	case KX_PYGENSEQ_OB_TYPE_CONSTRAINTS:
+		return ((BL_ArmatureObject *)self_plus)->GetConstraintNumber();
+	case KX_PYGENSEQ_OB_TYPE_CHANNELS:
+		return ((BL_ArmatureObject *)self_plus)->GetChannelNumber();
 	default:
 		/* Should never happen */
 		PyErr_SetString(PyExc_SystemError, "invalid type, internal error");
@@ -139,6 +144,29 @@ static PyObject *KX_PythonSeq_getIndex(PyObject* self, int index)
 			}
 			return linkedactuators[index]->GetProxy();
 		}
+		case KX_PYGENSEQ_OB_TYPE_CONSTRAINTS:
+		{
+			int nb_constraint = ((BL_ArmatureObject *)self_plus)->GetConstraintNumber();
+			if(index<0) 
+				index += nb_constraint;
+			if(index<0 || index>= nb_constraint) {
+				PyErr_SetString(PyExc_IndexError, "seq[i]: index out of range");
+				return NULL;
+			}
+			return ((BL_ArmatureObject *)self_plus)->GetConstraint(index)->GetProxy();
+		}
+		case KX_PYGENSEQ_OB_TYPE_CHANNELS:
+		{
+			int nb_channel = ((BL_ArmatureObject *)self_plus)->GetChannelNumber();
+			if(index<0) 
+				index += nb_channel;
+			if(index<0 || index>= nb_channel) {
+				PyErr_SetString(PyExc_IndexError, "seq[i]: index out of range");
+				return NULL;
+			}
+			return ((BL_ArmatureObject *)self_plus)->GetChannel(index)->GetProxy();
+		}
+
 	}
 	
 	PyErr_SetString(PyExc_SystemError, "invalid sequence type, this is a bug");
@@ -206,6 +234,14 @@ static PyObjectPlus * KX_PythonSeq_subscript__internal(PyObject *self, char *key
 			}
 			break;
 		}
+		case KX_PYGENSEQ_OB_TYPE_CONSTRAINTS:
+		{
+			return ((BL_ArmatureObject*)self_plus)->GetConstraint(key);
+		}
+		case KX_PYGENSEQ_OB_TYPE_CHANNELS:
+		{
+			return ((BL_ArmatureObject*)self_plus)->GetChannel(key);
+		}
 	}
 	
 	return NULL;
diff --git a/source/gameengine/Ketsji/KX_PythonSeq.h b/source/gameengine/Ketsji/KX_PythonSeq.h
index 15a016224a9..34243aa9b9c 100644
--- a/source/gameengine/Ketsji/KX_PythonSeq.h
+++ b/source/gameengine/Ketsji/KX_PythonSeq.h
@@ -39,7 +39,9 @@ enum KX_PYGENSEQ_TYPE {
 	KX_PYGENSEQ_CONT_TYPE_ACTUATORS,
 	KX_PYGENSEQ_OB_TYPE_SENSORS,
 	KX_PYGENSEQ_OB_TYPE_CONTROLLERS,
-	KX_PYGENSEQ_OB_TYPE_ACTUATORS
+	KX_PYGENSEQ_OB_TYPE_ACTUATORS,
+	KX_PYGENSEQ_OB_TYPE_CONSTRAINTS,
+	KX_PYGENSEQ_OB_TYPE_CHANNELS,
 };
 
 /* The Main PyType Object defined in Main.c */
diff --git a/source/gameengine/Ketsji/KX_SCA_AddObjectActuator.cpp b/source/gameengine/Ketsji/KX_SCA_AddObjectActuator.cpp
index 099403fc28d..c1e74070d72 100644
--- a/source/gameengine/Ketsji/KX_SCA_AddObjectActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SCA_AddObjectActuator.cpp
@@ -57,7 +57,7 @@ KX_SCA_AddObjectActuator::KX_SCA_AddObjectActuator(SCA_IObject *gameobj,
 												   const float *angvel,
 												   bool angv_local)
 	: 
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_ADD_OBJECT),
 	m_OriginalObject(original),
 	m_scene(scene),
 	
diff --git a/source/gameengine/Ketsji/KX_SCA_DynamicActuator.cpp b/source/gameengine/Ketsji/KX_SCA_DynamicActuator.cpp
index 646cfb7219f..73c85ad07b3 100644
--- a/source/gameengine/Ketsji/KX_SCA_DynamicActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SCA_DynamicActuator.cpp
@@ -87,7 +87,7 @@ KX_SCA_DynamicActuator::KX_SCA_DynamicActuator(SCA_IObject *gameobj,
 													   short dyn_operation,
 													   float setmass) :
 
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_DYNAMIC),
 	m_dyn_operation(dyn_operation),
 	m_setmass(setmass)
 {
diff --git a/source/gameengine/Ketsji/KX_SCA_EndObjectActuator.cpp b/source/gameengine/Ketsji/KX_SCA_EndObjectActuator.cpp
index dd9d8015724..e7dc71d9b27 100644
--- a/source/gameengine/Ketsji/KX_SCA_EndObjectActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SCA_EndObjectActuator.cpp
@@ -44,7 +44,7 @@
 
 KX_SCA_EndObjectActuator::KX_SCA_EndObjectActuator(SCA_IObject *gameobj,
 												   SCA_IScene* scene):
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_END_OBJECT),
 	m_scene(scene)
 {
     // intentionally empty 
diff --git a/source/gameengine/Ketsji/KX_SCA_ReplaceMeshActuator.cpp b/source/gameengine/Ketsji/KX_SCA_ReplaceMeshActuator.cpp
index e85b8a32798..e118972edc8 100644
--- a/source/gameengine/Ketsji/KX_SCA_ReplaceMeshActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SCA_ReplaceMeshActuator.cpp
@@ -122,7 +122,7 @@ KX_SCA_ReplaceMeshActuator::KX_SCA_ReplaceMeshActuator(SCA_IObject *gameobj,
 													   bool use_gfx,
 													   bool use_phys) :
 
-	SCA_IActuator(gameobj),
+	SCA_IActuator(gameobj, KX_ACT_REPLACE_MESH),
 	m_mesh(mesh),
 	m_scene(scene),
 	m_use_gfx(use_gfx),
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index 3483496c3a6..140be3fb3db 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -40,14 +40,15 @@
 #include "ListValue.h"
 #include "SCA_LogicManager.h"
 #include "SCA_TimeEventManager.h"
-#include "SCA_AlwaysEventManager.h"
-#include "SCA_RandomEventManager.h"
-#include "KX_RayEventManager.h"
+//#include "SCA_AlwaysEventManager.h"
+//#include "SCA_RandomEventManager.h"
+//#include "KX_RayEventManager.h"
 #include "KX_TouchEventManager.h"
 #include "SCA_KeyboardManager.h"
 #include "SCA_MouseManager.h"
-#include "SCA_PropertyEventManager.h"
+//#include "SCA_PropertyEventManager.h"
 #include "SCA_ActuatorEventManager.h"
+#include "SCA_BasicEventManager.h"
 #include "KX_Camera.h"
 #include "SCA_JoystickManager.h"
 
@@ -168,25 +169,27 @@ KX_Scene::KX_Scene(class SCA_IInputDevice* keyboarddevice,
 	m_keyboardmgr = new SCA_KeyboardManager(m_logicmgr,keyboarddevice);
 	m_mousemgr = new SCA_MouseManager(m_logicmgr,mousedevice);
 	
-	SCA_AlwaysEventManager* alwaysmgr = new SCA_AlwaysEventManager(m_logicmgr);
-	SCA_PropertyEventManager* propmgr = new SCA_PropertyEventManager(m_logicmgr);
+	//SCA_AlwaysEventManager* alwaysmgr = new SCA_AlwaysEventManager(m_logicmgr);
+	//SCA_PropertyEventManager* propmgr = new SCA_PropertyEventManager(m_logicmgr);
 	SCA_ActuatorEventManager* actmgr = new SCA_ActuatorEventManager(m_logicmgr);
-	SCA_RandomEventManager* rndmgr = new SCA_RandomEventManager(m_logicmgr);
-	KX_RayEventManager* raymgr = new KX_RayEventManager(m_logicmgr);
+	//SCA_RandomEventManager* rndmgr = new SCA_RandomEventManager(m_logicmgr);
+	SCA_BasicEventManager* basicmgr = new SCA_BasicEventManager(m_logicmgr);
+	//KX_RayEventManager* raymgr = new KX_RayEventManager(m_logicmgr);
 
 	KX_NetworkEventManager* netmgr = new KX_NetworkEventManager(m_logicmgr, ndi);
 	
 	
 
-	m_logicmgr->RegisterEventManager(alwaysmgr);
-	m_logicmgr->RegisterEventManager(propmgr);
+	//m_logicmgr->RegisterEventManager(alwaysmgr);
+	//m_logicmgr->RegisterEventManager(propmgr);
 	m_logicmgr->RegisterEventManager(actmgr);
 	m_logicmgr->RegisterEventManager(m_keyboardmgr);
 	m_logicmgr->RegisterEventManager(m_mousemgr);
 	m_logicmgr->RegisterEventManager(m_timemgr);
-	m_logicmgr->RegisterEventManager(rndmgr);
-	m_logicmgr->RegisterEventManager(raymgr);
+	//m_logicmgr->RegisterEventManager(rndmgr);
+	//m_logicmgr->RegisterEventManager(raymgr);
 	m_logicmgr->RegisterEventManager(netmgr);
+	m_logicmgr->RegisterEventManager(basicmgr);
 
 
 	SYS_SystemHandle hSystem = SYS_GetSystem();
diff --git a/source/gameengine/Ketsji/KX_SceneActuator.cpp b/source/gameengine/Ketsji/KX_SceneActuator.cpp
index ea1be7bca6c..c7721c480e0 100644
--- a/source/gameengine/Ketsji/KX_SceneActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SceneActuator.cpp
@@ -50,7 +50,7 @@ KX_SceneActuator::KX_SceneActuator(SCA_IObject *gameobj,
 								   KX_KetsjiEngine* ketsjiEngine,
 								   const STR_String& nextSceneName,
 								   KX_Camera* camera)
-								   : SCA_IActuator(gameobj)
+								   : SCA_IActuator(gameobj, KX_ACT_SCENE)
 {
 	m_mode = mode;
 	m_scene  = scene;
diff --git a/source/gameengine/Ketsji/KX_SoundActuator.cpp b/source/gameengine/Ketsji/KX_SoundActuator.cpp
index e2b4022a312..9d261137497 100644
--- a/source/gameengine/Ketsji/KX_SoundActuator.cpp
+++ b/source/gameengine/Ketsji/KX_SoundActuator.cpp
@@ -49,7 +49,7 @@ KX_SoundActuator::KX_SoundActuator(SCA_IObject* gameobj,
 								   bool is3d,
 								   KX_3DSoundSettings settings,
 								   KX_SOUNDACT_TYPE type)//,
-								   : SCA_IActuator(gameobj)
+								   : SCA_IActuator(gameobj, KX_ACT_SOUND)
 {
 	m_sound = sound;
 	m_volume = volume;
diff --git a/source/gameengine/Ketsji/KX_StateActuator.cpp b/source/gameengine/Ketsji/KX_StateActuator.cpp
index 4159e9c373d..60812220953 100644
--- a/source/gameengine/Ketsji/KX_StateActuator.cpp
+++ b/source/gameengine/Ketsji/KX_StateActuator.cpp
@@ -40,7 +40,7 @@ KX_StateActuator::KX_StateActuator(
 	int operation,
 	unsigned int mask
 	) 
-	: SCA_IActuator(gameobj),
+	: SCA_IActuator(gameobj, KX_ACT_STATE),
 	  m_operation(operation),
 	  m_mask(mask)
 {
diff --git a/source/gameengine/Ketsji/KX_TrackToActuator.cpp b/source/gameengine/Ketsji/KX_TrackToActuator.cpp
index e6c2f86bbce..ace1cf8a6f4 100644
--- a/source/gameengine/Ketsji/KX_TrackToActuator.cpp
+++ b/source/gameengine/Ketsji/KX_TrackToActuator.cpp
@@ -60,7 +60,7 @@ KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj,
 									   bool allow3D,
 									   int trackflag,
 									   int upflag)
-									   : SCA_IActuator(gameobj)
+									   : SCA_IActuator(gameobj, KX_ACT_TRACKTO)
 {
     m_time = time;
     m_allow3D = allow3D;
diff --git a/source/gameengine/Ketsji/KX_VisibilityActuator.cpp b/source/gameengine/Ketsji/KX_VisibilityActuator.cpp
index 184e127209f..5b0f6e6e9f2 100644
--- a/source/gameengine/Ketsji/KX_VisibilityActuator.cpp
+++ b/source/gameengine/Ketsji/KX_VisibilityActuator.cpp
@@ -41,7 +41,7 @@ KX_VisibilityActuator::KX_VisibilityActuator(
 	bool occlusion,
 	bool recursive
 	) 
-	: SCA_IActuator(gameobj),
+	: SCA_IActuator(gameobj, KX_ACT_VISIBILITY),
 	  m_visible(visible),
 	  m_occlusion(occlusion),
 	  m_recursive(recursive)
diff --git a/source/gameengine/PyDoc/GameTypes.py b/source/gameengine/PyDoc/GameTypes.py
index c391d0c3dec..60511f41c2b 100644
--- a/source/gameengine/PyDoc/GameTypes.py
+++ b/source/gameengine/PyDoc/GameTypes.py
@@ -4,15 +4,17 @@ Documentation for the GameTypes Module.
 
 @group Base: PyObjectPlus, CValue, CPropValue, SCA_ILogicBrick, SCA_IObject, SCA_ISensor, SCA_IController, SCA_IActuator
 
-@group Object: KX_GameObject, KX_LightObject, KX_Camera
+@group Object: KX_GameObject, KX_LightObject, KX_Camera, BL_ArmatureObject
+
+@group Animation: BL_ArmatureConstraint
 
 @group Mesh: KX_MeshProxy, KX_PolyProxy, KX_VertexProxy
 
 @group Shading: KX_PolygonMaterial, KX_BlenderMaterial, BL_Shader
 
-@group Sensors: SCA_ActuatorSensor, SCA_AlwaysSensor, SCA_DelaySensor, SCA_JoystickSensor, SCA_KeyboardSensor, KX_MouseFocusSensor, SCA_MouseSensor, KX_NearSensor, KX_NetworkMessageSensor, SCA_PropertySensor, KX_RadarSensor, SCA_RandomSensor, KX_RaySensor, KX_TouchSensor
+@group Sensors: SCA_ActuatorSensor, SCA_AlwaysSensor, SCA_DelaySensor, SCA_JoystickSensor, SCA_KeyboardSensor, KX_MouseFocusSensor, SCA_MouseSensor, KX_NearSensor, KX_NetworkMessageSensor, SCA_PropertySensor, KX_RadarSensor, SCA_RandomSensor, KX_RaySensor, KX_TouchSensor, KX_ArmatureSensor
 
-@group Actuators: SCA_2DFilterActuator, BL_ActionActuator, KX_SCA_AddObjectActuator, KX_CameraActuator, KX_ConstraintActuator, KX_SCA_DynamicActuator, KX_SCA_EndObjectActuator, KX_GameActuator, KX_IpoActuator, KX_NetworkMessageActuator, KX_ObjectActuator, KX_ParentActuator, SCA_PropertyActuator, SCA_RandomActuator, KX_SCA_ReplaceMeshActuator, KX_SceneActuator, BL_ShapeActionActuator, KX_SoundActuator, KX_StateActuator, KX_TrackToActuator, KX_VisibilityActuator
+@group Actuators: SCA_2DFilterActuator, BL_ActionActuator, BL_ArmatureActuator, KX_SCA_AddObjectActuator, KX_CameraActuator, KX_ConstraintActuator, KX_SCA_DynamicActuator, KX_SCA_EndObjectActuator, KX_GameActuator, KX_IpoActuator, KX_NetworkMessageActuator, KX_ObjectActuator, KX_ParentActuator, SCA_PropertyActuator, SCA_RandomActuator, KX_SCA_ReplaceMeshActuator, KX_SceneActuator, BL_ShapeActionActuator, KX_SoundActuator, KX_StateActuator, KX_TrackToActuator, KX_VisibilityActuator
 
 @group Controllers: SCA_ANDController, SCA_NANDController, SCA_NORController, SCA_ORController, SCA_PythonController, SCA_XNORController, SCA_XORController
 """
@@ -5686,6 +5688,332 @@ class KX_Camera(KX_GameObject):
 		@return: the first object hit or None if no object or object does not match prop
 		"""
 
+class BL_ArmatureObject(KX_GameObject):
+	"""
+	An armature object.
+	
+	@ivar constraints: The list of armature constraint defined on this armature
+	                   Elements of the list can be accessed by index or string.
+	                   The key format for string access is '<bone_name>:<constraint_name>'
+	@type constraints: list of L{BL_ArmatureConstraint}
+	@ivar channels: The list of armature channels. 
+		Elements of the list can be accessed by index or name the bone. 
+	@type channels: list of L{BL_ArmatureChannel}
+	"""
+	
+	def update():
+		"""
+		Ensures that the armature will be updated on next graphic frame.
+		
+		This action is unecessary if a KX_ArmatureActuator with mode run is active
+		or if an action is playing. Use this function in other cases. It must be called
+		on each frame to ensure that the armature is updated continously.
+		"""
+		
+class BL_ArmatureActuator(SCA_IActuator):
+	"""
+	Armature Actuators change constraint condition on armatures.
+	
+	@group Constants: KX_ACT_ARMATURE_RUN, KX_ACT_ARMATURE_ENABLE, KX_ACT_ARMATURE_DISABLE, KX_ACT_ARMATURE_SETTARGET, KX_ACT_ARMATURE_SETWEIGHT
+	@ivar KX_ACT_ARMATURE_RUN: see type
+	@ivar KX_ACT_ARMATURE_ENABLE: see type
+	@ivar KX_ACT_ARMATURE_DISABLE: see type
+	@ivar KX_ACT_ARMATURE_SETTARGET: see type
+	@ivar KX_ACT_ARMATURE_SETWEIGHT: see type
+ 	@ivar type: The type of action that the actuator executes when it is active.
+
+				KX_ACT_ARMATURE_RUN(0): just make sure the armature will be updated on the next graphic frame
+				This is the only persistent mode of the actuator: it executes automatically once per frame until stopped by a controller
+
+				KX_ACT_ARMATURE_ENABLE(1): enable the constraint.
+
+				KX_ACT_ARMATURE_DISABLE(2): disable the constraint (runtime constraint values are not updated).
+
+				KX_ACT_ARMATURE_SETTARGET(3): change target and subtarget of constraint
+
+				KX_ACT_ARMATURE_SETWEIGHT(4): change weight of (only for IK constraint)
+	@type type: integer
+	@ivar constraint: The constraint object this actuator is controlling.
+	@type constraint: L{BL_ArmatureConstraint}
+	@ivar target: The object that this actuator will set as primary target to the constraint it controls
+	@type target: L{KX_GameObject}
+	@ivar subtarget: The object that this actuator will set as secondary target to the constraint it controls.
+	                 Currently, the only secondary target is the pole target for IK constraint.
+	@type subtarget: L{KX_GameObject}
+	@ivar weight: The weight this actuator will set on the constraint it controls.
+	              Currently only the IK constraint has a weight. It must be a value between 0 and 1.
+	              A weight of 0 disables a constraint while still updating constraint runtime values (see L{BL_ArmatureConstraint})
+	@type weight: float
+	"""
+
+class KX_ArmatureSensor(SCA_ISensor):
+	"""
+	Armature sensor detect conditions on armatures.
+	
+	@group Constants: KX_ARMSENSOR_STATE_CHANGED, KX_ARMSENSOR_LIN_ERROR_BELOW, KX_ARMSENSOR_LIN_ERROR_ABOVE, KX_ARMSENSOR_ROT_ERROR_BELOW, KX_ARMSENSOR_ROT_ERROR_ABOVE
+	@ivar KX_ARMSENSOR_STATE_CHANGED: see type
+ 	@ivar KX_ARMSENSOR_LIN_ERROR_BELOW: see type
+	@ivar KX_ARMSENSOR_LIN_ERROR_ABOVE: see type
+	@ivar KX_ARMSENSOR_ROT_ERROR_BELOW: see type
+	@ivar KX_ARMSENSOR_ROT_ERROR_ABOVE: see type
+	@ivar type: The type of measurement that the sensor make when it is active.
+
+				KX_ARMSENSOR_STATE_CHANGED(0): detect that the constraint is changing state (active/inactive)
+
+				KX_ARMSENSOR_LIN_ERROR_BELOW(1): detect that the constraint linear error is above a threshold
+
+				KX_ARMSENSOR_LIN_ERROR_ABOVE(2): detect that the constraint linear error is below a threshold
+
+				KX_ARMSENSOR_ROT_ERROR_BELOW(3): detect that the constraint rotation error is above a threshold
+
+				KX_ARMSENSOR_ROT_ERROR_ABOVE(4): detect that the constraint rotation error is below a threshold
+	@type type: integer
+	@ivar constraint: The constraint object this sensor is watching.
+	@type constraint: L{BL_ArmatureConstraint}
+	@ivar value: The threshold used in the comparison with the constraint error
+	             The linear error is only updated on CopyPose/Distance IK constraint with iTaSC solver
+	             The rotation error is only updated on CopyPose+rotation IK constraint with iTaSC solver
+	             The linear error on CopyPose is always >= 0: it is the norm of the distance between the target and the bone
+	             The rotation error on CopyPose is always >= 0: it is the norm of the equivalent rotation vector between the bone and the target orientations
+	             The linear error on Distance can be positive if the distance between the bone and the target is greater than the desired distance, and negative if the distance is smaller
+	@type value: float
+	"""
+
+class BL_ArmatureConstraint(PyObjectPlus):
+	"""
+	Proxy to Armature Constraint. Allows to change constraint on the fly.
+	Obtained through L{BL_ArmatureObject}.constraints.
+	Note: not all armature constraints are supported in the GE.
+	
+	@group Constants: CONSTRAINT_TYPE_TRACKTO, CONSTRAINT_TYPE_KINEMATIC, CONSTRAINT_TYPE_ROTLIKE, CONSTRAINT_TYPE_LOCLIKE, CONSTRAINT_TYPE_MINMAX, CONSTRAINT_TYPE_SIZELIKE, CONSTRAINT_TYPE_LOCKTRACK, CONSTRAINT_TYPE_STRETCHTO, CONSTRAINT_TYPE_CLAMPTO, CONSTRAINT_TYPE_TRANSFORM, CONSTRAINT_TYPE_DISTLIMIT,CONSTRAINT_IK_COPYPOSE, CONSTRAINT_IK_DISTANCE,CONSTRAINT_IK_MODE_INSIDE, CONSTRAINT_IK_MODE_OUTSIDE,CONSTRAINT_IK_MODE_ONSURFACE,CONSTRAINT_IK_FLAG_TIP,CONSTRAINT_IK_FLAG_ROT, CONSTRAINT_IK_FLAG_STRETCH, CONSTRAINT_IK_FLAG_POS
+	@ivar CONSTRAINT_TYPE_TRACKTO: see type
+	@ivar CONSTRAINT_TYPE_KINEMATIC: see type
+	@ivar CONSTRAINT_TYPE_ROTLIKE: see type
+	@ivar CONSTRAINT_TYPE_LOCLIKE: see type
+	@ivar CONSTRAINT_TYPE_MINMAX: see type
+	@ivar CONSTRAINT_TYPE_SIZELIKE: see type
+	@ivar CONSTRAINT_TYPE_LOCKTRACK: see type
+	@ivar CONSTRAINT_TYPE_STRETCHTO: see type
+	@ivar CONSTRAINT_TYPE_CLAMPTO: see type
+	@ivar CONSTRAINT_TYPE_TRANSFORM: see type
+	@ivar CONSTRAINT_TYPE_DISTLIMIT: see type
+	@ivar CONSTRAINT_IK_COPYPOSE: see ik_type
+	@ivar CONSTRAINT_IK_DISTANCE: see ik_type
+	@ivar CONSTRAINT_IK_MODE_INSIDE: see ik_mode
+	@ivar CONSTRAINT_IK_MODE_OUTSIDE: see ik_mode
+	@ivar CONSTRAINT_IK_MODE_ONSURFACE: see ik_mode
+	@ivar CONSTRAINT_IK_FLAG_TIP: see ik_flag
+	@ivar CONSTRAINT_IK_FLAG_ROT: see ik_flag
+	@ivar CONSTRAINT_IK_FLAG_STRETCH: see ik_flag
+	@ivar CONSTRAINT_IK_FLAG_POS: see ik_flag
+	@ivar type: Type of constraint, read-only
+	@type type: integer, one of CONSTRAINT_TYPE_ constant
+	@ivar name: Name of constraint constructed as <bone_name>:<constraint_name>
+	            This name is also the key subscript on L{BL_ArmatureObject}.constraints list
+	@type name: string
+	@ivar enforce: fraction of constraint effect that is enforced. Between 0 and 1.
+	@type enforce: float
+	@ivar headtail: position of target between head and tail of the target bone: 0=head, 1=tail
+	                Only used if the target is a bone (i.e target object is an armature)
+	@type headtail: float
+	@ivar lin_error: runtime linear error (in Blender unit) on constraint at the current frame.
+	                 This is a runtime value updated on each frame by the IK solver. Only available on IK constraint and iTaSC solver.
+	@type lin_error: float
+	@ivar rot_error: runtime rotation error (in radiant) on constraint at the current frame.
+	                 This is a runtime value updated on each frame by the IK solver. Only available on IK constraint and iTaSC solver.
+	                 It is only set if the constraint has a rotation part, for example, a CopyPose+Rotation IK constraint.
+	@type rot_error: float
+	@ivar target: Primary target object for the constraint. The position of this object in the GE will be used as target for the constraint.
+	@type target: L{KX_GameObject}
+	@ivar subtarget: Secondary target object for the constraint. The position of this object in the GE will be used as secondary target for the constraint.
+	                 Currently this is only used for pole target on IK constraint.
+	@type subtarget: L{KX_GameObject}
+	@ivar active: True if the constraint is active.
+	              Note: an inactive constraint does not update lin_error and rot_error.
+	@type active: boolean
+	@ivar ik_weight: Weight of the IK constraint between 0 and 1. 
+	                 Only defined for IK constraint.
+	@type ik_weight: float
+	@ivar ik_type: Type of IK constraint, read-only
+
+				   CONSTRAINT_IK_COPYPOSE(0): constraint is trying to match the position and eventually the rotation of the target.
+
+				   CONSTRAINT_IK_DISTANCE(1): constraint is maintaining a certain distance to target subject to ik_mode
+	@type ik_type: integer
+	@ivar ik_flag: Combination of IK constraint option flags, read-only
+
+				   CONSTRAINT_IK_FLAG_TIP(1) : set when the constraint operates on the head of the bone and not the tail
+
+				   CONSTRAINT_IK_FLAG_ROT(2) : set when the constraint tries to match the orientation of the target
+
+				   CONSTRAINT_IK_FLAG_STRETCH(16) : set when the armature is allowed to stretch (only the bones with stretch factor > 0.0)
+
+				   CONSTRAINT_IK_FLAG_POS(32) : set when the constraint tries to match the position of the target
+	@type ik_flag: integer
+	@ivar ik_dist: Distance the constraint is trying to maintain with target, only used when ik_type=CONSTRAINT_IK_DISTANCE
+	@type ik_dist: float
+	@ivar ik_mode: Additional mode for IK constraint. Currently only used for Distance constraint:
+
+				   CONSTRAINT_IK_MODE_INSIDE(0) : the constraint tries to keep the bone within ik_dist of target
+
+				   CONSTRAINT_IK_MODE_OUTSIDE(1) : the constraint tries to keep the bone outside ik_dist of the target
+
+				   CONSTRAINT_IK_MODE_ONSURFACE(2) : the constraint tries to keep the bone exactly at ik_dist of the target
+	@type ik_mode: integer
+	"""
+
+class BL_ArmatureChannel(PyObjectPlus):
+	"""
+	Proxy to armature pose channel. Allows to read and set armature pose.
+	The attributes are identical to RNA attributes, but mostly in read-only mode.
+	
+	@group Constants: PCHAN_ROT_QUAT, PCHAN_ROT_XYZ, PCHAN_ROT_XZY, PCHAN_ROT_YXZ, PCHAN_ROT_YZX, PCHAN_ROT_ZXY, PCHAN_ROT_ZYX
+	@ivar PCHAN_ROT_QUAT: see rotation_mode
+	@ivar PCHAN_ROT_XYZ: see rotation_mode
+	@ivar PCHAN_ROT_XZY: see rotation_mode
+	@ivar PCHAN_ROT_YXZ: see rotation_mode
+	@ivar PCHAN_ROT_YZX: see rotation_mode
+	@ivar PCHAN_ROT_ZXY: see rotation_mode
+	@ivar PCHAN_ROT_ZYX: see rotation_mode
+	@ivar name: channel name (=bone name), read-only.
+	@type name: string
+	@ivar bone: return the bone object corresponding to this pose channel, read-only.
+	@type bone: L{BL_ArmatureBone}
+	@ivar parent: return the parent channel object, None if root channel, read-only.
+	@type parent: L{BL_ArmatureChannel}
+	@ivar has_ik: true if the bone is part of an active IK chain, read-only.
+		This flag is not set when an IK constraint is defined but not enabled (miss target information for example)
+	@type has_ik: boolean
+	@ivar ik_dof_x: true if the bone is free to rotation in the X axis, read-only.
+	@type ik_dof_x: boolean
+	@ivar ik_dof_y: true if the bone is free to rotation in the Y axis, read-only.
+	@type ik_dof_y: boolean
+	@ivar ik_dof_z: true if the bone is free to rotation in the Z axis, read-only.
+	@type ik_dof_z: boolean
+	@ivar ik_limit_x: true if a limit is imposed on X rotation, read-only.
+	@type ik_limit_x: boolean
+	@ivar ik_limit_y: true if a limit is imposed on Y rotation, read-only.
+	@type ik_limit_y: boolean
+	@ivar ik_limit_z: true if a limit is imposed on Z rotation, read-only.
+	@type ik_limit_z: boolean
+	@ivar ik_rot_control: true if channel rotation should applied as IK constraint, read-only.
+	@type ik_rot_control: boolean
+	@ivar ik_lin_control: true if channel size should applied as IK constraint, read-only.
+	@type ik_lin_control: boolean
+	@ivar location: displacement of the bone head in armature local space, read-write.
+		You can only move a bone if it is unconnected to its parent. An action playing on the armature may change the value. An IK chain does not update this value, see joint_rotation.
+		Changing this field has no immediate effect, the pose is updated when the armature is updated during the graphic render (see L{BL_ArmatureObject.update})
+	@type location: vector [X,Y,Z]
+	@ivar scale: scale of the bone relative to its parent, read-write.
+		An action playing on the armature may change the value.  An IK chain does not update this value, see joint_rotation.
+		Changing this field has no immediate effect, the pose is updated when the armature is updated during the graphic render (see L{BL_ArmatureObject.update})
+	@type scale: vector [sizeX, sizeY, sizeZ]
+	@ivar rotation: rotation of the bone relative to its parent expressed as a quaternion, read-write.
+		This field is only used if rotation_mode is 0. An action playing on the armature may change the value.  An IK chain does not update this value, see joint_rotation.
+		Changing this field has no immediate effect, the pose is updated when the armature is updated during the graphic render (see L{BL_ArmatureObject.update})
+	@type rotation: vector [qr, qi, qj, qk]
+	@ivar euler_rotation: rotation of the bone relative to its parent expressed as a set of euler angles, read-write.
+		This field is only used if rotation_mode is > 0. You must always pass the angles in [X,Y,Z] order; the order of applying the angles to the bone depends on rotation_mode. An action playing on the armature may change this field.  An IK chain does not update this value, see joint_rotation.
+		Changing this field has no immediate effect, the pose is updated when the armature is updated during the graphic render (see L{BL_ArmatureObject.update})
+	@type euler_rotation: vector [X, Y, Z]
+	@ivar rotation_mode: method of updating the bone rotation, read-write.
+		Use the following constants (euler mode are named as in BLender UI but the actual axis order is reversed):
+		  - PCHAN_ROT_QUAT(0) : use quaternioin in rotation attribute to update bone rotation
+		  - PCHAN_ROT_XYZ(1) : use euler_rotation and apply angles on bone's Z, Y, X axis successively
+		  - PCHAN_ROT_XZY(2) : use euler_rotation and apply angles on bone's Y, Z, X axis successively
+		  - PCHAN_ROT_YXZ(3) : use euler_rotation and apply angles on bone's Z, X, Y axis successively
+		  - PCHAN_ROT_YZX(4) : use euler_rotation and apply angles on bone's X, Z, Y axis successively
+		  - PCHAN_ROT_ZXY(5) : use euler_rotation and apply angles on bone's Y, X, Z axis successively
+		  - PCHAN_ROT_ZYX(6) : use euler_rotation and apply angles on bone's X, Y, Z axis successively
+	@type rotation_mode: integer
+	@ivar channel_matrix: pose matrix in bone space (deformation of the bone due to action, constraint, etc), Read-only.
+		This field is updated after the graphic render, it represents the current pose.
+	@type channel_matrix: matrix [4][4]
+	@ivar pose_matrix: pose matrix in armature space, read-only,
+		This field is updated after the graphic render, it represents the current pose.
+	@type pose_matrix: matrix [4][4]
+	@ivar pose_head: position of bone head in armature space, read-only.
+	@type pose_head: vector [x, y, z]
+	@ivar pose_tail: position of bone tail in armature space, read-only.
+	@type pose_tail: vector [x, y, z]
+	@ivar ik_min_x: minimum value of X rotation in degree (<= 0) when X rotation is limited (see ik_limit_x), read-only.
+	@type ik_min_x: float
+	@ivar ik_max_x: maximum value of X rotation in degree (>= 0) when X rotation is limited (see ik_limit_x), read-only.
+	@type ik_max_x: float
+	@ivar ik_min_y: minimum value of Y rotation in degree (<= 0) when Y rotation is limited (see ik_limit_y), read-only.
+	@type ik_min_y: float
+	@ivar ik_max_y: maximum value of Y rotation in degree (>= 0) when Y rotation is limited (see ik_limit_y), read-only.
+	@type ik_max_y: float
+	@ivar ik_min_z: minimum value of Z rotation in degree (<= 0) when Z rotation is limited (see ik_limit_z), read-only.
+	@type ik_min_z: float
+	@ivar ik_max_z: maximum value of Z rotation in degree (>= 0) when Z rotation is limited (see ik_limit_z), read-only.
+	@type ik_max_z: float
+	@ivar ik_stiffness_x: bone rotation stiffness in X axis, read-only
+	@type ik_stiffness_x: float between 0 and 1
+	@ivar ik_stiffness_y: bone rotation stiffness in Y axis, read-only
+	@type ik_stiffness_y: float between 0 and 1
+	@ivar ik_stiffness_z: bone rotation stiffness in Z axis, read-only
+	@type ik_stiffness_z: float between 0 and 1
+	@ivar ik_stretch: ratio of scale change that is allowed, 0=bone can't change size, read-only.
+	@type ik_stretch: float
+	@ivar ik_rot_weight: weight of rotation constraint when ik_rot_control is set, read-write.
+	@type ik_rot_weight: float between 0 and 1
+	@ivar ik_lin_weight: weight of size constraint when ik_lin_control is set, read-write.
+	@type ik_lin_weight: float between 0 and 1
+	@ivar joint_rotation: control bone rotation in term of joint angle (for robotic applications), read-write.
+		When writing to this attribute, you pass a [x, y, z] vector and an appropriate set of euler angles or quaternion is calculated according to the rotation_mode.
+		When you read this attribute, the current pose matrix is converted into a [x, y, z] vector representing the joint angles.
+		The value and the meaning of the x, y, z depends on the ik_dof_ attributes:
+		  - 1DoF joint X, Y or Z: the corresponding x, y, or z value is used an a joint angle in radiant
+		  - 2DoF joint X+Y or Z+Y: treated as 2 successive 1DoF joints: first X or Z, then Y. The x or z value is used as a joint angle in radiant along the X or Z axis, followed by a rotation along the new Y axis of y radiants.
+		  - 2DoF joint X+Z: treated as a 2DoF joint with rotation axis on the X/Z plane. The x and z values are used as the coordinates of the rotation vector in the X/Z plane.
+		  - 3DoF joint X+Y+Z: treated as a revolute joint. The [x,y,z] vector represents the equivalent rotation vector to bring the joint from the rest pose to the new pose.
+
+		Notes:
+		  - The bone must be part of an IK chain if you want to set the ik_dof_ attributes via the UI, but this will interfere with this attribute since the IK solver will overwrite the pose. You can stay in control of the armature if you create an IK constraint but do not finalize it (e.g. don't set a target): the IK solver will not run but the IK panel will show up on the UI for each bone in the chain.
+		  - [0,0,0] always corresponds to the rest pose. 
+		  - You must request the armature pose to update and wait for the next graphic frame to see the effect of setting this attribute (see L{BL_ArmatureObject.update}). 
+		  - You can read the result of the calculation in rotation or euler_rotation attributes after setting this attribute.
+	@type joint_rotation: vector [x, y, z]
+	"""
+	
+class BL_ArmatureBone(PyObjectPlus):
+	"""
+	Proxy to Blender bone structure. All fields are read-only and comply to RNA names.
+	All space attribute correspond to the rest pose.
+	
+	@ivar name: bone name
+	@type name: string
+	@ivar connected: true when the bone head is struck to the parent's tail
+	@type connected: boolean
+	@ivar hinge: true when bone doesn't inherit rotation or scale from parent bone
+	@type hinge: boolean
+	@ivar inherit_scale: true when bone inherits scaling from parent bone
+	@type inherit_scale: boolean
+	@ivar bbone_segments: number of B-bone segments
+	@type bbone_segments: integer
+	@ivar roll: bone rotation around head-tail axis
+	@type roll: float
+	@ivar head: location of head end of the bone in parent bone space
+	@type head: vector [x, y, z]
+	@ivar tail: location of head end of the bone in parent bone space
+	@type tail: vector [x, y, z]
+	@ivar length: bone length
+	@type length: float
+	@ivar arm_head: location of head end of the bone in armature space
+	@type arm_head: vector [x, y, z]
+	@ivar arm_tail: location of tail end of the bone in armature space
+	@type arm_tail: vector [x, y, z]
+	@ivar arm_mat: matrix of the bone head in armature space
+		This matrix has no scale part.
+	@type arm_mat: matrix [4][4]
+	@ivar bone_mat: rotation matrix of the bone in parent bone space.
+	@type bone_mat: matrix [3][3]
+	@ivar parent: parent bone, or None for root bone
+	@type parent: L{BL_ArmatureBone}
+	@ivar children: list of bone's children
+	@type children: list of L{BL_ArmatureBone}
+	"""
 # Util func to extract all attrs
 """
 import types
diff --git a/source/gameengine/SceneGraph/SG_DList.h b/source/gameengine/SceneGraph/SG_DList.h
index 3e17fb55dc0..97951792ab5 100644
--- a/source/gameengine/SceneGraph/SG_DList.h
+++ b/source/gameengine/SceneGraph/SG_DList.h
@@ -89,6 +89,46 @@ public:
 		}
 	};
 
+	template<typename T> class const_iterator
+	{
+	private:
+		const SG_DList&	m_head;
+		const T*		m_current;
+	public:
+		typedef const_iterator<T> _myT;
+		const_iterator(const SG_DList& head) : m_head(head), m_current(NULL) {}
+		~const_iterator() {}
+
+		void begin()
+		{
+			m_current = (const T*)m_head.Peek();
+		}
+		void back()
+		{
+			m_current = (const T*)m_head.Back();
+		}
+		bool end()
+		{
+			return (m_current == (const T*)m_head.Self());
+		}
+		const T* operator*()
+		{
+			return m_current;
+		}
+		_myT& operator++()
+		{
+			// no check of NULL! make sure you don't try to increment beyond end
+			m_current = (const T*)m_current->Peek();
+			return *this;
+		}
+		_myT& operator--()
+		{
+			// no check of NULL! make sure you don't try to increment beyond end
+			m_current = (const T*)m_current->Back();
+			return *this;
+		}
+	};
+
     SG_DList() 
     { 
         m_flink = m_blink = this; 
@@ -159,6 +199,18 @@ public:
     { 
         return this; 
     }
+    inline const SG_DList *Peek() const			// Look at front without removing
+    { 
+        return (const SG_DList*)m_flink; 
+    }  
+    inline const SG_DList *Back() const			// Look at front without removing
+    { 
+        return (const SG_DList*)m_blink; 
+    }  
+    inline const SG_DList *Self() const 
+    { 
+        return this; 
+    }
 	
 	
 #ifdef WITH_CXX_GUARDEDALLOC
@@ -168,5 +220,32 @@ public:
 #endif
 };
 
+/**
+ * SG_DListHead : Template class that implements copy constructor to duplicate list automatically
+ *                The elements of the list must have themselves a copy constructor.
+ */
+template<typename T> class SG_DListHead : public SG_DList
+{
+public:
+	typedef SG_DListHead<T> _myT;
+	SG_DListHead() : SG_DList() {}
+	SG_DListHead(const _myT& other) : SG_DList()
+	{
+		// copy the list, assuming objects of type T
+		const_iterator<T> eit(other);
+		T* elem;
+		for (eit.begin(); !eit.end(); ++eit) {
+			elem = (*eit)->GetReplica();
+			AddBack(elem);
+		}
+	}
+	virtual ~SG_DListHead() {}
+    T* Remove()
+    {
+		return static_cast<T*>(SG_DList::Remove());
+    }
+
+};
+
 #endif //__SG_DLIST