diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp
index e7d6c122f..97646dc43 100644
--- a/src/BulletInverseDynamics/IDConfig.hpp
+++ b/src/BulletInverseDynamics/IDConfig.hpp
@@ -1,5 +1,5 @@
 ///@file Configuration for Inverse Dynamics Library,
-///      such as choice of linear algebra library and underlying scalar type
+///	  such as choice of linear algebra library and underlying scalar type
 #ifndef IDCONFIG_HPP_
 #define IDCONFIG_HPP_
 // If we have a custom configuration, compile without using other parts of bullet.
@@ -8,6 +8,7 @@
 #define BT_ID_POW(a,b) std::pow(a,b)
 #define BT_ID_SNPRINTF snprintf
 #define BT_ID_PI M_PI
+#define BT_ID_USE_DOUBLE_PRECISION
 #else
 #define BT_ID_POW(a,b) btPow(a,b)
 #define BT_ID_PI SIMD_PI
@@ -33,6 +34,9 @@
 // Use the same scalar type as rest of bullet library
 #include "LinearMath/btScalar.h"
 typedef btScalar idScalar;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define BT_ID_USE_DOUBLE_PRECISION
+#endif
 // 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:
@@ -40,7 +44,7 @@ typedef btScalar idScalar;
 // using idArray = b3AlignedObjectArray<T>;
 template <typename T>
 struct idArray {
-    typedef b3AlignedObjectArray<T> type;
+	typedef b3AlignedObjectArray<T> type;
 };
 typedef int idArrayIdx;
 #define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR
diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp
index 3965bc5eb..130c19c6d 100644
--- a/src/BulletInverseDynamics/IDConfigBuiltin.hpp
+++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp
@@ -15,7 +15,7 @@ typedef float idScalar;
 // using idArray = std::vector<T>;
 template <typename T>
 struct idArray {
-    typedef std::vector<T> type;
+	typedef std::vector<T> type;
 };
 typedef std::vector<int>::size_type idArrayIdx;
 // default to standard malloc/free
@@ -23,15 +23,15 @@ typedef std::vector<int>::size_type idArrayIdx;
 #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*) {}
+#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/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp
index f0b72b425..a83ee8e3f 100644
--- a/src/BulletInverseDynamics/IDConfigEigen.hpp
+++ b/src/BulletInverseDynamics/IDConfigEigen.hpp
@@ -16,7 +16,7 @@ typedef float idScalar;
 // using idArray = std::vector<T>;
 template <typename T>
 struct idArray {
-    typedef std::vector<T> type;
+	typedef std::vector<T> type;
 };
 typedef std::vector<int>::size_type idArrayIdx;
 // default to standard malloc/free
@@ -25,15 +25,15 @@ typedef std::vector<int>::size_type idArrayIdx;
 
 #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*) {}
+#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*) {}
 
 // Note on interfaces:
 // Eigen::Matrix has data(), to get c-array storage
diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp
index a66ea8323..a3866edc5 100644
--- a/src/BulletInverseDynamics/IDErrorMessages.hpp
+++ b/src/BulletInverseDynamics/IDErrorMessages.hpp
@@ -13,22 +13,22 @@
 #else  // BT_ID_WO_BULLET
 #include <cstdio>
 /// print error message with file/line information
