Remove unused Bullet module and thirdparty code

It has been disabled in `master` since one year (#45852) and our plan
is for Bullet, and possibly other thirdparty physics engines, to be
implemented via GDExtension so that they can be selected by the users
who need them.

15 files changed, 0 insertions, 4281 deletions

diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp
deleted file mode 100644
index b662b80152..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp
+++ /dev/null
@@ -1,107 +0,0 @@
-///@file Configuration for Inverse Dynamics Library,
-///	  such as choice of linear algebra library and underlying scalar type
-#ifndef IDCONFIG_HPP_
-#define IDCONFIG_HPP_
-
-// If true, enable jacobian calculations.
-// This adds a 3xN matrix to every body, + 2 3-Vectors.
-// so it is not advised for large systems if it is not absolutely necessary.
-// Also, this is not required for standard inverse dynamics calculations.
-// Will only work with vector math libraries that support 3xN matrices.
-#define BT_ID_WITH_JACOBIANS
-
-// If we have a custom configuration, compile without using other parts of bullet.
-#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
-#include <cmath>
-#define BT_ID_WO_BULLET
-#define BT_ID_SQRT(x) std::sqrt(x)
-#define BT_ID_FABS(x) std::fabs(x)
-#define BT_ID_COS(x) std::cos(x)
-#define BT_ID_SIN(x) std::sin(x)
-#define BT_ID_ATAN2(x, y) std::atan2(x, y)
-#define BT_ID_POW(x, y) std::pow(x, y)
-#define BT_ID_SNPRINTF snprintf
-#define BT_ID_PI M_PI
-#define BT_ID_USE_DOUBLE_PRECISION
-#else
-#define BT_ID_SQRT(x) btSqrt(x)
-#define BT_ID_FABS(x) btFabs(x)
-#define BT_ID_COS(x) btCos(x)
-#define BT_ID_SIN(x) btSin(x)
-#define BT_ID_ATAN2(x, y) btAtan2(x, y)
-#define BT_ID_POW(x, y) btPow(x, y)
-#define BT_ID_PI SIMD_PI
-#ifdef _WIN32
-#define BT_ID_SNPRINTF _snprintf
-#else
-#define BT_ID_SNPRINTF snprintf
-#endif  //
-#endif
-// error messages
-#include "IDErrorMessages.hpp"
-
-#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
-/*
-#include "IDConfigEigen.hpp"
-#include "IDConfigBuiltin.hpp"
-*/
-#define INVDYN_INCLUDE_HELPER_2(x) #x
-#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
-#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
-#ifndef btInverseDynamics
-#error "custom inverse dynamics config, but no custom namespace defined"
-#endif
-
-#define BT_ID_MAX(a, b) std::max(a, b)
-#define BT_ID_MIN(a, b) std::min(a, b)
-
-#else
-#define btInverseDynamics btInverseDynamicsBullet3
-// Use default configuration with bullet's types
-// Use the same scalar type as rest of bullet library
-#include "LinearMath/btScalar.h"
-typedef btScalar idScalar;
-#include "LinearMath/btMinMax.h"
-#define BT_ID_MAX(a, b) btMax(a, b)
-#define BT_ID_MIN(a, b) btMin(a, b)
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define BT_ID_USE_DOUBLE_PRECISION
-#endif
-
-#ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-// use bullet types for arrays and array indices
-#include "Bullet3Common/b3AlignedObjectArray.h"
-// this is to make it work with C++2003, otherwise we could do this:
-// template <typename T>
-// using idArray = b3AlignedObjectArray<T>;
-template <typename T>
-struct idArray
-{
-	typedef b3AlignedObjectArray<T> type;
-};
-typedef int idArrayIdx;
-#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR()
-
-#else  // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-#include "LinearMath/btAlignedObjectArray.h"
-template <typename T>
-struct idArray
-{
-	typedef btAlignedObjectArray<T> type;
-};
-typedef int idArrayIdx;
-#define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR()
-
-#endif  // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-// use bullet's allocator functions
-#define idMalloc btAllocFunc
-#define idFree btFreeFunc
-
-#define ID_LINEAR_MATH_USE_BULLET
-#include "details/IDLinearMathInterface.hpp"
-#endif
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp
deleted file mode 100644
index 6392367924..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp
+++ /dev/null
@@ -1,38 +0,0 @@
-///@file Configuration for Inverse Dynamics Library without external dependencies
-#ifndef INVDYNCONFIG_BUILTIN_HPP_
-#define INVDYNCONFIG_BUILTIN_HPP_
-#define btInverseDynamics btInverseDynamicsBuiltin
-#ifdef BT_USE_DOUBLE_PRECISION
-// choose double/single precision version
-typedef double idScalar;
-#else
-typedef float idScalar;
-#endif
-// use std::vector for arrays
-#include <vector>
-// this is to make it work with C++2003, otherwise we could do this
-// template <typename T>
-// using idArray = std::vector<T>;
-template <typename T>
-struct idArray
-{
-	typedef std::vector<T> type;
-};
-typedef std::vector<int>::size_type idArrayIdx;
-// default to standard malloc/free
-#include <cstdlib>
-#define idMalloc ::malloc
-#define idFree ::free
-// currently not aligned at all...
-#define ID_DECLARE_ALIGNED_ALLOCATOR()                                                     \
-	inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); }   \
-	inline void operator delete(void* ptr) { idFree(ptr); }                                \
-	inline void* operator new(std::size_t, void* ptr) { return ptr; }                      \
-	inline void operator delete(void*, void*) {}                                           \
-	inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
-	inline void operator delete[](void* ptr) { idFree(ptr); }                              \
-	inline void* operator new[](std::size_t, void* ptr) { return ptr; }                    \
-	inline void operator delete[](void*, void*) {}
-
-#include "details/IDMatVec.hpp"
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp b/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp
deleted file mode 100644
index cfb308ee55..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-///@file Configuration for Inverse Dynamics Library with Eigen
-#ifndef INVDYNCONFIG_EIGEN_HPP_
-#define INVDYNCONFIG_EIGEN_HPP_
-#define btInverseDynamics btInverseDynamicsEigen
-#ifdef BT_USE_DOUBLE_PRECISION
-// choose double/single precision version
-typedef double idScalar;
-#else
-typedef float idScalar;
-#endif
-
-// use std::vector for arrays
-#include <vector>
-// this is to make it work with C++2003, otherwise we could do this
-// template <typename T>
-// using idArray = std::vector<T>;
-template <typename T>
-struct idArray
-{
-	typedef std::vector<T> type;
-};
-typedef std::vector<int>::size_type idArrayIdx;
-// default to standard malloc/free
-#include <cstdlib>
-#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-// Note on interfaces:
-// Eigen::Matrix has data(), to get c-array storage
-// HOWEVER: default storage is column-major!
-#define ID_LINEAR_MATH_USE_EIGEN
-#include "Eigen/Eigen"
-#include "details/IDEigenInterface.hpp"
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp b/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp
deleted file mode 100644
index 5a98f01498..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp
+++ /dev/null
@@ -1,31 +0,0 @@
-///@file error message utility functions
-#ifndef IDUTILS_HPP_
-#define IDUTILS_HPP_
-#include <cstring>
-/// name of file being compiled, without leading path components
-#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
-
-#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
-#include "Bullet3Common/b3Logging.h"
-#define bt_id_error_message(...) b3Error(__VA_ARGS__)
-#define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
-#define id_printf(...) b3Printf(__VA_ARGS__)
-#else  // BT_ID_WO_BULLET
-#include <cstdio>
-/// print error message with file/line information
-#define bt_id_error_message(...)                                             \
-	do                                                                       \
-	{                                                                        \
-		fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
-		fprintf(stderr, __VA_ARGS__);                                        \
-	} while (0)
-/// print warning message with file/line information
-#define bt_id_warning_message(...)                                             \
-	do                                                                         \
-	{                                                                          \
-		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
-		fprintf(stderr, __VA_ARGS__);                                          \
-	} while (0)
-#define id_printf(...) printf(__VA_ARGS__)
-#endif  // BT_ID_WO_BULLET
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp
deleted file mode 100644
index 2f120ed489..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDMath.cpp
+++ /dev/null
@@ -1,510 +0,0 @@
-#include "IDMath.hpp"
-
-#include <cmath>
-#include <limits>
-
-namespace btInverseDynamics
-{
-static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
-// requirements for axis length deviation from 1.0
-// experimentally set from random euler angle rotation matrices
-static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
-
-void setZero(vec3 &v)
-{
-	v(0) = 0;
-	v(1) = 0;
-	v(2) = 0;
-}
-
-void setZero(vecx &v)
-{
-	for (int i = 0; i < v.size(); i++)
-	{
-		v(i) = 0;
-	}
-}
-
-void setZero(mat33 &m)
-{
-	m(0, 0) = 0;
-	m(0, 1) = 0;
-	m(0, 2) = 0;
-	m(1, 0) = 0;
-	m(1, 1) = 0;
-	m(1, 2) = 0;
-	m(2, 0) = 0;
-	m(2, 1) = 0;
-	m(2, 2) = 0;
-}
-
-void skew(vec3 &v, mat33 *result)
-{
-	(*result)(0, 0) = 0.0;
-	(*result)(0, 1) = -v(2);
-	(*result)(0, 2) = v(1);
-	(*result)(1, 0) = v(2);
-	(*result)(1, 1) = 0.0;
-	(*result)(1, 2) = -v(0);
-	(*result)(2, 0) = -v(1);
-	(*result)(2, 1) = v(0);
-	(*result)(2, 2) = 0.0;
-}
-
-idScalar maxAbs(const vecx &v)
-{
-	idScalar result = 0.0;
-	for (int i = 0; i < v.size(); i++)
-	{
-		const idScalar tmp = BT_ID_FABS(v(i));
-		if (tmp > result)
-		{
-			result = tmp;
-		}
-	}
-	return result;
-}
-
-idScalar maxAbs(const vec3 &v)
-{
-	idScalar result = 0.0;
-	for (int i = 0; i < 3; i++)
-	{
-		const idScalar tmp = BT_ID_FABS(v(i));
-		if (tmp > result)
-		{
-			result = tmp;
-		}
-	}
-	return result;
-}
-
-#if (defined BT_ID_HAVE_MAT3X)
-idScalar maxAbsMat3x(const mat3x &m)
-{
-	// only used for tests -- so just loop here for portability
-	idScalar result = 0.0;
-	for (idArrayIdx col = 0; col < m.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			result = BT_ID_MAX(result, std::fabs(m(row, col)));
-		}
-	}
-	return result;
-}
-
-void mul(const mat33 &a, const mat3x &b, mat3x *result)
-{
-	if (b.cols() != result->cols())
-	{
-		bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
-							static_cast<int>(b.cols()), static_cast<int>(result->cols()));
-		abort();
-	}
-
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col);
-		const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col);
-		const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col);
-		setMat3xElem(0, col, x, result);
-		setMat3xElem(1, col, y, result);
-		setMat3xElem(2, col, z, result);
-	}
-}
-void add(const mat3x &a, const mat3x &b, mat3x *result)
-{
-	if (a.cols() != b.cols())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
-							static_cast<int>(a.cols()), static_cast<int>(b.cols()));
-		abort();
-	}
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			setMat3xElem(row, col, a(row, col) + b(row, col), result);
-		}
-	}
-}
-void sub(const mat3x &a, const mat3x &b, mat3x *result)
-{
-	if (a.cols() != b.cols())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
-							static_cast<int>(a.cols()), static_cast<int>(b.cols()));
-		abort();
-	}
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			setMat3xElem(row, col, a(row, col) - b(row, col), result);
-		}
-	}
-}
-#endif
-
-mat33 transformX(const idScalar &alpha)
-{
-	mat33 T;
-	const idScalar cos_alpha = BT_ID_COS(alpha);
-	const idScalar sin_alpha = BT_ID_SIN(alpha);
-	// [1  0 0]
-	// [0  c s]
-	// [0 -s c]
-	T(0, 0) = 1.0;
-	T(0, 1) = 0.0;
-	T(0, 2) = 0.0;
-
-	T(1, 0) = 0.0;
-	T(1, 1) = cos_alpha;
-	T(1, 2) = sin_alpha;
-
-	T(2, 0) = 0.0;
-	T(2, 1) = -sin_alpha;
-	T(2, 2) = cos_alpha;
-
-	return T;
-}
-
-mat33 transformY(const idScalar &beta)
-{
-	mat33 T;
-	const idScalar cos_beta = BT_ID_COS(beta);
-	const idScalar sin_beta = BT_ID_SIN(beta);
-	// [c 0 -s]
-	// [0 1  0]
-	// [s 0  c]
-	T(0, 0) = cos_beta;
-	T(0, 1) = 0.0;
-	T(0, 2) = -sin_beta;
-
-	T(1, 0) = 0.0;
-	T(1, 1) = 1.0;
-	T(1, 2) = 0.0;
-
-	T(2, 0) = sin_beta;
-	T(2, 1) = 0.0;
-	T(2, 2) = cos_beta;
-
-	return T;
-}
-
-mat33 transformZ(const idScalar &gamma)
-{
-	mat33 T;
-	const idScalar cos_gamma = BT_ID_COS(gamma);
-	const idScalar sin_gamma = BT_ID_SIN(gamma);
-	// [ c s 0]
-	// [-s c 0]
-	// [ 0 0 1]
-	T(0, 0) = cos_gamma;
-	T(0, 1) = sin_gamma;
-	T(0, 2) = 0.0;
-
-	T(1, 0) = -sin_gamma;
-	T(1, 1) = cos_gamma;
-	T(1, 2) = 0.0;
-
-	T(2, 0) = 0.0;
-	T(2, 1) = 0.0;
-	T(2, 2) = 1.0;
-
-	return T;
-}
-
-mat33 tildeOperator(const vec3 &v)
-{
-	mat33 m;
-	m(0, 0) = 0.0;
-	m(0, 1) = -v(2);
-	m(0, 2) = v(1);
-	m(1, 0) = v(2);
-	m(1, 1) = 0.0;
-	m(1, 2) = -v(0);
-	m(2, 0) = -v(1);
-	m(2, 1) = v(0);
-	m(2, 2) = 0.0;
-	return m;
-}
-
-void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T)
-{
-	const idScalar sa = BT_ID_SIN(alpha);
-	const idScalar ca = BT_ID_COS(alpha);
-	const idScalar st = BT_ID_SIN(theta);
-	const idScalar ct = BT_ID_COS(theta);
-
-	(*r)(0) = a;
-	(*r)(1) = -sa * d;
-	(*r)(2) = ca * d;
-
-	(*T)(0, 0) = ct;
-	(*T)(0, 1) = -st;
-	(*T)(0, 2) = 0.0;
-
-	(*T)(1, 0) = st * ca;
-	(*T)(1, 1) = ct * ca;
-	(*T)(1, 2) = -sa;
-
-	(*T)(2, 0) = st * sa;
-	(*T)(2, 1) = ct * sa;
-	(*T)(2, 2) = ca;
-}
-
-void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
-{
-	const idScalar c = BT_ID_COS(angle);
-	const idScalar s = -BT_ID_SIN(angle);
-	const idScalar one_m_c = 1.0 - c;
-
-	const idScalar &x = axis(0);
-	const idScalar &y = axis(1);
-	const idScalar &z = axis(2);
-
-	(*T)(0, 0) = x * x * one_m_c + c;
-	(*T)(0, 1) = x * y * one_m_c - z * s;
-	(*T)(0, 2) = x * z * one_m_c + y * s;
-
-	(*T)(1, 0) = x * y * one_m_c + z * s;
-	(*T)(1, 1) = y * y * one_m_c + c;
-	(*T)(1, 2) = y * z * one_m_c - x * s;
-
-	(*T)(2, 0) = x * z * one_m_c - y * s;
-	(*T)(2, 1) = y * z * one_m_c + x * s;
-	(*T)(2, 2) = z * z * one_m_c + c;
-}
-
-bool isPositiveDefinite(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) <= 0)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
-	{
-		return false;
-	}
-	return true;
-}
-
-bool isPositiveSemiDefinite(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) < 0)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
-	{
-		return false;
-	}
-	return true;
-}
-
-bool isPositiveSemiDefiniteFuzzy(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) < -kIsZero)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero)
-	{
-		return false;
-	}
-	return true;
-}
-
-idScalar determinant(const mat33 &m)
-{
-	return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
-		   m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
-}
-
-bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
-{
-	// TODO(Thomas) do we really want this?
-	//			  in cases where the inertia tensor about the center of mass is zero,
-	//			  the determinant of the inertia tensor about the joint axis is almost
-	//			  zero and can have a very small negative value.
-	if (!isPositiveSemiDefiniteFuzzy(I))
-	{
-		bt_id_error_message(
-			"invalid inertia matrix for body %d, not positive definite "
-			"(fixed joint)\n",
-			index);
-		bt_id_error_message(
-			"matrix is:\n"
-			"[%.20e %.20e %.20e;\n"
-			"%.20e %.20e %.20e;\n"
-			"%.20e %.20e %.20e]\n",
-			I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-			I(2, 2));
-
-		return false;
-	}
-
-	// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
-	if (!has_fixed_joint)
-	{
-		if (I(0, 0) + I(1, 1) < I(2, 2))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-		if (I(0, 0) + I(1, 1) < I(2, 2))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-		if (I(1, 1) + I(2, 2) < I(0, 0))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-	}
-	// check positive/zero diagonal elements
-	for (int i = 0; i < 3; i++)
-	{
-		if (I(i, i) < 0)
-		{  // accept zero
-			bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
-			return false;
-		}
-	}
-	// check symmetry
-	if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero)
-	{
-		bt_id_error_message(
-			"invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
-			"%e\n",
-			index, I(1, 0) - I(0, 1));
-		return false;
-	}
-	if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero)
-	{
-		bt_id_error_message(
-			"invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
-			"%e\n",
-			index, I(2, 0) - I(0, 2));
-		return false;
-	}
-	if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero)
-	{
-		bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
-							I(1, 2) - I(2, 1));
-		return false;
-	}
-	return true;
-}
-
-bool isValidTransformMatrix(const mat33 &m)
-{
-#define print_mat(x)                                                                                   \
-	bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
-						x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
-
-	// check for unit length column vectors
-	for (int i = 0; i < 3; i++)
-	{
-		const idScalar length_minus_1 =
-			BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
-		if (length_minus_1 > kAxisLengthEpsilon)
-		{
-			bt_id_error_message(
-				"Not a valid rotation matrix (column %d not unit length)\n"
-				"column = [%.18e %.18e %.18e]\n"
-				"length-1.0= %.18e\n",
-				i, m(0, i), m(1, i), m(2, i), length_minus_1);
-			print_mat(m);
-			return false;
-		}
-	}
-	// check for orthogonal column vectors
-	if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	// check determinant (rotation not reflection)
-	if (determinant(m) <= 0)
-	{
-		bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
-		print_mat(m);
-		return false;
-	}
-	return true;
-}
-
-bool isUnitVector(const vec3 &vector)
-{
-	return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
-		   kIsZero;
-}
-
-vec3 rpyFromMatrix(const mat33 &rot)
-{
-	vec3 rpy;
-	rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
-	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
-	rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
-	return rpy;
-}
-}  // namespace btInverseDynamics
diff --git a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp b/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp
deleted file mode 100644
index 40bee5302b..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/IDMath.hpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/// @file Math utility functions used in inverse dynamics library.
-///	   Defined here as they may not be provided by the math library.
-
-#ifndef IDMATH_HPP_
-#define IDMATH_HPP_
-#include "IDConfig.hpp"
-
-namespace btInverseDynamics
-{
-/// set all elements to zero
-void setZero(vec3& v);
-/// set all elements to zero
-void setZero(vecx& v);
-/// set all elements to zero
-void setZero(mat33& m);
-/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a)
-void skew(vec3& v, mat33* result);
-/// return maximum absolute value
-idScalar maxAbs(const vecx& v);
-#ifndef ID_LINEAR_MATH_USE_EIGEN
-/// return maximum absolute value
-idScalar maxAbs(const vec3& v);
-#endif  //ID_LINEAR_MATH_USE_EIGEN
-
-#if (defined BT_ID_HAVE_MAT3X)
-idScalar maxAbsMat3x(const mat3x& m);
-void setZero(mat3x& m);
-// define math functions on mat3x here to avoid allocations in operators.
-void mul(const mat33& a, const mat3x& b, mat3x* result);
-void add(const mat3x& a, const mat3x& b, mat3x* result);
-void sub(const mat3x& a, const mat3x& b, mat3x* result);
-#endif
-
-/// get offset vector & transform matrix from DH parameters
-/// TODO: add documentation
-void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);
-
-/// Check if a 3x3 matrix is positive definite
-/// @param m a 3x3 matrix
-/// @return true if m>0, false otherwise
-bool isPositiveDefinite(const mat33& m);
-
-/// Check if a 3x3 matrix is positive semi definite
-/// @param m a 3x3 matrix
-/// @return true if m>=0, false otherwise
-bool isPositiveSemiDefinite(const mat33& m);
-/// Check if a 3x3 matrix is positive semi definite within numeric limits
-/// @param m a 3x3 matrix
-/// @return true if m>=-eps, false otherwise
-bool isPositiveSemiDefiniteFuzzy(const mat33& m);
-
-/// Determinant of 3x3 matrix
-/// NOTE: implemented here for portability, as determinant operation
-///	   will be implemented differently for various matrix/vector libraries
-/// @param m a 3x3 matrix
-/// @return det(m)
-idScalar determinant(const mat33& m);
-
-/// Test if a 3x3 matrix satisfies some properties of inertia matrices
-/// @param I a 3x3 matrix
-/// @param index body index (for error messages)
-/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted
-/// @return true if I satisfies inertia matrix properties, false otherwise.
-bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint);
-
-/// Check if a 3x3 matrix is a valid transform (rotation) matrix
-/// @param m a 3x3 matrix
-/// @return true if m is a rotation matrix, false otherwise
-bool isValidTransformMatrix(const mat33& m);
-/// Transform matrix from parent to child frame,
-/// when the child frame is rotated about @param axis by @angle
-/// (mathematically positive)
-/// @param axis the axis of rotation
-/// @param angle rotation angle
-/// @param T pointer to transform matrix
-void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T);
-
-/// Check if this is a unit vector
-/// @param vector
-/// @return true if |vector|=1 within numeric limits
-bool isUnitVector(const vec3& vector);
-
-/// @input a vector in R^3
-/// @returns corresponding spin tensor
-mat33 tildeOperator(const vec3& v);
-/// @param alpha angle in radians
-/// @returns transform matrix for ratation with @param alpha about x-axis
-mat33 transformX(const idScalar& alpha);
-/// @param beta angle in radians
-/// @returns transform matrix for ratation with @param beta about y-axis
-mat33 transformY(const idScalar& beta);
-/// @param gamma angle in radians
-/// @returns transform matrix for ratation with @param gamma about z-axis
-mat33 transformZ(const idScalar& gamma);
-///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
-/// @param rot rotation matrix
-/// @returns x-y-z Euler angles
-vec3 rpyFromMatrix(const mat33& rot);
-}  // namespace btInverseDynamics
-#endif  // IDMATH_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
deleted file mode 100644
index 9326b0d098..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp
+++ /dev/null
@@ -1,548 +0,0 @@
-#include "MultiBodyTree.hpp"
-
-#include <cmath>
-#include <limits>
-#include <vector>
-
-#include "IDMath.hpp"
-#include "details/MultiBodyTreeImpl.hpp"
-#include "details/MultiBodyTreeInitCache.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::MultiBodyTree()
-	: m_is_finalized(false),
-	  m_mass_parameters_are_valid(true),
-	  m_accept_invalid_mass_parameters(false),
-	  m_impl(0x0),
-	  m_init_cache(0x0)
-{
-	m_init_cache = new InitCache();
-}
-
-MultiBodyTree::~MultiBodyTree()
-{
-	delete m_impl;
-	delete m_init_cache;
-}
-
-void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
-{
-	m_accept_invalid_mass_parameters = flag;
-}
-
-bool MultiBodyTree::getAcceptInvalidMassProperties() const
-{
-	return m_accept_invalid_mass_parameters;
-}
-
-int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
-{
-	return m_impl->getBodyOrigin(body_index, world_origin);
-}
-
-int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
-{
-	return m_impl->getBodyCoM(body_index, world_com);
-}
-
-int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
-{
-	return m_impl->getBodyTransform(body_index, world_T_body);
-}
-int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
-{
-	return m_impl->getBodyAngularVelocity(body_index, world_omega);
-}
-int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
-{
-	return m_impl->getBodyLinearVelocity(body_index, world_velocity);
-}
-
-int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
-{
-	return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
-}
-
-int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
-{
-	return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
-}
-int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
-{
-	return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
-}
-
-int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
-{
-	return m_impl->getParentRParentBodyRef(body_index, r);
-}
-
-int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
-{
-	return m_impl->getBodyTParentRef(body_index, T);
-}
-
-int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
-{
-	return m_impl->getBodyAxisOfMotion(body_index, axis);
-}
-
-void MultiBodyTree::printTree() { m_impl->printTree(); }
-void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
-
-int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
-
-int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
-
-int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
-											vecx *joint_forces)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
-	{
-		bt_id_error_message("error in inverse dynamics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
-									   const bool initialize_matrix,
-									   const bool set_lower_triangular_matrix, matxx *mass_matrix)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 ==
-		m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
-									set_lower_triangular_matrix, mass_matrix))
-	{
-		bt_id_error_message("error in mass matrix calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
-{
-	return calculateMassMatrix(q, true, true, true, mass_matrix);
-}
-
-int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
-{
-	vec3 world_gravity(m_impl->m_world_gravity);
-	// temporarily set gravity to zero, to ensure we get the actual accelerations
-	setZero(m_impl->m_world_gravity);
-
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, u, dot_u,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-
-	m_impl->m_world_gravity = world_gravity;
-	return 0;
-}
-
-int MultiBodyTree::calculatePositionKinematics(const vecx &q)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, q, q,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, u, u,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateJacobians(q, u,
-										 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in jacobian calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateJacobians(const vecx &q)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateJacobians(q, q,
-										 MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
-	{
-		bt_id_error_message("error in jacobian calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
-{
-	return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
-}
-
-int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
-{
-	return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
-}
-
-int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
-{
-	return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
-}
-
-int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
-{
-	return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
-}
-
-#endif
-
-int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
-						   const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
-						   const vec3 &body_axis_of_motion_, idScalar mass,
-						   const vec3 &body_r_body_com, const mat33 &body_I_body,
-						   const int user_int, void *user_ptr)
-{
-	if (body_index < 0)
-	{
-		bt_id_error_message("body index must be positive (got %d)\n", body_index);
-		return -1;
-	}
-	vec3 body_axis_of_motion(body_axis_of_motion_);
-	switch (joint_type)
-	{
-		case REVOLUTE:
-		case PRISMATIC:
-			// check if axis is unit vector
-			if (!isUnitVector(body_axis_of_motion))
-			{
-				bt_id_warning_message(
-					"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
-					body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
-				idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
-											 BT_ID_POW(body_axis_of_motion(1), 2) +
-											 BT_ID_POW(body_axis_of_motion(2), 2));
-				if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min()))
-				{
-					bt_id_error_message("axis of motion vector too short (%e)\n", length);
-					return -1;
-				}
-				body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
-			}
-			break;
-		case FIXED:
-			break;
-		case FLOATING:
-			break;
-		case SPHERICAL:
-			break;
-		default:
-			bt_id_error_message("unknown joint type %d\n", joint_type);
-			return -1;
-	}
-
-	// sanity check for mass properties. Zero mass is OK.
-	if (mass < 0)
-	{
-		m_mass_parameters_are_valid = false;
-		bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
-		if (!m_accept_invalid_mass_parameters)
-		{
-			return -1;
-		}
-	}
-
-	if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
-	{
-		m_mass_parameters_are_valid = false;
-		// error message printed in function call
-		if (!m_accept_invalid_mass_parameters)
-		{
-			return -1;
-		}
-	}
-
-	if (!isValidTransformMatrix(body_T_parent_ref))
-	{
-		return -1;
-	}
-
-	return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref,
-								 body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com,
-								 body_I_body, user_int, user_ptr);
-}
-
-int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
-{
-	return m_impl->getParentIndex(body_index, parent_index);
-}
-
-int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
-{
-	return m_impl->getUserInt(body_index, user_int);
-}
-
-int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
-{
-	return m_impl->getUserPtr(body_index, user_ptr);
-}
-
-int MultiBodyTree::setUserInt(const int body_index, const int user_int)
-{
-	return m_impl->setUserInt(body_index, user_int);
-}
-
-int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
-{
-	return m_impl->setUserPtr(body_index, user_ptr);
-}
-
-int MultiBodyTree::finalize()
-{
-	const int &num_bodies = m_init_cache->numBodies();
-	const int &num_dofs = m_init_cache->numDoFs();
-
-	if (num_dofs < 0)
-	{
-		bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
-		//return -1;
-	}
-
-	// 1 allocate internal MultiBody structure
-	m_impl = new MultiBodyImpl(num_bodies, num_dofs);
-
-	// 2 build new index set assuring index(parent) < index(child)
-	if (-1 == m_init_cache->buildIndexSets())
-	{
-		return -1;
-	}
-	m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
-
-	// 3 setup internal kinematic and dynamic data
-	for (int index = 0; index < num_bodies; index++)
-	{
-		InertiaData inertia;
-		JointData joint;
-		if (-1 == m_init_cache->getInertiaData(index, &inertia))
-		{
-			return -1;
-		}
-		if (-1 == m_init_cache->getJointData(index, &joint))
-		{
-			return -1;
-		}
-
-		RigidBody &rigid_body = m_impl->m_body_list[index];
-
-		rigid_body.m_mass = inertia.m_mass;
-		rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
-		rigid_body.m_body_I_body = inertia.m_body_I_body;
-		rigid_body.m_joint_type = joint.m_type;
-		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
-		rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref;
-		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
-		rigid_body.m_joint_type = joint.m_type;
-
-		int user_int;
-		if (-1 == m_init_cache->getUserInt(index, &user_int))
-		{
-			return -1;
-		}
-		if (-1 == m_impl->setUserInt(index, user_int))
-		{
-			return -1;
-		}
-
-		void *user_ptr;
-		if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
-		{
-			return -1;
-		}
-		if (-1 == m_impl->setUserPtr(index, user_ptr))
-		{
-			return -1;
-		}
-
-		// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
-		// matrices.
-		switch (rigid_body.m_joint_type)
-		{
-			case REVOLUTE:
-				rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
-				rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
-				rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2);
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case PRISMATIC:
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0);
-				rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1);
-				rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
-				break;
-			case FIXED:
-				// NOTE/TODO: dimension really should be zero ..
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case SPHERICAL:
-				// NOTE/TODO: this is not really correct.
-				// the Jacobians should be 3x3 matrices here !
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case FLOATING:
-				// NOTE/TODO: this is not really correct.
-				// the Jacobians should be 3x3 matrices here !
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			default:
-				bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
-				return -1;
-		}
-	}
-
-	// 4 assign degree of freedom indices & build per-joint-type index arrays
-	if (-1 == m_impl->generateIndexSets())
-	{
-		bt_id_error_message("generating index sets\n");
-		return -1;
-	}
-
-	// 5 do some pre-computations ..
-	m_impl->calculateStaticData();
-
-	// 6. make sure all user forces are set to zero, as this might not happen
-	//	in the vector ctors.
-	m_impl->clearAllUserForcesAndMoments();
-
-	m_is_finalized = true;
-	return 0;
-}
-
-int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
-{
-	return m_impl->setGravityInWorldFrame(gravity);
-}
-
-int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
-{
-	return m_impl->getJointType(body_index, joint_type);
-}
-
-int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
-{
-	return m_impl->getJointTypeStr(body_index, joint_type);
-}
-
-int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
-{
-	return m_impl->getDoFOffset(body_index, q_offset);
-}
-
-int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
-{
-	return m_impl->setBodyMass(body_index, mass);
-}
-
-int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
-{
-	return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
-}
-
-int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
-{
-	return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
-}
-
-int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
-{
-	return m_impl->getBodyMass(body_index, mass);
-}
-
-int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
-{
-	return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
-}
-
-int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
-{
-	return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
-}
-
-void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
-
-int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
-{
-	return m_impl->addUserForce(body_index, body_force);
-}
-
-int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
-{
-	return m_impl->addUserMoment(body_index, body_moment);
-}
-
-}  // namespace btInverseDynamics
diff --git a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp b/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp
deleted file mode 100644
index 7b852f976c..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp
+++ /dev/null
@@ -1,367 +0,0 @@
-#ifndef MULTIBODYTREE_HPP_
-#define MULTIBODYTREE_HPP_
-
-#include "IDConfig.hpp"
-#include "IDMath.hpp"
-
-namespace btInverseDynamics
-{
-/// Enumeration of supported joint types
-enum JointType
-{
-	/// no degree of freedom, moves with parent
-	FIXED = 0,
-	/// one rotational degree of freedom relative to parent
-	REVOLUTE,
-	/// one translational degree of freedom relative to parent
-	PRISMATIC,
-	/// six degrees of freedom relative to parent
-	FLOATING,
-	/// three degrees of freedom, relative to parent
-	SPHERICAL
-};
-
-/// Interface class for calculating inverse dynamics for tree structured
-/// multibody systems
-///
-/// Note on degrees of freedom
-/// The q vector contains the generalized coordinate set defining the tree's configuration.
-/// Every joint adds elements that define the corresponding link's frame pose relative to
-/// its parent. For the joint types that is:
-///	- FIXED:	 none
-///	- REVOLUTE:  angle of rotation [rad]
-///	- PRISMATIC: displacement [m]
-///	- FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
-///				 (in that order)
-/// - SPHERICAL: Euler x-y-z angles [rad]
-/// The u vector contains the generalized speeds, which are
-///	- FIXED:	 none
-///	- REVOLUTE:  time derivative of angle of rotation [rad/s]
-///	- PRISMATIC: time derivative of displacement [m/s]
-///	- FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
-///				 and time derivative of displacement in parent frame [m/s]
-//	- SPHERICAL:  angular velocity [rad/s]
-///
-/// The q and u vectors are obtained by stacking contributions of all bodies in one
-/// vector in the order of body indices.
-///
-/// Note on generalized forces: analogous to u, i.e.,
-///	 - FIXED:	 none
-///	 - REVOLUTE:  moment [Nm], about joint axis
-///	 - PRISMATIC: force  [N], along joint axis
-///	 - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
-///				  (in that order)
-///  - SPHERICAL: moment vector [Nm] 
-/// TODO - force element interface (friction, springs, dampers, etc)
-///	  - gears and motor inertia
-class MultiBodyTree
-{
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	/// The contructor.
-	/// Initialization & allocation is via addBody and buildSystem calls.
-	MultiBodyTree();
-	/// the destructor. This also deallocates all memory
-	~MultiBodyTree();
-
-	/// Add body to the system. this allocates memory and not real-time safe.
-	/// This only adds the data to an initial cache. After all bodies have been
-	/// added,
-	/// the system is setup using the buildSystem call
-	/// @param body_index index of the body to be added. Must >=0, <number of bodies,
-	///		and index of parent must be < index of body
-	/// @param parent_index index of the parent body
-	///		The root of the tree has index 0 and its parent (the world frame)
-	///		is assigned index -1
-	///		the rotation and translation relative to the parent are taken as
-	///		pose of the root body relative to the world frame. Other parameters
-	///		are ignored
-	/// @param JointType type of joint connecting the body to the parent
-	/// @param mass the mass of the body
-	/// @param body_r_body_com the center of mass of the body relative to and
-	/// described in
-	///		the body fixed frame, which is located in the joint axis connecting
-	/// the body to its parent
-	/// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
-	/// frame
-	///		(ie, the reference point is the origin of the body-fixed frame and
-	/// the matrix is written
-	///		 w.r.t. those unit vectors)
-	/// @param parent_r_parent_body_ref position of joint relative to the parent
-	/// body's reference frame
-	///		for q=0, written in the parent bodies reference frame
-	/// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
-	///		Ignored for joints that are not revolute or prismatic.
-	///		must be a unit vector.
-	/// @param body_T_parent_ref transform matrix from parent to body reference
-	/// frame for q=0.
-	///		This is the matrix transforming a vector represented in the
-	/// parent's reference frame into one represented
-	///		in this body's reference frame.
-	///		ie, if parent_vec is a vector in R^3 whose components are w.r.t to
-	/// the parent's reference frame,
-	///		then the same vector written w.r.t. this body's frame (for q=0) is
-	/// given by
-	///		body_vec = parent_R_body_ref * parent_vec
-	/// @param user_ptr pointer to user data
-	/// @param user_int pointer to user integer
-	/// @return 0 on success, -1 on error
-	int addBody(int body_index, int parent_index, JointType joint_type,
-				const vec3& parent_r_parent_body_ref, const mat33& body_T_parent_ref,
-				const vec3& body_axis_of_motion, idScalar mass, const vec3& body_r_body_com,
-				const mat33& body_I_body, const int user_int, void* user_ptr);
-	/// set policy for invalid mass properties
-	/// @param flag if true, invalid mass properties are accepted,
-	///		the default is false
-	void setAcceptInvalidMassParameters(bool flag);
-	/// @return the mass properties policy flag
-	bool getAcceptInvalidMassProperties() const;
-	/// build internal data structures
-	/// call this after all bodies have been added via addBody
-	/// @return 0 on success, -1 on error
-	int finalize();
-	/// pretty print ascii description of tree to stdout
-	void printTree();
-	/// print tree data to stdout
-	void printTreeData();
-	/// Calculate joint forces for given generalized state & derivatives.
-	/// This also updates kinematic terms computed in calculateKinematics.
-	/// If gravity is not set to zero, acceleration terms will contain
-	/// gravitational acceleration.
-	/// @param q generalized coordinates
-	/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
-	/// @param dot_u time derivative of u
-	/// @param joint_forces this is where the resulting joint forces will be
-	///		stored. dim(joint_forces) = dim(u)
-	/// @return 0 on success, -1 on error
-	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
-								 vecx* joint_forces);
-	/// Calculate joint space mass matrix
-	/// @param q generalized coordinates
-	/// @param initialize_matrix if true, initialize mass matrix with zero.
-	///		If mass_matrix is initialized to zero externally and only used
-	///		for mass matrix computations for the same system, it is safe to
-	///		set this to false.
-	/// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
-	///		is also populated, otherwise not.
-	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
-	/// @return -1 on error, 0 on success
-	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
-							const bool initialize_matrix, const bool set_lower_triangular_matrix,
-							matxx* mass_matrix);
-
-	/// Calculate joint space mass matrix.
-	/// This version will update kinematics, initialize all mass_matrix elements to zero and
-	/// populate all mass matrix entries.
-	/// @param q generalized coordinates
-	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
-	/// @return -1 on error, 0 on success
-	int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
-
-	/// Calculates kinematics also calculated in calculateInverseDynamics,
-	/// but not dynamics.
-	/// This function ensures that correct accelerations are computed that do not
-	/// contain gravitational acceleration terms.
-	/// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
-	int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
-	/// Calculate position kinematics
-	int calculatePositionKinematics(const vecx& q);
-	/// Calculate position and velocity kinematics
-	int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
-	/// d(Jacobian)/dt*u
-	/// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
-	/// or calculatePositionAndVelocityKinematics
-	int calculateJacobians(const vecx& q, const vecx& u);
-	/// Calculate Jacobians (dvel/du)
-	/// This function assumes that calculateInverseDynamics was called, or
-	/// one of the calculateKineamtics functions
-	int calculateJacobians(const vecx& q);
-#endif  // BT_ID_HAVE_MAT3X
-
-	/// set gravitational acceleration
-	/// the default is [0;0;-9.8] in the world frame
-	/// @param gravity the gravitational acceleration in world frame
-	/// @return 0 on success, -1 on error
-	int setGravityInWorldFrame(const vec3& gravity);
-	/// returns number of bodies in tree
-	int numBodies() const;
-	/// returns number of mechanical degrees of freedom (dimension of q-vector)
-	int numDoFs() const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyOrigin(const int body_index, vec3* world_origin) const;
-	/// get center of mass of a body, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_com pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyCoM(const int body_index, vec3* world_com) const;
-	/// get transform from of a body-fixed frame to the world frame
-	/// @param body_index index for frame/body
-	/// @param world_T_body pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyTransform(const int body_index, mat33* world_T_body) const;
-	/// get absolute angular velocity for a body, represented in the world frame
-	/// @param body_index index for frame/body
-	/// @param world_omega pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
-	/// get linear velocity of a body, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_velocity pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
-	/// get linear velocity of a body's CoM, represented in world frame
-	/// (not required for inverse dynamics, provided for convenience)
-	/// @param body_index index for frame/body
-	/// @param world_vel_com pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
-	/// obtainened by setting gravitational acceleration to zero, or subtracting it.
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	// get translational jacobian, in world frame (dworld_velocity/du)
-	int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
-	// get rotational jacobian, in world frame (dworld_omega/du)
-	int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
-	// get product of translational jacobian derivative * generatlized velocities
-	int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
-	// get product of rotational jacobian derivative * generatlized velocities
-	int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
-#endif  // BT_ID_HAVE_MAT3X
-
-	/// returns the (internal) index of body
-	/// @param body_index is the index of a body
-	/// @param parent_index pointer to where parent index will be stored
-	/// @return 0 on success, -1 on error
-	int getParentIndex(const int body_index, int* parent_index) const;
-	/// get joint type
-	/// @param body_index index of the body
-	/// @param joint_type the corresponding joint type
-	/// @return 0 on success, -1 on failure
-	int getJointType(const int body_index, JointType* joint_type) const;
-	/// get joint type as string
-	/// @param body_index index of the body
-	/// @param joint_type string naming the corresponding joint type
-	/// @return 0 on success, -1 on failure
-	int getJointTypeStr(const int body_index, const char** joint_type) const;
-	/// get offset translation to parent body (see addBody)
-	/// @param body_index index of the body
-	/// @param r the offset translation (see above)
-	/// @return 0 on success, -1 on failure
-	int getParentRParentBodyRef(const int body_index, vec3* r) const;
-	/// get offset rotation to parent body (see addBody)
-	/// @param body_index index of the body
-	/// @param T the transform (see above)
-	/// @return 0 on success, -1 on failure
-	int getBodyTParentRef(const int body_index, mat33* T) const;
-	/// get axis of motion (see addBody)
-	/// @param body_index index of the body
-	/// @param axis the axis (see above)
-	/// @return 0 on success, -1 on failure
-	int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
-	/// get offset for degrees of freedom of this body into the q-vector
-	/// @param body_index index of the body
-	/// @param q_offset offset the q vector
-	/// @return -1 on error, 0 on success
-	int getDoFOffset(const int body_index, int* q_offset) const;
-	/// get user integer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_int   the user integer
-	/// @return 0 on success, -1 on error
-	int getUserInt(const int body_index, int* user_int) const;
-	/// get user pointer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_ptr   the user pointer
-	/// @return 0 on success, -1 on error
-	int getUserPtr(const int body_index, void** user_ptr) const;
-	/// set user integer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_int   the user integer
-	/// @return 0 on success, -1 on error
-	int setUserInt(const int body_index, const int user_int);
-	/// set user pointer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_ptr   the user pointer
-	/// @return 0 on success, -1 on error
-	int setUserPtr(const int body_index, void* const user_ptr);
-	/// set mass for a body
-	/// @param body_index index of the body
-	/// @param mass the mass to set
-	/// @return 0 on success, -1 on failure
-	int setBodyMass(const int body_index, const idScalar mass);
-	/// set first moment of mass for a body
-	/// (mass * center of mass, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param first_mass_moment the vector to set
-	/// @return 0 on success, -1 on failure
-	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
-	/// set second moment of mass for a body
-	/// (moment of inertia, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param second_mass_moment the inertia matrix
-	/// @return 0 on success, -1 on failure
-	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
-	/// get mass for a body
-	/// @param body_index index of the body
-	/// @param mass the mass
-	/// @return 0 on success, -1 on failure
-	int getBodyMass(const int body_index, idScalar* mass) const;
-	/// get first moment of mass for a body
-	/// (mass * center of mass, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param first_moment the vector
-	/// @return 0 on success, -1 on failure
-	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
-	/// get second moment of mass for a body
-	/// (moment of inertia, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param second_mass_moment the inertia matrix
-	/// @return 0 on success, -1 on failure
-	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
-	/// set all user forces and moments to zero
-	void clearAllUserForcesAndMoments();
-	/// Add an external force to a body, acting at the origin of the body-fixed frame.
-	/// Calls to addUserForce are cumulative. Set the user force and moment to zero
-	/// via clearAllUserForcesAndMoments()
-	/// @param body_force the force represented in the body-fixed frame of reference
-	/// @return 0 on success, -1 on error
-	int addUserForce(const int body_index, const vec3& body_force);
-	/// Add an external moment to a body.
-	/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
-	/// via clearAllUserForcesAndMoments()
-	/// @param body_moment the moment represented in the body-fixed frame of reference
-	/// @return 0 on success, -1 on error
-	int addUserMoment(const int body_index, const vec3& body_moment);
-
-private:
-	// flag indicating if system has been initialized
-	bool m_is_finalized;
-	// flag indicating if mass properties are physically valid
-	bool m_mass_parameters_are_valid;
-	// flag defining if unphysical mass parameters are accepted
-	bool m_accept_invalid_mass_parameters;
-	// This struct implements the inverse dynamics calculations
-	class MultiBodyImpl;
-	MultiBodyImpl* m_impl;
-	// cache data structure for initialization
-	class InitCache;
-	InitCache* m_init_cache;
-};
-}  // namespace btInverseDynamics
-#endif  // MULTIBODYTREE_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp
deleted file mode 100644
index fe4f102513..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp
+++ /dev/null
@@ -1,39 +0,0 @@
-#ifndef INVDYNEIGENINTERFACE_HPP_
-#define INVDYNEIGENINTERFACE_HPP_
-#include "../IDConfig.hpp"
-namespace btInverseDynamics
-{
-#define BT_ID_HAVE_MAT3X
-
-#ifdef BT_USE_DOUBLE_PRECISION
-typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
-typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
-#else
-typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
-typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
-#endif
-
-inline void resize(mat3x &m, Eigen::Index size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx *m)
-{
-	(*m)(row, col) = val;
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m)
-{
-	(*m)(row, col) = val;
-}
-
-}  // namespace btInverseDynamics
-#endif  // INVDYNEIGENINTERFACE_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
deleted file mode 100644
index 0c398a3727..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ /dev/null
@@ -1,202 +0,0 @@
-#ifndef IDLINEARMATHINTERFACE_HPP_
-#define IDLINEARMATHINTERFACE_HPP_
-
-#include <cstdlib>
-
-#include "../IDConfig.hpp"
-
-#include "../../LinearMath/btMatrix3x3.h"
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btMatrixX.h"
-#define BT_ID_HAVE_MAT3X
-
-namespace btInverseDynamics
-{
-class vec3;
-class vecx;
-class mat33;
-typedef btMatrixX<idScalar> matxx;
-
-class vec3 : public btVector3
-{
-public:
-	vec3() : btVector3() {}
-	vec3(const btVector3& btv) { *this = btv; }
-	idScalar& operator()(int i) { return (*this)[i]; }
-	const idScalar& operator()(int i) const { return (*this)[i]; }
-	int size() const { return 3; }
-	const vec3& operator=(const btVector3& rhs)
-	{
-		*static_cast<btVector3*>(this) = rhs;
-		return *this;
-	}
-};
-
-class mat33 : public btMatrix3x3
-{
-public:
-	mat33() : btMatrix3x3() {}
-	mat33(const btMatrix3x3& btm) { *this = btm; }
-	idScalar& operator()(int i, int j) { return (*this)[i][j]; }
-	const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
-	const mat33& operator=(const btMatrix3x3& rhs)
-	{
-		*static_cast<btMatrix3x3*>(this) = rhs;
-		return *this;
-	}
-	friend mat33 operator*(const idScalar& s, const mat33& a);
-	friend mat33 operator/(const mat33& a, const idScalar& s);
-};
-
-inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); }
-
-inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
-
-class vecx : public btVectorX<idScalar>
-{
-public:
-	vecx(int size) : btVectorX<idScalar>(size) {}
-	const vecx& operator=(const btVectorX<idScalar>& rhs)
-	{
-		*static_cast<btVectorX<idScalar>*>(this) = rhs;
-		return *this;
-	}
-
-	idScalar& operator()(int i) { return (*this)[i]; }
-	const idScalar& operator()(int i) const { return (*this)[i]; }
-
-	friend vecx operator*(const vecx& a, const idScalar& s);
-	friend vecx operator*(const idScalar& s, const vecx& a);
-
-	friend vecx operator+(const vecx& a, const vecx& b);
-	friend vecx operator-(const vecx& a, const vecx& b);
-	friend vecx operator/(const vecx& a, const idScalar& s);
-};
-
-inline vecx operator*(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result(i) = a(i) * s;
-	}
-	return result;
-}
-inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
-inline vecx operator+(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result(i) = a(i) + b(i);
-	}
-
-	return result;
-}
-
-inline vecx operator-(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result(i) = a(i) - b(i);
-	}
-	return result;
-}
-inline vecx operator/(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result(i) = a(i) / s;
-	}
-
-	return result;
-}
-
-// use btMatrixX to implement 3xX matrix
-class mat3x : public matxx
-{
-public:
-	mat3x() {}
-	mat3x(const mat3x& rhs)
-	{
-		matxx::resize(rhs.rows(), rhs.cols());
-		*this = rhs;
-	}
-	mat3x(int rows, int cols) : matxx(3, cols)
-	{
-	}
-	void operator=(const mat3x& rhs)
-	{
-		if (m_cols != rhs.m_cols)
-		{
-			bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
-			abort();
-		}
-		for (int i = 0; i < rows(); i++)
-		{
-			for (int k = 0; k < cols(); k++)
-			{
-				setElem(i, k, rhs(i, k));
-			}
-		}
-	}
-	void setZero()
-	{
-		matxx::setZero();
-	}
-};
-
-inline vec3 operator*(const mat3x& a, const vecx& b)
-{
-	vec3 result;
-	if (a.cols() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
-		abort();
-	}
-	result(0) = 0.0;
-	result(1) = 0.0;
-	result(2) = 0.0;
-	for (int i = 0; i < b.size(); i++)
-	{
-		for (int k = 0; k < 3; k++)
-		{
-			result(k) += a(k, i) * b(i);
-		}
-	}
-	return result;
-}
-
-inline void resize(mat3x& m, idArrayIdx size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
-{
-	m->setElem(row, col, val);
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
-{
-	m->setElem(row, col, val);
-}
-
-}  // namespace btInverseDynamics
-
-#endif  // IDLINEARMATHINTERFACE_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
deleted file mode 100644
index 1c786095e7..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
+++ /dev/null
@@ -1,489 +0,0 @@
-/// @file Built-In Matrix-Vector functions
-#ifndef IDMATVEC_HPP_
-#define IDMATVEC_HPP_
-
-#include <cstdlib>
-
-#include "../IDConfig.hpp"
-#define BT_ID_HAVE_MAT3X
-
-namespace btInverseDynamics
-{
-class vec3;
-class vecx;
-class mat33;
-class matxx;
-class mat3x;
-
-/// This is a very basic implementation to enable stand-alone use of the library.
-/// The implementation is not really optimized and misses many features that you would
-/// want from a "fully featured" linear math library.
-class vec3
-{
-public:
-	idScalar& operator()(int i) { return m_data[i]; }
-	const idScalar& operator()(int i) const { return m_data[i]; }
-	const int size() const { return 3; }
-	const vec3& operator=(const vec3& rhs);
-	const vec3& operator+=(const vec3& b);
-	const vec3& operator-=(const vec3& b);
-	vec3 cross(const vec3& b) const;
-	idScalar dot(const vec3& b) const;
-
-	friend vec3 operator*(const mat33& a, const vec3& b);
-	friend vec3 operator*(const vec3& a, const idScalar& s);
-	friend vec3 operator*(const idScalar& s, const vec3& a);
-
-	friend vec3 operator+(const vec3& a, const vec3& b);
-	friend vec3 operator-(const vec3& a, const vec3& b);
-	friend vec3 operator/(const vec3& a, const idScalar& s);
-
-private:
-	idScalar m_data[3];
-};
-
-class mat33
-{
-public:
-	idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
-	const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
-	const mat33& operator=(const mat33& rhs);
-	mat33 transpose() const;
-	const mat33& operator+=(const mat33& b);
-	const mat33& operator-=(const mat33& b);
-
-	friend mat33 operator*(const mat33& a, const mat33& b);
-	friend vec3 operator*(const mat33& a, const vec3& b);
-	friend mat33 operator*(const mat33& a, const idScalar& s);
-	friend mat33 operator*(const idScalar& s, const mat33& a);
-	friend mat33 operator+(const mat33& a, const mat33& b);
-	friend mat33 operator-(const mat33& a, const mat33& b);
-	friend mat33 operator/(const mat33& a, const idScalar& s);
-
-private:
-	// layout is [0,1,2;3,4,5;6,7,8]
-	idScalar m_data[9];
-};
-
-class vecx
-{
-public:
-	vecx(int size) : m_size(size)
-	{
-		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
-	}
-	~vecx() { idFree(m_data); }
-	const vecx& operator=(const vecx& rhs);
-	idScalar& operator()(int i) { return m_data[i]; }
-	const idScalar& operator()(int i) const { return m_data[i]; }
-	const int& size() const { return m_size; }
-
-	friend vecx operator*(const vecx& a, const idScalar& s);
-	friend vecx operator*(const idScalar& s, const vecx& a);
-
-	friend vecx operator+(const vecx& a, const vecx& b);
-	friend vecx operator-(const vecx& a, const vecx& b);
-	friend vecx operator/(const vecx& a, const idScalar& s);
-
-private:
-	int m_size;
-	idScalar* m_data;
-};
-
-class matxx
-{
-public:
-	matxx()
-	{
-		m_data = 0x0;
-		m_cols = 0;
-		m_rows = 0;
-	}
-	matxx(int rows, int cols) : m_rows(rows), m_cols(cols)
-	{
-		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
-	}
-	~matxx() { idFree(m_data); }
-	idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
-	const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
-	const int& rows() const { return m_rows; }
-	const int& cols() const { return m_cols; }
-
-private:
-	int m_rows;
-	int m_cols;
-	idScalar* m_data;
-};
-
-class mat3x
-{
-public:
-	mat3x()
-	{
-		m_data = 0x0;
-		m_cols = 0;
-	}
-	mat3x(const mat3x& rhs)
-	{
-		m_cols = rhs.m_cols;
-		allocate();
-		*this = rhs;
-	}
-	mat3x(int rows, int cols) : m_cols(cols)
-	{
-		allocate();
-	};
-	void operator=(const mat3x& rhs)
-	{
-		if (m_cols != rhs.m_cols)
-		{
-			bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
-			abort();
-		}
-		for (int i = 0; i < 3 * m_cols; i++)
-		{
-			m_data[i] = rhs.m_data[i];
-		}
-	}
-
-	~mat3x()
-	{
-		free();
-	}
-	idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
-	const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
-	int rows() const { return m_rows; }
-	const int& cols() const { return m_cols; }
-	void resize(int rows, int cols)
-	{
-		m_cols = cols;
-		free();
-		allocate();
-	}
-	void setZero()
-	{
-		memset(m_data, 0x0, sizeof(idScalar) * m_rows * m_cols);
-	}
-	// avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
-private:
-	void allocate() { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols)); }
-	void free() { idFree(m_data); }
-	enum
-	{
-		m_rows = 3
-	};
-	int m_cols;
-	idScalar* m_data;
-};
-
-inline void resize(mat3x& m, idArrayIdx size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-//////////////////////////////////////////////////
-// Implementations
-inline const vec3& vec3::operator=(const vec3& rhs)
-{
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
-	}
-	return *this;
-}
-
-inline vec3 vec3::cross(const vec3& b) const
-{
-	vec3 result;
-	result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
-	result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
-	result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
-
-	return result;
-}
-
-inline idScalar vec3::dot(const vec3& b) const
-{
-	return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
-}
-
-inline const mat33& mat33::operator=(const mat33& rhs)
-{
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
-	}
-	return *this;
-}
-inline mat33 mat33::transpose() const
-{
-	mat33 result;
-	result.m_data[0] = m_data[0];
-	result.m_data[1] = m_data[3];
-	result.m_data[2] = m_data[6];
-	result.m_data[3] = m_data[1];
-	result.m_data[4] = m_data[4];
-	result.m_data[5] = m_data[7];
-	result.m_data[6] = m_data[2];
-	result.m_data[7] = m_data[5];
-	result.m_data[8] = m_data[8];
-
-	return result;
-}
-
-inline mat33 operator*(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	result.m_data[0] =
-		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
-	result.m_data[1] =
-		a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7];
-	result.m_data[2] =
-		a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8];
-	result.m_data[3] =
-		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6];
-	result.m_data[4] =
-		a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7];
-	result.m_data[5] =
-		a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8];
-	result.m_data[6] =
-		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6];
-	result.m_data[7] =
-		a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7];
-	result.m_data[8] =
-		a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
-
-	return result;
-}
-
-inline const mat33& mat33::operator+=(const mat33& b)
-{
-	for (int i = 0; i < 9; i++)
-	{
-		m_data[i] += b.m_data[i];
-	}
-
-	return *this;
-}
-
-inline const mat33& mat33::operator-=(const mat33& b)
-{
-	for (int i = 0; i < 9; i++)
-	{
-		m_data[i] -= b.m_data[i];
-	}
-	return *this;
-}
-
-inline vec3 operator*(const mat33& a, const vec3& b)
-{
-	vec3 result;
-
-	result.m_data[0] =
-		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2];
-	result.m_data[1] =
-		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2];
-	result.m_data[2] =
-		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
-
-	return result;
-}
-
-inline const vec3& vec3::operator+=(const vec3& b)
-{
-	for (int i = 0; i < 3; i++)
-	{
-		m_data[i] += b.m_data[i];
-	}
-	return *this;
-}
-
-inline const vec3& vec3::operator-=(const vec3& b)
-{
-	for (int i = 0; i < 3; i++)
-	{
-		m_data[i] -= b.m_data[i];
-	}
-	return *this;
-}
-
-inline mat33 operator*(const mat33& a, const idScalar& s)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-
-inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
-
-inline vec3 operator*(const vec3& a, const idScalar& s)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
-
-inline mat33 operator+(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-	return result;
-}
-inline vec3 operator+(const vec3& a, const vec3& b)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-	return result;
-}
-
-inline mat33 operator-(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-inline vec3 operator-(const vec3& a, const vec3& b)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-
-inline mat33 operator/(const mat33& a, const idScalar& s)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-	return result;
-}
-
-inline vec3 operator/(const vec3& a, const idScalar& s)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-	return result;
-}
-
-inline const vecx& vecx::operator=(const vecx& rhs)
-{
-	if (size() != rhs.size())
-	{
-		bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
-		abort();
-	}
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
-	}
-	return *this;
-}
-inline vecx operator*(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
-inline vecx operator+(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-
-	return result;
-}
-inline vecx operator-(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-inline vecx operator/(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-
-	return result;
-}
-
-inline vec3 operator*(const mat3x& a, const vecx& b)
-{
-	vec3 result;
-	if (a.cols() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
-		abort();
-	}
-	result(0) = 0.0;
-	result(1) = 0.0;
-	result(2) = 0.0;
-	for (int i = 0; i < b.size(); i++)
-	{
-		for (int k = 0; k < 3; k++)
-		{
-			result(k) += a(k, i) * b(i);
-		}
-	}
-	return result;
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
-{
-	(*m)(row, col) = val;
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
-{
-	(*m)(row, col) = val;
-}
-
-}  // namespace btInverseDynamics
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
deleted file mode 100644
index ec9a562295..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ /dev/null
@@ -1,1286 +0,0 @@
-#include "MultiBodyTreeImpl.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
-	: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	  ,
-	  m_m3x(3, m_num_dofs)
-#endif
-{
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	resize(m_m3x, m_num_dofs);
-#endif
-	m_body_list.resize(num_bodies_);
-	m_parent_index.resize(num_bodies_);
-	m_child_indices.resize(num_bodies_);
-	m_user_int.resize(num_bodies_);
-	m_user_ptr.resize(num_bodies_);
-
-	m_world_gravity(0) = 0.0;
-	m_world_gravity(1) = 0.0;
-	m_world_gravity(2) = -9.8;
-}
-
-const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const
-{
-	switch (type)
-	{
-		case FIXED:
-			return "fixed";
-		case REVOLUTE:
-			return "revolute";
-		case PRISMATIC:
-			return "prismatic";
-		case FLOATING:
-			return "floating";
-		case SPHERICAL:
-			return "spherical";
-	}
-	return "error: invalid";
-}
-
-inline void indent(const int &level)
-{
-	for (int j = 0; j < level; j++)
-		id_printf("  ");  // indent
-}
-
-void MultiBodyTree::MultiBodyImpl::printTree()
-{
-	id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
-	printTree(0, 0);
-}
-
-void MultiBodyTree::MultiBodyImpl::printTreeData()
-{
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		id_printf("body: %d\n", static_cast<int>(i));
-		id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
-		id_printf("q_index= %d\n", body.m_q_index);
-		id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2));
-		id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2));
-
-		id_printf("mass = %f\n", body.m_mass);
-		id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
-				  body.m_body_mass_com(2));
-		id_printf(
-			"I_o= [%f %f %f;\n"
-			"	  %f %f %f;\n"
-			"	  %f %f %f]\n",
-			body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
-			body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
-			body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
-
-		id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
-				  body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
-	}
-}
-int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
-{
-	switch (type)
-	{
-		case FIXED:
-			return 0;
-		case REVOLUTE:
-		case PRISMATIC:
-			return 1;
-		case FLOATING:
-			return 6;
-		case SPHERICAL:
-			return 3;
-	}
-	bt_id_error_message("unknown joint type %d\n", type);
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation)
-{
-	// this is adapted from URDF2Bullet.
-	// TODO: fix this and print proper graph (similar to git --log --graph)
-	int num_children = m_child_indices[index].size();
-
-	indentation += 2;
-	int count = 0;
-
-	for (int i = 0; i < num_children; i++)
-	{
-		int child_index = m_child_indices[index][i];
-		indent(indentation);
-		id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
-				  jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1,
-				  m_body_list[index].m_q_index,
-				  m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type));
-		// first grandchild
-		printTree(child_index, indentation);
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity)
-{
-	m_world_gravity = gravity;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::generateIndexSets()
-{
-	m_body_revolute_list.resize(0);
-	m_body_prismatic_list.resize(0);
-	int q_index = 0;
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		body.m_q_index = -1;
-		switch (body.m_joint_type)
-		{
-			case REVOLUTE:
-				m_body_revolute_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index++;
-				break;
-			case PRISMATIC:
-				m_body_prismatic_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index++;
-				break;
-			case FIXED:
-				// do nothing
-				break;
-			case FLOATING:
-				m_body_floating_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index += 6;
-				break;
-			case SPHERICAL:
-				m_body_spherical_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index += 3;
-				break;
-			default:
-				bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
-				return -1;
-		}
-	}
-	// sanity check
-	if (q_index != m_num_dofs)
-	{
-		bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
-		return -1;
-	}
-
-	m_child_indices.resize(m_body_list.size());
-
-	for (idArrayIdx child = 1; child < m_parent_index.size(); child++)
-	{
-		const int &parent = m_parent_index[child];
-		if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1))
-		{
-			m_child_indices[parent].push_back(child);
-		}
-		else
-		{
-			if (-1 == parent)
-			{
-				// multiple bodies are directly linked to the environment, ie, not a single root
-				bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
-			}
-			else
-			{
-				// should never happen
-				bt_id_error_message(
-					"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
-					child, parent, static_cast<int>(m_parent_index.size()));
-			}
-			return -1;
-		}
-	}
-
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::calculateStaticData()
-{
-	// relative kinematics that are not a function of q, u, dot_u
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		switch (body.m_joint_type)
-		{
-			case REVOLUTE:
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				break;
-			case PRISMATIC:
-				body.m_body_T_parent = body.m_body_T_parent_ref;
-				body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT;
-				body.m_body_ang_vel_rel(0) = 0;
-				body.m_body_ang_vel_rel(1) = 0;
-				body.m_body_ang_vel_rel(2) = 0;
-				body.m_body_ang_acc_rel(0) = 0;
-				body.m_body_ang_acc_rel(1) = 0;
-				body.m_body_ang_acc_rel(2) = 0;
-				break;
-			case FIXED:
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				body.m_body_T_parent = body.m_body_T_parent_ref;
-				body.m_body_ang_vel_rel(0) = 0;
-				body.m_body_ang_vel_rel(1) = 0;
-				body.m_body_ang_vel_rel(2) = 0;
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_body_ang_acc_rel(0) = 0;
-				body.m_body_ang_acc_rel(1) = 0;
-				body.m_body_ang_acc_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				break;
-			case FLOATING:
-				// no static data
-				break;
-			case SPHERICAL:
-				//todo: review
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				break;
-		}
-
-			// resize & initialize jacobians to zero.
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-		body.m_body_dot_Jac_T_u(0) = 0.0;
-		body.m_body_dot_Jac_T_u(1) = 0.0;
-		body.m_body_dot_Jac_T_u(2) = 0.0;
-		body.m_body_dot_Jac_R_u(0) = 0.0;
-		body.m_body_dot_Jac_R_u(1) = 0.0;
-		body.m_body_dot_Jac_R_u(2) = 0.0;
-		resize(body.m_body_Jac_T, m_num_dofs);
-		resize(body.m_body_Jac_R, m_num_dofs);
-		body.m_body_Jac_T.setZero();
-		body.m_body_Jac_R.setZero();
-#endif  //
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
-														   const vecx &dot_u, vecx *joint_forces)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
-		joint_forces->size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
-			static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
-		return -1;
-	}
-	// 1. relative kinematics
-	if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION))
-	{
-		bt_id_error_message("error in calculateKinematics\n");
-		return -1;
-	}
-	// 2. update contributions to equations of motion for every body.
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		// 3.4 update dynamic terms (rate of change of angular & linear momentum)
-		body.m_eom_lhs_rotational =
-			body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
-			body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
-			body.m_body_moment_user;
-		body.m_eom_lhs_translational =
-			body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
-			body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
-			body.m_body_force_user;
-	}
-
-	// 3. calculate full set of forces at parent joint
-	// (not directly calculating the joint force along the free direction
-	// simplifies inclusion of fixed joints.
-	// An alternative would be to fuse bodies in a pre-processing step,
-	// but that would make changing masses online harder (eg, payload masses
-	// added with fixed  joints to a gripper)
-	// Also, this enables adding zero weight bodies as a way to calculate frame poses
-	// for force elements, etc.
-
-	for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--)
-	{
-		// sum of forces and moments acting on this body from its children
-		vec3 sum_f_children;
-		vec3 sum_m_children;
-		setZero(sum_f_children);
-		setZero(sum_m_children);
-		for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
-			 child_list_idx++)
-		{
-			const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
-			vec3 child_joint_force_in_this_frame =
-				child.m_body_T_parent.transpose() * child.m_force_at_joint;
-			sum_f_children -= child_joint_force_in_this_frame;
-			sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
-							  child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
-		}
-		RigidBody &body = m_body_list[body_idx];
-
-		body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
-		body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
-	}
-
-	// 4. Calculate Joint forces.
-	// These are the components of force_at_joint/moment_at_joint
-	// in the free directions given by Jac_JT/Jac_JR
-	// 4.1 revolute joints
-	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_revolute_list[i]];
-		// (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
-		(*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
-	}
-	// 4.2 for prismatic joints
-	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-		// (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
-		(*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
-	}
-	// 4.3 floating bodies (6-DoF joints)
-	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_floating_list[i]];
-		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
-		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
-		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
-
-		(*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
-		(*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
-		(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
-	}
-
-	// 4.4 spherical bodies (3-DoF joints)
-	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-	{
-		//todo: review
-		RigidBody &body = m_body_list[m_body_spherical_list[i]];
-		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
-		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
-		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
-	}
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u,
-													  const KinUpdateType type)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
-			static_cast<int>(dot_u.size()));
-		return -1;
-	}
-	if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION)
-	{
-		bt_id_error_message("invalid type %d\n", type);
-		return -1;
-	}
-
-	// 1. update relative kinematics
-	// 1.1 for revolute
-	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_revolute_list[i]];
-		mat33 T;
-		bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
-		body.m_body_T_parent = T * body.m_body_T_parent_ref;
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
-		}
-	}
-	// 1.2 for prismatic
-	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-		body.m_parent_pos_parent_body =
-			body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_parent_vel_rel =
-				body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
-		}
-	}
-	// 1.3 fixed joints: nothing to do
-	// 1.4 6dof joints:
-	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_floating_list[i]];
-
-		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-							   transformY(q(body.m_q_index + 1)) *
-							   transformX(q(body.m_q_index));
-		body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
-		body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
-		body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
-		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
-			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
-			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
-
-			body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
-			body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
-			body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
-
-			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
-			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
-			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
-
-			body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
-			body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
-			body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
-
-			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
-		}
-	}
-	
-	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-	{
-		//todo: review
-		RigidBody &body = m_body_list[m_body_spherical_list[i]];
-
-		mat33 T;
-
-		T = transformX(q(body.m_q_index)) *
-				transformY(q(body.m_q_index + 1)) *
-				transformZ(q(body.m_q_index + 2));
-		body.m_body_T_parent = T * body.m_body_T_parent_ref;
-			
-		body.m_parent_pos_parent_body(0)=0;
-		body.m_parent_pos_parent_body(1)=0;
-		body.m_parent_pos_parent_body(2)=0;
-		
-		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
-			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
-			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
-			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
-			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
-			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
-			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
-		}
-	}
-
-	// 2. absolute kinematic quantities (vector valued)
-	// NOTE: this should be optimized by specializing for different body types
-	// (e.g., relative rotation is always zero for prismatic joints, etc.)
-
-	// calculations for root body
-	{
-		RigidBody &body = m_body_list[0];
-		// 3.1 update absolute positions and orientations:
-		// will be required if we add force elements (eg springs between bodies,
-		// or contacts)
-		// not required right now, added here for debugging purposes
-		body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		body.m_body_T_world = body.m_body_T_parent;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			// 3.2 update absolute velocities
-			body.m_body_ang_vel = body.m_body_ang_vel_rel;
-			body.m_body_vel = body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			// 3.3 update absolute accelerations
-			// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
-			body.m_body_ang_acc = body.m_body_ang_acc_rel;
-			body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
-			// add gravitational acceleration to root body
-			// this is an efficient way to add gravitational terms,
-			// but it does mean that the kinematics are no longer
-			// correct at the acceleration level
-			// NOTE: To get correct acceleration kinematics, just set world_gravity to zero
-			body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
-		}
-	}
-
-	for (idArrayIdx i = 1; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		RigidBody &parent = m_body_list[m_parent_index[i]];
-		// 2.1 update absolute positions and orientations:
-		// will be required if we add force elements (eg springs between bodies,
-		// or contacts)  not required right now added here for debugging purposes
-		body.m_body_pos =
-			body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
-		body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			// 2.2 update absolute velocities
-			body.m_body_ang_vel =
-				body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
-
-			body.m_body_vel =
-				body.m_body_T_parent *
-				(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
-				 body.m_parent_vel_rel);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			// 2.3 update absolute accelerations
-			// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
-			body.m_body_ang_acc =
-				body.m_body_T_parent * parent.m_body_ang_acc -
-				body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
-				body.m_body_ang_acc_rel;
-			body.m_body_acc =
-				body.m_body_T_parent *
-				(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
-				 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
-				 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
-		}
-	}
-
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-
-void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
-{
-	const int &idx = body.m_q_index;
-	switch (body.m_joint_type)
-	{
-		case FIXED:
-			break;
-		case REVOLUTE:
-			setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
-			setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
-			setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
-			break;
-		case PRISMATIC:
-			setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			break;
-		case FLOATING:
-			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
-			// body_Jac_T = body_T_parent.transpose();
-			setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T);
-			setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T);
-			setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T);
-
-			setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T);
-			setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T);
-			setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T);
-
-			setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T);
-			setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
-			setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
-
-			break;
-		case SPHERICAL:
-			//todo: review
-			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
-			break;
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
-		return -1;
-	}
-	if (type != POSITION_ONLY && type != POSITION_VELOCITY)
-	{
-		bt_id_error_message("invalid type %d\n", type);
-		return -1;
-	}
-
-	addRelativeJacobianComponent(m_body_list[0]);
-	for (idArrayIdx i = 1; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		RigidBody &parent = m_body_list[m_parent_index[i]];
-
-		mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R);
-		body.m_body_Jac_T = parent.m_body_Jac_T;
-		mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x);
-		sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T);
-
-		addRelativeJacobianComponent(body);
-		mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T);
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
-									  body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
-			body.m_body_dot_Jac_T_u = body.m_body_T_parent *
-									  (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
-									   parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
-									   2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
-		}
-	}
-	return 0;
-}
-#endif
-
-static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
-{
-	switch (dof)
-	{
-		// rotational part
-		case 0:
-			Jac_JR(0) = 1;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 1:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 1;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 2:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 1;
-			setZero(Jac_JT);
-			break;
-	}
-}
-
-static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
-{
-	switch (dof)
-	{
-		// rotational part
-		case 0:
-			Jac_JR(0) = 1;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 1:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 1;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 2:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 1;
-			setZero(Jac_JT);
-			break;
-		// translational part
-		case 3:
-			setZero(Jac_JR);
-			Jac_JT(0) = 1;
-			Jac_JT(1) = 0;
-			Jac_JT(2) = 0;
-			break;
-		case 4:
-			setZero(Jac_JR);
-			Jac_JT(0) = 0;
-			Jac_JT(1) = 1;
-			Jac_JT(2) = 0;
-			break;
-		case 5:
-			setZero(Jac_JR);
-			Jac_JT(0) = 0;
-			Jac_JT(1) = 0;
-			Jac_JT(2) = 1;
-			break;
-	}
-}
-
-static inline int jointNumDoFs(const JointType &type)
-{
-	switch (type)
-	{
-		case FIXED:
-			return 0;
-		case REVOLUTE:
-		case PRISMATIC:
-			return 1;
-		case FLOATING:
-			return 6;
-		case SPHERICAL:
-			return 3;
-	}
-	// this should never happen
-	bt_id_error_message("invalid joint type\n");
-	// TODO add configurable abort/crash function
-	abort();
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
-													  const bool initialize_matrix,
-													  const bool set_lower_triangular_matrix,
-													  matxx *mass_matrix)
-{
-	// This calculates the joint space mass matrix for the multibody system.
-	// The algorithm is essentially an implementation of "method 3"
-	// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
-	// (Later named "Composite Rigid Body Algorithm" by Featherstone).
-	//
-	// This implementation, however, handles branched systems and uses a formulation centered
-	// on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
-
-	if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
-		mass_matrix->cols() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"Dimension error. System has %d DOFs,\n"
-			"but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
-			static_cast<int>(mass_matrix->cols()));
-		return -1;
-	}
-
-	// TODO add optimized zeroing function?
-	if (initialize_matrix)
-	{
-		for (int i = 0; i < m_num_dofs; i++)
-		{
-			for (int j = 0; j < m_num_dofs; j++)
-			{
-				setMatxxElem(i, j, 0.0, mass_matrix);
-			}
-		}
-	}
-
-	if (update_kinematics)
-	{
-		// 1. update relative kinematics
-		// 1.1 for revolute joints
-		for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_revolute_list[i]];
-			// from reference orientation (q=0) of body-fixed frame to current orientation
-			mat33 body_T_body_ref;
-			bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
-			body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
-		}
-		// 1.2 for prismatic joints
-		for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-			// body.m_body_T_parent= fixed
-			body.m_parent_pos_parent_body =
-				body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
-		}
-		// 1.3 fixed joints: nothing to do
-		// 1.4 6dof joints:
-		for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_floating_list[i]];
-
-			body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-								   transformY(q(body.m_q_index + 1)) *
-								   transformX(q(body.m_q_index));
-			body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
-			body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
-			body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
-
-			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		}
-
-		for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-		{
-			//todo: review
-			RigidBody &body = m_body_list[m_body_spherical_list[i]];
-
-			mat33 T;
-
-			T = transformX(q(body.m_q_index)) *
-				transformY(q(body.m_q_index + 1)) *
-				transformZ(q(body.m_q_index + 2));
-			body.m_body_T_parent = T * body.m_body_T_parent_ref;
-
-			body.m_parent_pos_parent_body(0)=0;
-			body.m_parent_pos_parent_body(1)=0;
-			body.m_parent_pos_parent_body(2)=0;
-			
-			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		}
-	}
-	for (int i = m_body_list.size() - 1; i >= 0; i--)
-	{
-		RigidBody &body = m_body_list[i];
-		// calculate mass, center of mass and inertia of "composite rigid body",
-		// ie, sub-tree starting at current body
-		body.m_subtree_mass = body.m_mass;
-		body.m_body_subtree_mass_com = body.m_body_mass_com;
-		body.m_body_subtree_I_body = body.m_body_I_body;
-
-		for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++)
-		{
-			RigidBody &child = m_body_list[m_child_indices[i][c]];
-			mat33 body_T_child = child.m_body_T_parent.transpose();
-
-			body.m_subtree_mass += child.m_subtree_mass;
-			body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com +
-											child.m_parent_pos_parent_body * child.m_subtree_mass;
-			body.m_body_subtree_I_body +=
-				body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
-
-			if (child.m_subtree_mass > 0)
-			{
-				// Shift the reference point for the child subtree inertia using the
-				// Huygens-Steiner ("parallel axis") theorem.
-				// (First shift from child origin to child com, then from there to this body's
-				// origin)
-				vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass;
-				mat33 tilde_r_child_com = tildeOperator(r_com);
-				mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com);
-				body.m_body_subtree_I_body +=
-					child.m_subtree_mass *
-					(tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com);
-			}
-		}
-	}
-
-	for (int i = m_body_list.size() - 1; i >= 0; i--)
-	{
-		const RigidBody &body = m_body_list[i];
-
-		// determine DoF-range for body
-		const int q_index_min = body.m_q_index;
-		const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1;
-		// loop over the DoFs used by this body
-		// local joint jacobians (ok as is for 1-DoF joints)
-		vec3 Jac_JR = body.m_Jac_JR;
-		vec3 Jac_JT = body.m_Jac_JT;
-		for (int col = q_index_max; col >= q_index_min; col--)
-		{
-			// set jacobians for 6-DoF joints
-			if (FLOATING == body.m_joint_type)
-			{
-				setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
-			}
-			if (SPHERICAL == body.m_joint_type)
-			{
-				//todo: review
-				setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
-			}
-
-			vec3 body_eom_rot =
-				body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
-			vec3 body_eom_trans =
-				body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
-			setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
-
-			// rest of the mass matrix column upwards
-			{
-				// 1. for multi-dof joints, rest of the dofs of this body
-				for (int row = col - 1; row >= q_index_min; row--)
-				{
-					if (SPHERICAL == body.m_joint_type)
-					{
-						//todo: review
-						setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-					if (FLOATING == body.m_joint_type)
-					{
-						setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-				}
-				// 2. ancestor dofs
-				int child_idx = i;
-				int parent_idx = m_parent_index[i];
-				while (parent_idx >= 0)
-				{
-					const RigidBody &child_body = m_body_list[child_idx];
-					const RigidBody &parent_body = m_body_list[parent_idx];
-
-					const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
-					body_eom_rot = parent_T_child * body_eom_rot;
-					body_eom_trans = parent_T_child * body_eom_trans;
-					body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
-
-					const int parent_body_q_index_min = parent_body.m_q_index;
-					const int parent_body_q_index_max =
-						parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
-					vec3 Jac_JR = parent_body.m_Jac_JR;
-					vec3 Jac_JT = parent_body.m_Jac_JT;
-					for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
-					{
-						if (SPHERICAL == parent_body.m_joint_type)
-						{
-							//todo: review
-							setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
-						}
-						// set jacobians for 6-DoF joints
-						if (FLOATING == parent_body.m_joint_type)
-						{
-							setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
-						}
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-
-					child_idx = parent_idx;
-					parent_idx = m_parent_index[child_idx];
-				}
-			}
-		}
-	}
-
-	if (set_lower_triangular_matrix)
-	{
-		for (int col = 0; col < m_num_dofs; col++)
-		{
-			for (int row = 0; row < col; row++)
-			{
-				setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
-			}
-		}
-	}
-	return 0;
-}
-
-// utility macro
-#define CHECK_IF_BODY_INDEX_IS_VALID(index)                                                  \
-	do                                                                                       \
-	{                                                                                        \
-		if (index < 0 || index >= m_num_bodies)                                              \
-		{                                                                                    \
-			bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
-			return -1;                                                                       \
-		}                                                                                    \
-	} while (0)
-
-int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*p = m_parent_index[body_index];
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*user_int = m_user_int[body_index];
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*user_ptr = m_user_ptr[body_index];
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_user_int[body_index] = user_int;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_user_ptr[body_index] = user_ptr;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	if (body.m_mass > 0)
-	{
-		*world_com = body.m_body_T_world.transpose() *
-					 (body.m_body_pos + body.m_body_mass_com / body.m_mass);
-	}
-	else
-	{
-		*world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
-	}
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_T_body = body.m_body_T_world.transpose();
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
-														vec3 *world_velocity) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
-														   vec3 *world_velocity) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	vec3 com;
-	if (body.m_mass > 0)
-	{
-		com = body.m_body_mass_com / body.m_mass;
-	}
-	else
-	{
-		com(0) = 0;
-		com(1) = 0;
-		com(2) = 0;
-	}
-
-	*world_velocity =
-		body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com));
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
-															 vec3 *world_dot_omega) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
-															vec3 *world_acceleration) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*joint_type = m_body_list[body_index].m_joint_type;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
-												  const char **joint_type) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*r = m_body_list[body_index].m_parent_pos_parent_body_ref;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*T = m_body_list[body_index].m_body_T_parent_ref;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	if (m_body_list[body_index].m_joint_type == REVOLUTE)
-	{
-		*axis = m_body_list[body_index].m_Jac_JR;
-		return 0;
-	}
-	if (m_body_list[body_index].m_joint_type == PRISMATIC)
-	{
-		*axis = m_body_list[body_index].m_Jac_JT;
-		return 0;
-	}
-	setZero(*axis);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*q_index = m_body_list[body_index].m_q_index;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_mass = mass;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
-														 const vec3 &first_mass_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_mass_com = first_mass_moment;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
-														  const mat33 &second_mass_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_I_body = second_mass_moment;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*mass = m_body_list[body_index].m_mass;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
-														 vec3 *first_mass_moment) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*first_mass_moment = m_body_list[body_index].m_body_mass_com;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
-														  mat33 *second_mass_moment) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*second_mass_moment = m_body_list[body_index].m_body_I_body;
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments()
-{
-	for (int index = 0; index < m_num_bodies; index++)
-	{
-		RigidBody &body = m_body_list[index];
-		setZero(body.m_body_force_user);
-		setZero(body.m_body_moment_user);
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_force_user += body_force;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_moment_user += body_moment;
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot);
-	return 0;
-}
-
-#endif
-}  // namespace btInverseDynamics
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
deleted file mode 100644
index eabdbe161b..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
+++ /dev/null
@@ -1,288 +0,0 @@
-// The structs and classes defined here provide a basic inverse fynamics implementation used
-// by MultiBodyTree
-// User interaction should be through MultiBodyTree
-
-#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_
-#define MULTI_BODY_REFERENCE_IMPL_HPP_
-
-#include "../IDConfig.hpp"
-#include "../MultiBodyTree.hpp"
-
-namespace btInverseDynamics
-{
-/// Structure for for rigid body mass properties, connectivity and kinematic state
-/// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
-/// The body-fixed frame is located in the joint connecting the body to its parent.
-struct RigidBody
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	// 1 Inertial properties
-	/// Mass
-	idScalar m_mass;
-	/// Mass times center of gravity in body-fixed frame
-	vec3 m_body_mass_com;
-	/// Moment of inertia w.r.t. body-fixed frame
-	mat33 m_body_I_body;
-
-	// 2 dynamic properties
-	/// Left-hand side of the body equation of motion, translational part
-	vec3 m_eom_lhs_translational;
-	/// Left-hand side of the body equation of motion, rotational part
-	vec3 m_eom_lhs_rotational;
-	/// Force acting at the joint when the body is cut from its parent;
-	/// includes impressed joint force in J_JT direction,
-	/// as well as constraint force,
-	/// in body-fixed frame
-	vec3 m_force_at_joint;
-	/// Moment acting at the joint when the body is cut from its parent;
-	/// includes impressed joint moment in J_JR direction, and constraint moment
-	/// in body-fixed frame
-	vec3 m_moment_at_joint;
-	/// external (user provided) force acting at the body-fixed frame's origin, written in that
-	/// frame
-	vec3 m_body_force_user;
-	/// external (user provided) moment acting at the body-fixed frame's origin, written in that
-	/// frame
-	vec3 m_body_moment_user;
-	// 3 absolute kinematic properties
-	/// Position of body-fixed frame relative to world frame
-	/// this is currently only for debugging purposes
-	vec3 m_body_pos;
-	/// Absolute velocity of body-fixed frame
-	vec3 m_body_vel;
-	/// Absolute acceleration of body-fixed frame
-	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
-	/// acceleration!
-	vec3 m_body_acc;
-	/// Absolute angular velocity
-	vec3 m_body_ang_vel;
-	/// Absolute angular acceleration
-	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
-	/// acceleration!
-	vec3 m_body_ang_acc;
-
-	// 4 relative kinematic properties.
-	// these are in the parent body frame
-	/// Transform from world to body-fixed frame;
-	/// this is currently only for debugging purposes
-	mat33 m_body_T_world;
-	/// Transform from parent to body-fixed frame
-	mat33 m_body_T_parent;
-	/// Vector from parent to child frame in parent frame
-	vec3 m_parent_pos_parent_body;
-	/// Relative angular velocity
-	vec3 m_body_ang_vel_rel;
-	/// Relative linear velocity
-	vec3 m_parent_vel_rel;
-	/// Relative angular acceleration
-	vec3 m_body_ang_acc_rel;
-	/// Relative linear acceleration
-	vec3 m_parent_acc_rel;
-
-	// 5 Data describing the joint type and geometry
-	/// Type of joint
-	JointType m_joint_type;
-	/// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
-	/// Components are in body-fixed frame of the parent
-	vec3 m_parent_pos_parent_body_ref;
-	/// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
-	mat33 m_body_T_parent_ref;
-	/// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute
-	/// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
-	/// For revolute joints this is the joint axis, for prismatic joints it is a null matrix.
-	/// (NOTE: dimensions will have to be dynamic for additional joint types!)
-	vec3 m_Jac_JR;
-	/// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute
-	/// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
-	/// For prismatic joints this is the joint axis, for revolute joints it is a null matrix.
-	/// (NOTE: dimensions might have to be dynamic for additional joint types!)
-	vec3 m_Jac_JT;
-	/// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT
-	vec3 m_parent_Jac_JT;
-	/// Start of index range for the position degree(s) of freedom describing this body's motion
-	/// relative to
-	/// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
-	int m_q_index;
-
-	// 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
-	/// mass of the subtree rooted in this body
-	idScalar m_subtree_mass;
-	/// center of mass * mass for subtree rooted in this body, in body-fixed frame
-	vec3 m_body_subtree_mass_com;
-	/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
-	mat33 m_body_subtree_I_body;
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// translational jacobian in body-fixed frame d(m_body_vel)/du
-	mat3x m_body_Jac_T;
-	/// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
-	mat3x m_body_Jac_R;
-	/// components of linear acceleration depending on u
-	/// (same as is d(m_Jac_T)/dt*u)
-	vec3 m_body_dot_Jac_T_u;
-	/// components of angular acceleration depending on u
-	/// (same as is d(m_Jac_T)/dt*u)
-	vec3 m_body_dot_Jac_R_u;
-#endif
-};
-
-/// The MBS implements a tree structured multibody system
-class MultiBodyTree::MultiBodyImpl
-{
-	friend class MultiBodyTree;
-
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	enum KinUpdateType
-	{
-		POSITION_ONLY,
-		POSITION_VELOCITY,
-		POSITION_VELOCITY_ACCELERATION
-	};
-
-	/// constructor
-	/// @param num_bodies the number of bodies in the system
-	/// @param num_dofs number of degrees of freedom in the system
-	MultiBodyImpl(int num_bodies_, int num_dofs_);
-
-	/// \copydoc MultiBodyTree::calculateInverseDynamics
-	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
-								 vecx* joint_forces);
-	///\copydoc MultiBodyTree::calculateMassMatrix
-	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
-							const bool initialize_matrix, const bool set_lower_triangular_matrix,
-							matxx* mass_matrix);
-	/// calculate kinematics (vector quantities)
-	/// Depending on type, update positions only, positions & velocities, or positions, velocities
-	/// and accelerations.
-	int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
-	int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
-	/// \copydoc MultiBodyTree::getBodyDotJacobianTransU
-	int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
-	/// \copydoc MultiBodyTree::getBodyDotJacobianRotU
-	int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
-	/// \copydoc MultiBodyTree::getBodyJacobianTrans
-	int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
-	/// \copydoc MultiBodyTree::getBodyJacobianRot
-	int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
-	/// Add relative Jacobian component from motion relative to parent body
-	/// @param body the body to add the Jacobian component for
-	void addRelativeJacobianComponent(RigidBody& body);
-#endif
-	/// generate additional index sets from the parent_index array
-	/// @return -1 on error, 0 on success
-	int generateIndexSets();
-	/// set gravity acceleration in world frame
-	/// @param gravity gravity vector in the world frame
-	/// @return 0 on success, -1 on error
-	int setGravityInWorldFrame(const vec3& gravity);
-	/// pretty print tree
-	void printTree();
-	/// print tree data
-	void printTreeData();
-	/// initialize fixed data
-	void calculateStaticData();
-	/// \copydoc MultiBodyTree::getBodyFrame
-	int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
-	/// \copydoc MultiBodyTree::getParentIndex
-	int getParentIndex(const int body_index, int* m_parent_index);
-	/// \copydoc MultiBodyTree::getJointType
-	int getJointType(const int body_index, JointType* joint_type) const;
-	/// \copydoc MultiBodyTree::getJointTypeStr
-	int getJointTypeStr(const int body_index, const char** joint_type) const;
-	/// \copydoc MultiBodyTree::getParentRParentBodyRef
-	int getParentRParentBodyRef(const int body_index, vec3* r) const;
-	/// \copydoc MultiBodyTree::getBodyTParentRef
-	int getBodyTParentRef(const int body_index, mat33* T) const;
-	/// \copydoc MultiBodyTree::getBodyAxisOfMotion
-	int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
-	/// \copydoc MultiBodyTree:getDoFOffset
-	int getDoFOffset(const int body_index, int* q_index) const;
-	/// \copydoc MultiBodyTree::getBodyOrigin
-	int getBodyOrigin(const int body_index, vec3* world_origin) const;
-	/// \copydoc MultiBodyTree::getBodyCoM
-	int getBodyCoM(const int body_index, vec3* world_com) const;
-	/// \copydoc MultiBodyTree::getBodyTransform
-	int getBodyTransform(const int body_index, mat33* world_T_body) const;
-	/// \copydoc MultiBodyTree::getBodyAngularVelocity
-	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
-	/// \copydoc MultiBodyTree::getBodyLinearVelocity
-	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
-	/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
-	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
-	/// \copydoc MultiBodyTree::getBodyAngularAcceleration
-	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
-	/// \copydoc MultiBodyTree::getBodyLinearAcceleration
-	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
-	/// \copydoc MultiBodyTree::getUserInt
-	int getUserInt(const int body_index, int* user_int) const;
-	/// \copydoc MultiBodyTree::getUserPtr
-	int getUserPtr(const int body_index, void** user_ptr) const;
-	/// \copydoc MultiBodyTree::setUserInt
-	int setUserInt(const int body_index, const int user_int);
-	/// \copydoc MultiBodyTree::setUserPtr
-	int setUserPtr(const int body_index, void* const user_ptr);
-	///\copydoc MultiBodytTree::setBodyMass
-	int setBodyMass(const int body_index, const idScalar mass);
-	///\copydoc MultiBodytTree::setBodyFirstMassMoment
-	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
-	///\copydoc MultiBodytTree::setBodySecondMassMoment
-	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
-	///\copydoc MultiBodytTree::getBodyMass
-	int getBodyMass(const int body_index, idScalar* mass) const;
-	///\copydoc MultiBodytTree::getBodyFirstMassMoment
-	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
-	///\copydoc MultiBodytTree::getBodySecondMassMoment
-	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
-	/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
-	void clearAllUserForcesAndMoments();
-	/// \copydoc MultiBodyTree::addUserForce
-	int addUserForce(const int body_index, const vec3& body_force);
-	/// \copydoc MultiBodyTree::addUserMoment
-	int addUserMoment(const int body_index, const vec3& body_moment);
-
-private:
-	// debug function. print tree structure to stdout
-	void printTree(int index, int indentation);
-	// get string representation of JointType (for debugging)
-	const char* jointTypeToString(const JointType& type) const;
-	// get number of degrees of freedom from joint type
-	int bodyNumDoFs(const JointType& type) const;
-	// number of bodies in the system
-	int m_num_bodies;
-	// number of degrees of freedom
-	int m_num_dofs;
-	// Gravitational acceleration (in world frame)
-	vec3 m_world_gravity;
-	// vector of bodies in the system
-	// body 0 is used as an environment body and is allways fixed.
-	// The bodies are ordered such that a parent body always has an index
-	// smaller than its child.
-	idArray<RigidBody>::type m_body_list;
-	// Parent_index[i] is the index for i's parent body in body_list.
-	// This fully describes the tree.
-	idArray<int>::type m_parent_index;
-	// child_indices[i] contains a vector of indices of
-	// all children of the i-th body
-	idArray<idArray<int>::type>::type m_child_indices;
-	// Indices of rotary joints
-	idArray<int>::type m_body_revolute_list;
-	// Indices of prismatic joints
-	idArray<int>::type m_body_prismatic_list;
-	// Indices of floating joints
-	idArray<int>::type m_body_floating_list;
-	// Indices of spherical joints
-	idArray<int>::type m_body_spherical_list;
-	// a user-provided integer
-	idArray<int>::type m_user_int;
-	// a user-provided pointer
-	idArray<void*>::type m_user_ptr;
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	mat3x m_m3x;
-#endif
-};
-}  // namespace btInverseDynamics
-#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
deleted file mode 100644
index a718db051e..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-#include "MultiBodyTreeInitCache.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::InitCache::InitCache()
-{
-	m_inertias.resize(0);
-	m_joints.resize(0);
-	m_num_dofs = 0;
-	m_root_index = -1;
-}
-
-int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
-									  const JointType joint_type,
-									  const vec3& parent_r_parent_body_ref,
-									  const mat33& body_T_parent_ref,
-									  const vec3& body_axis_of_motion, const idScalar mass,
-									  const vec3& body_r_body_com, const mat33& body_I_body,
-									  const int user_int, void* user_ptr)
-{
-	switch (joint_type)
-	{
-		case REVOLUTE:
-		case PRISMATIC:
-			m_num_dofs += 1;
-			break;
-		case FIXED:
-			// does not add a degree of freedom
-			// m_num_dofs+=0;
-			break;
-		case SPHERICAL:
-			m_num_dofs += 3;
-			break;
-		case FLOATING:
-			m_num_dofs += 6;
-			break;
-		default:
-			bt_id_error_message("unknown joint type %d\n", joint_type);
-			return -1;
-	}
-
-	if (-1 == parent_index)
-	{
-		if (m_root_index >= 0)
-		{
-			bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
-								body_index, m_root_index);
-			return -1;
-		}
-		m_root_index = body_index;
-	}
-
-	JointData joint;
-	joint.m_child = body_index;
-	joint.m_parent = parent_index;
-	joint.m_type = joint_type;
-	joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
-	joint.m_child_T_parent_ref = body_T_parent_ref;
-	joint.m_child_axis_of_motion = body_axis_of_motion;
-
-	InertiaData body;
-	body.m_mass = mass;
-	body.m_body_pos_body_com = body_r_body_com;
-	body.m_body_I_body = body_I_body;
-
-	m_inertias.push_back(body);
-	m_joints.push_back(joint);
-	m_user_int.push_back(user_int);
-	m_user_ptr.push_back(user_ptr);
-	return 0;
-}
-int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const
-{
-	if (index < 0 || index > static_cast<int>(m_inertias.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-
-	*inertia = m_inertias[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const
-{
-	if (index < 0 || index > static_cast<int>(m_user_int.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*user_int = m_user_int[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
-{
-	if (index < 0 || index > static_cast<int>(m_user_ptr.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*user_ptr = m_user_ptr[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const
-{
-	if (index < 0 || index > static_cast<int>(m_joints.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*joint = m_joints[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::buildIndexSets()
-{
-	// NOTE: This function assumes that proper indices were provided
-	//	   User2InternalIndex from utils can be used to facilitate this.
-
-	m_parent_index.resize(numBodies());
-	for (idArrayIdx j = 0; j < m_joints.size(); j++)
-	{
-		const JointData& joint = m_joints[j];
-		m_parent_index[joint.m_child] = joint.m_parent;
-	}
-
-	return 0;
-}
-}  // namespace btInverseDynamics
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
deleted file mode 100644
index dbdb3ff604..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
+++ /dev/null
@@ -1,113 +0,0 @@
-#ifndef MULTIBODYTREEINITCACHE_HPP_
-#define MULTIBODYTREEINITCACHE_HPP_
-
-#include "../IDConfig.hpp"
-#include "../IDMath.hpp"
-#include "../MultiBodyTree.hpp"
-
-namespace btInverseDynamics
-{
-/// Mass properties of a rigid body
-struct InertiaData
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	/// mass
-	idScalar m_mass;
-	/// vector from body-fixed frame to center of mass,
-	/// in body-fixed frame, multiplied by the mass
-	vec3 m_body_pos_body_com;
-	/// moment of inertia w.r.t. the origin of the body-fixed
-	/// frame, represented in that frame
-	mat33 m_body_I_body;
-};
-
-/// Joint properties
-struct JointData
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	/// type of joint
-	JointType m_type;
-	/// index of parent body
-	int m_parent;
-	/// index of child body
-	int m_child;
-	/// vector from parent's body-fixed frame to child's body-fixed
-	/// frame for q=0, written in the parent's body fixed frame
-	vec3 m_parent_pos_parent_child_ref;
-	/// Transform matrix converting vectors written in the parent's frame
-	/// into vectors written in the child's frame for q=0
-	/// ie, child_vector = child_T_parent_ref * parent_vector;
-	mat33 m_child_T_parent_ref;
-	/// Axis of motion for 1 degree-of-freedom joints,
-	/// written in the child's frame
-	/// For revolute joints, the q-value is positive for a positive
-	/// rotation about this axis.
-	/// For prismatic joints, the q-value is positive for a positive
-	/// translation is this direction.
-	vec3 m_child_axis_of_motion;
-};
-
-/// Data structure to store data passed by the user.
-/// This is used in MultiBodyTree::finalize to build internal data structures.
-class MultiBodyTree::InitCache
-{
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	/// constructor
-	InitCache();
-	///\copydoc MultiBodyTree::addBody
-	int addBody(const int body_index, const int parent_index, const JointType joint_type,
-				const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
-				const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com,
-				const mat33 &body_I_body, const int user_int, void *user_ptr);
-	/// build index arrays
-	/// @return 0 on success, -1 on failure
-	int buildIndexSets();
-	/// @return number of degrees of freedom
-	int numDoFs() const { return m_num_dofs; }
-	/// @return number of bodies
-	int numBodies() const { return m_inertias.size(); }
-	/// get inertia data for index
-	/// @param index of the body
-	/// @param inertia pointer for return data
-	/// @return 0 on success, -1 on failure
-	int getInertiaData(const int index, InertiaData *inertia) const;
-	/// get joint data for index
-	/// @param index of the body
-	/// @param joint pointer for return data
-	/// @return 0 on success, -1 on failure
-	int getJointData(const int index, JointData *joint) const;
-	/// get parent index array (paren_index[i] is the index of the parent of i)
-	/// @param parent_index pointer for return data
-	void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
-	/// get user integer
-	/// @param index body index
-	/// @param user_int user integer
-	/// @return 0 on success, -1 on failure
-	int getUserInt(const int index, int *user_int) const;
-	/// get user pointer
-	/// @param index body index
-	/// @param user_int user pointer
-	/// @return 0 on success, -1 on failure
-	int getUserPtr(const int index, void **user_ptr) const;
-
-private:
-	// vector of bodies
-	idArray<InertiaData>::type m_inertias;
-	// vector of joints
-	idArray<JointData>::type m_joints;
-	// number of mechanical degrees of freedom
-	int m_num_dofs;
-	// parent index array
-	idArray<int>::type m_parent_index;
-	// user integers
-	idArray<int>::type m_user_int;
-	// user pointers
-	idArray<void *>::type m_user_ptr;
-	// index of root body (or -1 if not set)
-	int m_root_index;
-};
-}  // namespace btInverseDynamics
-#endif  // MULTIBODYTREEINITCACHE_HPP_