bullet: Streamline bundling, remove extraneous src/ folder

Document version and how to extract sources in thirdparty/README.md.
Drop unnecessary CMake and Premake files.
Simplify SCsub, drop unused one.

7 files changed, 2156 insertions, 0 deletions

diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp
new file mode 100644
index 0000000000..836395cea2
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp
@@ -0,0 +1,36 @@
+#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;
+}
+
+}
+#endif  // INVDYNEIGENINTERFACE_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
new file mode 100644
index 0000000000..5bb4a33bdd
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -0,0 +1,172 @@
+#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(size) {}
+	const vecx& operator=(const btVectorX<idScalar>& rhs) {
+		*static_cast<btVectorX*>(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()) {
+		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()) {
+		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) {
+            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()) {
+        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);
+}
+
+}
+
+#endif  // IDLINEARMATHINTERFACE_HPP_
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
new file mode 100644
index 0000000000..4d3f6c87e9
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp
@@ -0,0 +1,415 @@
+/// @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) {
+            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()) {
+		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()) {
+		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()) {
+		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()) {
+        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 btInverseDynamcis
+#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
new file mode 100644
index 0000000000..b35c55df61
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -0,0 +1,1028 @@
+#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";
+	}
+	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;
+	}
+	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;
+			default:
+				error_message("unsupported joint type %d\n", body.m_joint_type);
+				return -1;
+		}
+	}
+	// sanity check
+	if (q_index != m_num_dofs) {
+		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
+				error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
+			} else {
+				// should never happen
+				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;
+		}
+
+       // 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) {
+		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)) {
+            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);
+	}
+
+	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 ) {
+		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) {
+            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;
+                }
+	}
+
+	// 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;
+    }
+}
+
+int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) {
+    if (q.size() != m_num_dofs || u.size() != m_num_dofs) {
+        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) {
+        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 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;
+	}
+	// this should never happen
+	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) {
+		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 (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);
+			}
+
+			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 (FLOATING != body.m_joint_type) {
+						error_message("??\n");
+						return -1;
+					}
+					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--) {
+						// 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) {												  \
+			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
+}
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
new file mode 100644
index 0000000000..3efe9d0492
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
@@ -0,0 +1,283 @@
+// 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;
+	// 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
+};
+}
+#endif
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
new file mode 100644
index 0000000000..47b4ab3890
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
@@ -0,0 +1,113 @@
+#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 FLOATING:
+			m_num_dofs += 6;
+			break;
+		default:
+			error_message("unknown joint type %d\n", joint_type);
+			return -1;
+	}
+
+	if(-1 == parent_index) {
+		if(m_root_index>=0) {
+			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())) {
+		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())) {
+		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())) {
+		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())) {
+		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;
+}
+}
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
new file mode 100644
index 0000000000..0d2aa4a071
--- /dev/null
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
@@ -0,0 +1,109 @@
+#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;
+};
+}
+#endif  // MULTIBODYTREEINITCACHE_HPP_