-#define error_message(...)                                                                         \
-    do {                                                                                           \
-        fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);                       \
-        fprintf(stderr, __VA_ARGS__);                                                              \
-    } while (0)
+#define 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 warning_message(...)                                                                       \
-    do {                                                                                           \
-        fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);                     \
-        fprintf(stderr, __VA_ARGS__);                                                              \
-    } while (0)
-#define warning_message(...)                                                                         \
-    do {                                                                                           \
-        fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);                       \
-        fprintf(stderr, __VA_ARGS__);                                                            \
-    } while (0)
+#define warning_message(...)																	   \
+	do {																						   \
+		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__);					 \
+		fprintf(stderr, __VA_ARGS__);															  \
+	} while (0)
+#define 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/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp
index a3daaf1f5..7ce055fdf 100644
--- a/src/BulletInverseDynamics/IDMath.cpp
+++ b/src/BulletInverseDynamics/IDMath.cpp
@@ -10,362 +10,362 @@ static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
 static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
 
 void setZero(vec3 &v) {
-    v(0) = 0;
-    v(1) = 0;
-    v(2) = 0;
+	v(0) = 0;
+	v(1) = 0;
+	v(2) = 0;
 }
 
 void setZero(vecx &v) {
-    for (int i = 0; i < v.size(); i++) {
-        v(i) = 0;
-    }
+	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;
+	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;
 }
 
 idScalar maxAbs(const vecx &v) {
-    idScalar result = 0.0;
-    for (int i = 0; i < v.size(); i++) {
-        const idScalar tmp = std::fabs(v(i));
-        if (tmp > result) {
-            result = tmp;
-        }
-    }
-    return result;
+	idScalar result = 0.0;
+	for (int i = 0; i < v.size(); i++) {
+		const idScalar tmp = std::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 = std::fabs(v(i));
-        if (tmp > result) {
-            result = tmp;
-        }
-    }
-    return result;
+	idScalar result = 0.0;
+	for (int i = 0; i < 3; i++) {
+		const idScalar tmp = std::fabs(v(i));
+		if (tmp > result) {
+			result = tmp;
+		}
+	}
+	return result;
 }
 
 mat33 transformX(const idScalar &alpha) {
-    mat33 T;
-    const idScalar cos_alpha = std::cos(alpha);
-    const idScalar sin_alpha = std::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;
+	mat33 T;
+	const idScalar cos_alpha = std::cos(alpha);
+	const idScalar sin_alpha = std::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(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;
+	T(2, 0) = 0.0;
+	T(2, 1) = -sin_alpha;
+	T(2, 2) = cos_alpha;
 
-    return T;
+	return T;
 }
 
 mat33 transformY(const idScalar &beta) {
-    mat33 T;
-    const idScalar cos_beta = std::cos(beta);
-    const idScalar sin_beta = std::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;
+	mat33 T;
+	const idScalar cos_beta = std::cos(beta);
+	const idScalar sin_beta = std::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(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;
+	T(2, 0) = sin_beta;
+	T(2, 1) = 0.0;
+	T(2, 2) = cos_beta;
 
-    return T;
+	return T;
 }
 
 mat33 transformZ(const idScalar &gamma) {
-    mat33 T;
-    const idScalar cos_gamma = std::cos(gamma);
-    const idScalar sin_gamma = std::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;
+	mat33 T;
+	const idScalar cos_gamma = std::cos(gamma);
+	const idScalar sin_gamma = std::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(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;
+	T(2, 0) = 0.0;
+	T(2, 1) = 0.0;
+	T(2, 2) = 1.0;
 
-    return T;
+	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;
+	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 = std::sin(alpha);
-    const idScalar ca = std::cos(alpha);
-    const idScalar st = std::sin(theta);
-    const idScalar ct = std::cos(theta);
+	const idScalar sa = std::sin(alpha);
+	const idScalar ca = std::cos(alpha);
+	const idScalar st = std::sin(theta);
+	const idScalar ct = std::cos(theta);
 
-    (*r)(0) = a;
-    (*r)(1) = -sa * d;
-    (*r)(2) = ca * d;
+	(*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)(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)(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;
+	(*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 = cos(angle);
-    const idScalar s = -sin(angle);
-    const idScalar one_m_c = 1.0 - c;
+	const idScalar c = cos(angle);
+	const idScalar s = -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);
+	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)(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)(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;
+	(*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;
+	// 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;
+	// 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;
+	// 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);
+	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)) {
-        error_message("invalid inertia matrix for body %d, not positive definite "
-                      "(fixed joint)\n",
-                      index);
-        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));
+	// 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)) {
+		error_message("invalid inertia matrix for body %d, not positive definite "
+					  "(fixed joint)\n",
+					  index);
+		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;
-    }
+		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)) {
-            error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-            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)) {
-            error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-            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)) {
-            error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
-            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
-            error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
-            return false;
-        }
-    }
-    // check symmetry
-    if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
-        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 (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
-        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 (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
-        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;
+	// 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)) {
+			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			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)) {
+			error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
+			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)) {
+			error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
+			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
+			error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
+			return false;
+		}
+	}
+	// check symmetry
+	if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
+		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 (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
+		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 (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
+		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)                                                                               \
-    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))
+#define print_mat(x)																			   \
+	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 =
-            std::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) {
-            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 (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
-        error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
-        print_mat(m);
-        return false;
-    }
-    if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
-        error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
-        print_mat(m);
-        return false;
-    }
-    if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
-        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) {
-        error_message("Not a valid rotation matrix (determinant <=0)\n");
-        print_mat(m);
-        return false;
-    }
-    return true;
+	// check for unit length column vectors
+	for (int i = 0; i < 3; i++) {
+		const idScalar length_minus_1 =
+			std::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) {
+			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 (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
+		error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
+		print_mat(m);
+		return false;
+	}
+	if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
+		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
+		print_mat(m);
+		return false;
+	}
+	if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
+		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) {
+		error_message("Not a valid rotation matrix (determinant <=0)\n");
+		print_mat(m);
+		return false;
+	}
+	return true;
 }
 
 bool isUnitVector(const vec3 &vector) {
-    return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
-           kIsZero;
+	return std::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) = std::atan2(-rot(1, 0), rot(0, 0));
-    rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0));
-    rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
-    return rpy;
+	vec3 rpy;
+	rpy(2) = std::atan2(-rot(1, 0), rot(0, 0));
+	rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0));
+	rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
+	return rpy;
 }
 }
diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp
index 97e60c120..f7526b500 100644
--- a/src/BulletInverseDynamics/IDMath.hpp
+++ b/src/BulletInverseDynamics/IDMath.hpp
@@ -1,5 +1,5 @@
 /// @file Math utility functions used in inverse dynamics library.
-///       Defined here as they may not be provided by the math library.
+///	   Defined here as they may not be provided by the math library.
 
 #ifndef IDMATH_HPP_
 #define IDMATH_HPP_
@@ -39,7 +39,7 @@ 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
+///	   will be implemented differently for various matrix/vector libraries
 /// @param m a 3x3 matrix
 /// @return det(m)
 idScalar determinant(const mat33& m);
diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp
index 5c0af64e9..f5488b62a 100644
--- a/src/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.cpp
@@ -11,54 +11,54 @@
 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();
+	: 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;
+	delete m_impl;
+	delete m_init_cache;
 }
 
 void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) {
-    m_accept_invalid_mass_parameters = flag;
+	m_accept_invalid_mass_parameters = flag;
 }
 
 bool MultiBodyTree::getAcceptInvalidMassProperties() const {
-    return m_accept_invalid_mass_parameters;
+	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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
 }
 
 void MultiBodyTree::printTree() { m_impl->printTree(); }
@@ -69,263 +69,263 @@ 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) {
-        error_message("system has not been initialized\n");
-        return -1;
-    }
-    if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
-        error_message("error in inverse dynamics calculation\n");
-        return -1;
-    }
-    return 0;
+											vecx *joint_forces) {
+	if (false == m_is_finalized) {
+		error_message("system has not been initialized\n");
+		return -1;
+	}
+	if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) {
+		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) {
-        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)) {
-        error_message("error in mass matrix calculation\n");
-        return -1;
-    }
-    return 0;
+									   const bool initialize_matrix,
+									   const bool set_lower_triangular_matrix, matxx *mass_matrix) {
+	if (false == m_is_finalized) {
+		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)) {
+		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);
+	return calculateMassMatrix(q, true, true, true, mass_matrix);
 }
 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) {
-        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)) {
-                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 = std::sqrt(std::pow(body_axis_of_motion(0), 2) +
-                                            std::pow(body_axis_of_motion(1), 2) +
-                                            std::pow(body_axis_of_motion(2), 2));
-                if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
-                    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;
-        default:
-            error_message("unknown joint type %d\n", joint_type);
-            return -1;
-    }
+						   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) {
+		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)) {
+				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 = std::sqrt(std::pow(body_axis_of_motion(0), 2) +
+											std::pow(body_axis_of_motion(1), 2) +
+											std::pow(body_axis_of_motion(2), 2));
+				if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
+					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;
+		default:
+			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;
-        error_message("Body %d has invalid mass %e\n", body_index, mass);
-        if (!m_accept_invalid_mass_parameters) {
-            return -1;
-        }
-    }
+	// sanity check for mass properties. Zero mass is OK.
+	if (mass < 0) {
+		m_mass_parameters_are_valid = false;
+		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 (!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;
-    }
+	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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	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();
+	const int &num_bodies = m_init_cache->numBodies();
+	const int &num_dofs = m_init_cache->numDoFs();
 
-    // 1 allocate internal MultiBody structure
-    m_impl = new MultiBodyImpl(num_bodies, num_dofs);
+	// 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);
+	// 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;
-        }
+	// 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];
+		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;
+		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;
 
-        // 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 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:
-                error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
-                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 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:
+				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()) {
-        error_message("generating index sets\n");
-        return -1;
-    }
+	// 4 assign degree of freedom indices & build per-joint-type index arrays
+	if (-1 == m_impl->generateIndexSets()) {
+		error_message("generating index sets\n");
+		return -1;
+	}
 
-    // 5 do some pre-computations ..
-    m_impl->calculateStaticData();
+	// 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();
+	// 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;
+	m_is_finalized = true;
+	return 0;
 }
 
 int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) {
-    return m_impl->setGravityInWorldFrame(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);
+	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);
+	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);
+	return m_impl->getDoFOffset(body_index, q_offset);
 }
 
 int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) {
-    return m_impl->setBodyMass(body_index, 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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	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);
+	return m_impl->addUserMoment(body_index, body_moment);
 }
 
 }
diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp
index 9f3400b40..d33e60e12 100644
--- a/src/BulletInverseDynamics/MultiBodyTree.hpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.hpp
@@ -8,14 +8,14 @@ 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
+	/// 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
 };
 
 /// Interface class for calculating inverse dynamics for tree structured
@@ -25,284 +25,284 @@ enum JointType {
 /// 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)
+///	- 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)
 /// 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]
+///	- 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]
 ///
 /// 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)
+///	 - 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)
 ///
 /// TODO - force element interface (friction, springs, dampers, etc)
-///      - gears and motor inertia
+///	  - gears and motor inertia
 class MultiBodyTree {
 public:
-    /// The contructor.
-    /// Initialization & allocation is via addBody and buildSystem calls.
-    MultiBodyTree();
-    /// the destructor. This also deallocates all memory
-    ~MultiBodyTree();
+	/// 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
-    /// @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);
+	/// 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
+	/// @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);
+	/// 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);
 
-    /// 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;
-    /// returns the (internal) index of body
-    /// @param body_index is the index of a body (internal: TODO: fix/clarify
-    /// indexing!)
-    /// @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 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);
+	/// 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;
+	/// returns the (internal) index of body
+	/// @param body_index is the index of a body (internal: TODO: fix/clarify
+	/// indexing!)
+	/// @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 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;
+	// 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/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
index 2209121ba..c77ba3cba 100644
--- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -17,29 +17,29 @@ 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]; }
-    const int size() const { return 3; }
-    const vec3& operator=(const btVector3& rhs) {
-        *static_cast<btVector3*>(this) = rhs;
-        return *this;
-    }
+	vec3() : btVector3() {}
+	vec3(const btVector3& btv) { *this = btv; }
+	idScalar& operator()(int i) { return (*this)[i]; }
+	const idScalar& operator()(int i) const { return (*this)[i]; }
+	const 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);
+	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); }
@@ -48,64 +48,64 @@ 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;
-    }
+	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]; }
+	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 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);
+	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;
+	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);
-    }
+	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;
+	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;
+	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;
-    }
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result(i) = a(i) / s;
+	}
 
-    return result;
+	return result;
 }
 }
 
diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp
index a4e2ca5a4..de15e9ed4 100644
--- a/src/BulletInverseDynamics/details/IDMatVec.hpp
+++ b/src/BulletInverseDynamics/details/IDMatVec.hpp
@@ -17,312 +17,312 @@ class matxx;
 /// 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;
+	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 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);
+	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];
+	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);
+	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);
+	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];
+	// 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; }
+	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 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);
+	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;
+	int m_size;
+	idScalar* m_data;
 };
 
 class matxx {
 public:
-    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); }
-    const matxx& operator=(const matxx& rhs);
-    idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
-    const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; }
-    const int& rows() const { return m_rows; }
-    const int& cols() const { return m_cols; }
+	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); }
+	const matxx& operator=(const matxx& rhs);
+	idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; }
+	const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + 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;
+	int m_rows;
+	int m_cols;
+	idScalar* m_data;
 };
 
 inline const vec3& vec3::operator=(const vec3& rhs) {
-    if (&rhs != this) {
-        memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
-    }
-    return *this;
+	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];
+	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;
+	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];
+	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;
+	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];
+	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;
+	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];
+	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;
+	return result;
 }
 
 inline const mat33& mat33::operator+=(const mat33& b) {
-    for (int i = 0; i < 9; i++) {
-        m_data[i] += b.m_data[i];
-    }
+	for (int i = 0; i < 9; i++) {
+		m_data[i] += b.m_data[i];
+	}
 
-    return *this;
+	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;
+	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;
+	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];
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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];
-    }
+	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;
+	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;
+	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;
-    }
+	vecx result(a.size());
+	for (int i = 0; i < result.size(); i++) {
+		result.m_data[i] = a.m_data[i] / s;
+	}
 
-    return result;
+	return result;
 }
 }
 #endif
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
index 211139459..06a3595be 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
@@ -3,482 +3,482 @@
 namespace btInverseDynamics {
 
 MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
-    : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
-    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_num_bodies(num_bodies_), m_num_dofs(num_dofs_) {
+	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;
+	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";
+	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
+	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);
+	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));
+	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("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));
-    }
+		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;
+	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();
+	// 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;
+	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);
-    }
+	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;
+	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_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());
+	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;
-        }
-    }
+	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;
+	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;
-        }
-    }
+	// 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;
+		}
+	}
 }
 
 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. 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;
+														   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. 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;
 
-        // body.m_parent_r_parent_body= fixed
-        body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
-        // body.m_parent_dot_r_rel = fixed;
-        // NOTE: this assumes that Jac_JR is constant, which is true for revolute
-        // joints, but not in the general case (eg, slider-crank type mechanisms)
-        body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
-        // body.m_parent_ddot_r_rel = fixed;
-    }
-    // 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_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);
-        // body.m_parent_omega_rel = 0;
-        body.m_parent_vel_rel =
-            body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
-        // body.parent_dot_omega_rel = 0;
-        // NOTE: this assumes that Jac_JT is constant, which is true for
-        // prismatic joints, but not in the general case
-        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_parent_r_parent_body= fixed
+		body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
+		// body.m_parent_dot_r_rel = fixed;
+		// NOTE: this assumes that Jac_JR is constant, which is true for revolute
+		// joints, but not in the general case (eg, slider-crank type mechanisms)
+		body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
+		// body.m_parent_ddot_r_rel = fixed;
+	}
+	// 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_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);
+		// body.m_parent_omega_rel = 0;
+		body.m_parent_vel_rel =
+			body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
+		// body.parent_dot_omega_rel = 0;
+		// NOTE: this assumes that Jac_JT is constant, which is true for
+		// prismatic joints, but not in the general case
+		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_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_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_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(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_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_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(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_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-        body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
-        body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
-    }
+		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+		body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
+		body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
+	}
 
-    // 3. absolute kinematic quantities & dynamic quantities
-    // NOTE: this should be optimized by specializing for different body types
-    // (e.g., relative rotation is always zero for prismatic joints, etc.)
+	// 3. absolute kinematic quantities & dynamic quantities
+	// 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;
+	// 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;
 
-        // 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;
+		// 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;
 
-        // 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;
-    }
+		// 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]];
-        // 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 * (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;
+	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]];
+		// 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 * (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;
 
-        // 3.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;
+		// 3.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);
+		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);
 
-        // 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_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);
-    }
+		// 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_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);
+	}
 
-    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;
-    }
+	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;
+	}
 
-    // 4. 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.
+	// 4. 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];
+	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;
-    }
+		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);
+	// 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);
-    }
+		(*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;
+	return 0;
 }
 
 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;
-    }
+	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();
+	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();
 }
 
 int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
-                                                      const bool initialize_matrix,
-                                                      const bool set_lower_triangular_matrix,
-                                                      matxx *mass_matrix) {
+													  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)
@@ -494,351 +494,351 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 #define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val
 #endif
 
-    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;
-    }
+	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++) {
-                setMassMatrixElem(i, j, 0.0);
-            }
-        }
-    }
+	// 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++) {
+				setMassMatrixElem(i, j, 0.0);
+			}
+		}
+	}
 
-    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]];
+	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_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;
+			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();
+		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;
+			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);
-            }
-        }
-    }
+			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];
+	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);
-            }
+		// 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);
-            setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
+			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);
+			setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans));
 
-            // 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);
-                    setMassMatrixElem(col, row, Mrc);
-                }
-                // 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];
+			// 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);
+					setMassMatrixElem(col, row, Mrc);
+				}
+				// 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 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);
-                        setMassMatrixElem(col, row, Mrc);
-                    }
+					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);
+						setMassMatrixElem(col, row, Mrc);
+					}
 
-                    child_idx = parent_idx;
-                    parent_idx = m_parent_index[child_idx];
-                }
-            }
-        }
-    }
+					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++) {
-                setMassMatrixElem(row, col, (*mass_matrix)(col, row));
-            }
-        }
-    }
+	if (set_lower_triangular_matrix) {
+		for (int col = 0; col < m_num_dofs; col++) {
+			for (int row = 0; row < col; row++) {
+				setMassMatrixElem(row, col, (*mass_matrix)(col, row));
+			}
+		}
+	}
 
 #undef setMassMatrixElem
-    return 0;
+	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)
+#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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+														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;
-    }
+														   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;
+	*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;
+															 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;
+															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;
+	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;
+												  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::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;
+	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;
+	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;
+														 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;
+														  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;
+	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;
+														 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;
+														  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);
-    }
+	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;
+	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;
+	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
+	m_body_list[body_index].m_body_moment_user += body_moment;
+	return 0;
 }
 
 }
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
index 286b19d26..7787a4665 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
@@ -14,223 +14,223 @@ namespace btInverseDynamics {
 /// 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;
+	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;
+	// 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;
+	// 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;
+	// 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;
+	// 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;
 };
 
 /// The MBS implements a tree structured multibody system
 class MultiBodyTree::MultiBodyImpl {
-    friend class MultiBodyTree;
+	friend class MultiBodyTree;
 
 public:
-    ID_DECLARE_ALIGNED_ALLOCATOR();
+	ID_DECLARE_ALIGNED_ALLOCATOR();
 
-    /// 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_);
+	/// 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);
-    /// 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: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);
+	/// \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);
+	/// 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: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;
+	// 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;
 };
 }
 #endif
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
index 37063bfd7..47b4ab389 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
@@ -3,111 +3,111 @@
 namespace btInverseDynamics {
 
 MultiBodyTree::InitCache::InitCache() {
-    m_inertias.resize(0);
-    m_joints.resize(0);
-    m_num_dofs = 0;
-    m_root_index=-1;
+	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;
-    }
+									  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;
-    }
+	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;
+	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;
+	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;
+	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;
-    }
+	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;
+	*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;
+	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;
+	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;
+	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.
+	// 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;
-    }
+	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;
+	return 0;
 }
 }
diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
index a46cc16bb..0d2aa4a07 100644
--- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp
@@ -8,102 +8,102 @@
 namespace btInverseDynamics {
 /// Mass properties of a rigid body
 struct InertiaData {
-    ID_DECLARE_ALIGNED_ALLOCATOR();
+	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;
+	/// 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();
+	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;
+	/// 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;
+	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;
+	// 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_
diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp
index 4ee728a0d..d067123e9 100644
--- a/test/InverseDynamics/test_invdyn_kinematics.cpp
+++ b/test/InverseDynamics/test_invdyn_kinematics.cpp
@@ -248,7 +248,11 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar
 // first test: absolute difference between numerical and numerial
 // differentiation should be small
 TEST(InvDynKinematicsDifferentiation, errorAbsolute) {
+#ifdef BT_ID_USE_DOUBLE_PRECISION
     const idScalar kDeltaT = 1e-7;
+#else
+	const idScalar kDeltaT = 1e-2;
+#endif
     const idScalar kDuration = 1e-2;
     const idScalar kAcceptableError = 1e-4;