Bullet: Don't include unused BulletInverseDynamics

Follow-up to #63143.

modules/bullet/SCsub

@@ -158,10 +158,7 @@ if env["builtin_bullet"]:
         "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp",
         "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp",
         # BulletInverseDynamics
-        "BulletInverseDynamics/IDMath.cpp",
-        "BulletInverseDynamics/MultiBodyTree.cpp",
-        "BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp",
-        "BulletInverseDynamics/details/MultiBodyTreeImpl.cpp",
+        # -- Unused for now so not included.
         # BulletSoftBody
         "BulletSoftBody/btSoftBody.cpp",
         "BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp",
@@ -204,7 +201,7 @@ if env["builtin_bullet"]:
 
     env_bullet.Prepend(CPPPATH=[thirdparty_dir])
 
-    env_bullet.Append(CPPDEFINES=["BT_USE_OLD_DAMPING_METHOD", "BT_THREADSAFE", "BT_USE_INVERSE_DYNAMICS_WITH_BULLET2"])
+    env_bullet.Append(CPPDEFINES=["BT_USE_OLD_DAMPING_METHOD", "BT_THREADSAFE"])
 
     env_thirdparty = env_bullet.Clone()
     env_thirdparty.disable_warnings()

thirdparty/README.md

@@ -25,7 +25,8 @@ Files extracted from upstream source:
 
 Files extracted from upstream source:
 
-- `src/*` minus `Bullet3*` and `clew` folders, and CMakeLists.txt and premake4.lua files
+- `src/*` minus `Bullet3*`, `BulletInverseDynamics` and `clew` folders,
+  and CMakeLists.txt and premake4.lua files
 - `LICENSE.txt`, and `VERSION` as `VERSION.txt`
 
 Includes some patches in the `patches` folder which have been sent upstream.

thirdparty/bullet/BulletInverseDynamics/IDConfig.hpp

@@ -1,107 +0,0 @@
-///@file Configuration for Inverse Dynamics Library,
-///	  such as choice of linear algebra library and underlying scalar type
-#ifndef IDCONFIG_HPP_
-#define IDCONFIG_HPP_
-
-// If true, enable jacobian calculations.
-// This adds a 3xN matrix to every body, + 2 3-Vectors.
-// so it is not advised for large systems if it is not absolutely necessary.
-// Also, this is not required for standard inverse dynamics calculations.
-// Will only work with vector math libraries that support 3xN matrices.
-#define BT_ID_WITH_JACOBIANS
-
-// If we have a custom configuration, compile without using other parts of bullet.
-#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
-#include <cmath>
-#define BT_ID_WO_BULLET
-#define BT_ID_SQRT(x) std::sqrt(x)
-#define BT_ID_FABS(x) std::fabs(x)
-#define BT_ID_COS(x) std::cos(x)
-#define BT_ID_SIN(x) std::sin(x)
-#define BT_ID_ATAN2(x, y) std::atan2(x, y)
-#define BT_ID_POW(x, y) std::pow(x, y)
-#define BT_ID_SNPRINTF snprintf
-#define BT_ID_PI M_PI
-#define BT_ID_USE_DOUBLE_PRECISION
-#else
-#define BT_ID_SQRT(x) btSqrt(x)
-#define BT_ID_FABS(x) btFabs(x)
-#define BT_ID_COS(x) btCos(x)
-#define BT_ID_SIN(x) btSin(x)
-#define BT_ID_ATAN2(x, y) btAtan2(x, y)
-#define BT_ID_POW(x, y) btPow(x, y)
-#define BT_ID_PI SIMD_PI
-#ifdef _WIN32
-#define BT_ID_SNPRINTF _snprintf
-#else
-#define BT_ID_SNPRINTF snprintf
-#endif  //
-#endif
-// error messages
-#include "IDErrorMessages.hpp"
-
-#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
-/*
-#include "IDConfigEigen.hpp"
-#include "IDConfigBuiltin.hpp"
-*/
-#define INVDYN_INCLUDE_HELPER_2(x) #x
-#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x)
-#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H)
-#ifndef btInverseDynamics
-#error "custom inverse dynamics config, but no custom namespace defined"
-#endif
-
-#define BT_ID_MAX(a, b) std::max(a, b)
-#define BT_ID_MIN(a, b) std::min(a, b)
-
-#else
-#define btInverseDynamics btInverseDynamicsBullet3
-// Use default configuration with bullet's types
-// Use the same scalar type as rest of bullet library
-#include "LinearMath/btScalar.h"
-typedef btScalar idScalar;
-#include "LinearMath/btMinMax.h"
-#define BT_ID_MAX(a, b) btMax(a, b)
-#define BT_ID_MIN(a, b) btMin(a, b)
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define BT_ID_USE_DOUBLE_PRECISION
-#endif
-
-#ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-// use bullet types for arrays and array indices
-#include "Bullet3Common/b3AlignedObjectArray.h"
-// this is to make it work with C++2003, otherwise we could do this:
-// template <typename T>
-// using idArray = b3AlignedObjectArray<T>;
-template <typename T>
-struct idArray
-{
-	typedef b3AlignedObjectArray<T> type;
-};
-typedef int idArrayIdx;
-#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR()
-
-#else  // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-#include "LinearMath/btAlignedObjectArray.h"
-template <typename T>
-struct idArray
-{
-	typedef btAlignedObjectArray<T> type;
-};
-typedef int idArrayIdx;
-#define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR()
-
-#endif  // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2
-
-// use bullet's allocator functions
-#define idMalloc btAllocFunc
-#define idFree btFreeFunc
-
-#define ID_LINEAR_MATH_USE_BULLET
-#include "details/IDLinearMathInterface.hpp"
-#endif
-#endif

thirdparty/bullet/BulletInverseDynamics/IDConfigBuiltin.hpp

@@ -1,38 +0,0 @@
-///@file Configuration for Inverse Dynamics Library without external dependencies
-#ifndef INVDYNCONFIG_BUILTIN_HPP_
-#define INVDYNCONFIG_BUILTIN_HPP_
-#define btInverseDynamics btInverseDynamicsBuiltin
-#ifdef BT_USE_DOUBLE_PRECISION
-// choose double/single precision version
-typedef double idScalar;
-#else
-typedef float idScalar;
-#endif
-// use std::vector for arrays
-#include <vector>
-// this is to make it work with C++2003, otherwise we could do this
-// template <typename T>
-// using idArray = std::vector<T>;
-template <typename T>
-struct idArray
-{
-	typedef std::vector<T> type;
-};
-typedef std::vector<int>::size_type idArrayIdx;
-// default to standard malloc/free
-#include <cstdlib>
-#define idMalloc ::malloc
-#define idFree ::free
-// currently not aligned at all...
-#define ID_DECLARE_ALIGNED_ALLOCATOR()                                                     \
-	inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); }   \
-	inline void operator delete(void* ptr) { idFree(ptr); }                                \
-	inline void* operator new(std::size_t, void* ptr) { return ptr; }                      \
-	inline void operator delete(void*, void*) {}                                           \
-	inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \
-	inline void operator delete[](void* ptr) { idFree(ptr); }                              \
-	inline void* operator new[](std::size_t, void* ptr) { return ptr; }                    \
-	inline void operator delete[](void*, void*) {}
-
-#include "details/IDMatVec.hpp"
-#endif

thirdparty/bullet/BulletInverseDynamics/IDConfigEigen.hpp

@@ -1,32 +0,0 @@
-///@file Configuration for Inverse Dynamics Library with Eigen
-#ifndef INVDYNCONFIG_EIGEN_HPP_
-#define INVDYNCONFIG_EIGEN_HPP_
-#define btInverseDynamics btInverseDynamicsEigen
-#ifdef BT_USE_DOUBLE_PRECISION
-// choose double/single precision version
-typedef double idScalar;
-#else
-typedef float idScalar;
-#endif
-
-// use std::vector for arrays
-#include <vector>
-// this is to make it work with C++2003, otherwise we could do this
-// template <typename T>
-// using idArray = std::vector<T>;
-template <typename T>
-struct idArray
-{
-	typedef std::vector<T> type;
-};
-typedef std::vector<int>::size_type idArrayIdx;
-// default to standard malloc/free
-#include <cstdlib>
-#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-// Note on interfaces:
-// Eigen::Matrix has data(), to get c-array storage
-// HOWEVER: default storage is column-major!
-#define ID_LINEAR_MATH_USE_EIGEN
-#include "Eigen/Eigen"
-#include "details/IDEigenInterface.hpp"
-#endif

thirdparty/bullet/BulletInverseDynamics/IDErrorMessages.hpp

@@ -1,31 +0,0 @@
-///@file error message utility functions
-#ifndef IDUTILS_HPP_
-#define IDUTILS_HPP_
-#include <cstring>
-/// name of file being compiled, without leading path components
-#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
-
-#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2)
-#include "Bullet3Common/b3Logging.h"
-#define bt_id_error_message(...) b3Error(__VA_ARGS__)
-#define bt_id_warning_message(...) b3Warning(__VA_ARGS__)
-#define id_printf(...) b3Printf(__VA_ARGS__)
-#else  // BT_ID_WO_BULLET
-#include <cstdio>
-/// print error message with file/line information
-#define bt_id_error_message(...)                                             \
-	do                                                                       \
-	{                                                                        \
-		fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
-		fprintf(stderr, __VA_ARGS__);                                        \
-	} while (0)
-/// print warning message with file/line information
-#define bt_id_warning_message(...)                                             \
-	do                                                                         \
-	{                                                                          \
-		fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \
-		fprintf(stderr, __VA_ARGS__);                                          \
-	} while (0)
-#define id_printf(...) printf(__VA_ARGS__)
-#endif  // BT_ID_WO_BULLET
-#endif

thirdparty/bullet/BulletInverseDynamics/IDMath.cpp

@@ -1,510 +0,0 @@
-#include "IDMath.hpp"
-
-#include <cmath>
-#include <limits>
-
-namespace btInverseDynamics
-{
-static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
-// requirements for axis length deviation from 1.0
-// experimentally set from random euler angle rotation matrices
-static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
-
-void setZero(vec3 &v)
-{
-	v(0) = 0;
-	v(1) = 0;
-	v(2) = 0;
-}
-
-void setZero(vecx &v)
-{
-	for (int i = 0; i < v.size(); i++)
-	{
-		v(i) = 0;
-	}
-}
-
-void setZero(mat33 &m)
-{
-	m(0, 0) = 0;
-	m(0, 1) = 0;
-	m(0, 2) = 0;
-	m(1, 0) = 0;
-	m(1, 1) = 0;
-	m(1, 2) = 0;
-	m(2, 0) = 0;
-	m(2, 1) = 0;
-	m(2, 2) = 0;
-}
-
-void skew(vec3 &v, mat33 *result)
-{
-	(*result)(0, 0) = 0.0;
-	(*result)(0, 1) = -v(2);
-	(*result)(0, 2) = v(1);
-	(*result)(1, 0) = v(2);
-	(*result)(1, 1) = 0.0;
-	(*result)(1, 2) = -v(0);
-	(*result)(2, 0) = -v(1);
-	(*result)(2, 1) = v(0);
-	(*result)(2, 2) = 0.0;
-}
-
-idScalar maxAbs(const vecx &v)
-{
-	idScalar result = 0.0;
-	for (int i = 0; i < v.size(); i++)
-	{
-		const idScalar tmp = BT_ID_FABS(v(i));
-		if (tmp > result)
-		{
-			result = tmp;
-		}
-	}
-	return result;
-}
-
-idScalar maxAbs(const vec3 &v)
-{
-	idScalar result = 0.0;
-	for (int i = 0; i < 3; i++)
-	{
-		const idScalar tmp = BT_ID_FABS(v(i));
-		if (tmp > result)
-		{
-			result = tmp;
-		}
-	}
-	return result;
-}
-
-#if (defined BT_ID_HAVE_MAT3X)
-idScalar maxAbsMat3x(const mat3x &m)
-{
-	// only used for tests -- so just loop here for portability
-	idScalar result = 0.0;
-	for (idArrayIdx col = 0; col < m.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			result = BT_ID_MAX(result, std::fabs(m(row, col)));
-		}
-	}
-	return result;
-}
-
-void mul(const mat33 &a, const mat3x &b, mat3x *result)
-{
-	if (b.cols() != result->cols())
-	{
-		bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
-							static_cast<int>(b.cols()), static_cast<int>(result->cols()));
-		abort();
-	}
-
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col);
-		const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col);
-		const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col);
-		setMat3xElem(0, col, x, result);
-		setMat3xElem(1, col, y, result);
-		setMat3xElem(2, col, z, result);
-	}
-}
-void add(const mat3x &a, const mat3x &b, mat3x *result)
-{
-	if (a.cols() != b.cols())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
-							static_cast<int>(a.cols()), static_cast<int>(b.cols()));
-		abort();
-	}
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			setMat3xElem(row, col, a(row, col) + b(row, col), result);
-		}
-	}
-}
-void sub(const mat3x &a, const mat3x &b, mat3x *result)
-{
-	if (a.cols() != b.cols())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
-							static_cast<int>(a.cols()), static_cast<int>(b.cols()));
-		abort();
-	}
-	for (idArrayIdx col = 0; col < b.cols(); col++)
-	{
-		for (idArrayIdx row = 0; row < 3; row++)
-		{
-			setMat3xElem(row, col, a(row, col) - b(row, col), result);
-		}
-	}
-}
-#endif
-
-mat33 transformX(const idScalar &alpha)
-{
-	mat33 T;
-	const idScalar cos_alpha = BT_ID_COS(alpha);
-	const idScalar sin_alpha = BT_ID_SIN(alpha);
-	// [1  0 0]
-	// [0  c s]
-	// [0 -s c]
-	T(0, 0) = 1.0;
-	T(0, 1) = 0.0;
-	T(0, 2) = 0.0;
-
-	T(1, 0) = 0.0;
-	T(1, 1) = cos_alpha;
-	T(1, 2) = sin_alpha;
-
-	T(2, 0) = 0.0;
-	T(2, 1) = -sin_alpha;
-	T(2, 2) = cos_alpha;
-
-	return T;
-}
-
-mat33 transformY(const idScalar &beta)
-{
-	mat33 T;
-	const idScalar cos_beta = BT_ID_COS(beta);
-	const idScalar sin_beta = BT_ID_SIN(beta);
-	// [c 0 -s]
-	// [0 1  0]
-	// [s 0  c]
-	T(0, 0) = cos_beta;
-	T(0, 1) = 0.0;
-	T(0, 2) = -sin_beta;
-
-	T(1, 0) = 0.0;
-	T(1, 1) = 1.0;
-	T(1, 2) = 0.0;
-
-	T(2, 0) = sin_beta;
-	T(2, 1) = 0.0;
-	T(2, 2) = cos_beta;
-
-	return T;
-}
-
-mat33 transformZ(const idScalar &gamma)
-{
-	mat33 T;
-	const idScalar cos_gamma = BT_ID_COS(gamma);
-	const idScalar sin_gamma = BT_ID_SIN(gamma);
-	// [ c s 0]
-	// [-s c 0]
-	// [ 0 0 1]
-	T(0, 0) = cos_gamma;
-	T(0, 1) = sin_gamma;
-	T(0, 2) = 0.0;
-
-	T(1, 0) = -sin_gamma;
-	T(1, 1) = cos_gamma;
-	T(1, 2) = 0.0;
-
-	T(2, 0) = 0.0;
-	T(2, 1) = 0.0;
-	T(2, 2) = 1.0;
-
-	return T;
-}
-
-mat33 tildeOperator(const vec3 &v)
-{
-	mat33 m;
-	m(0, 0) = 0.0;
-	m(0, 1) = -v(2);
-	m(0, 2) = v(1);
-	m(1, 0) = v(2);
-	m(1, 1) = 0.0;
-	m(1, 2) = -v(0);
-	m(2, 0) = -v(1);
-	m(2, 1) = v(0);
-	m(2, 2) = 0.0;
-	return m;
-}
-
-void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T)
-{
-	const idScalar sa = BT_ID_SIN(alpha);
-	const idScalar ca = BT_ID_COS(alpha);
-	const idScalar st = BT_ID_SIN(theta);
-	const idScalar ct = BT_ID_COS(theta);
-
-	(*r)(0) = a;
-	(*r)(1) = -sa * d;
-	(*r)(2) = ca * d;
-
-	(*T)(0, 0) = ct;
-	(*T)(0, 1) = -st;
-	(*T)(0, 2) = 0.0;
-
-	(*T)(1, 0) = st * ca;
-	(*T)(1, 1) = ct * ca;
-	(*T)(1, 2) = -sa;
-
-	(*T)(2, 0) = st * sa;
-	(*T)(2, 1) = ct * sa;
-	(*T)(2, 2) = ca;
-}
-
-void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
-{
-	const idScalar c = BT_ID_COS(angle);
-	const idScalar s = -BT_ID_SIN(angle);
-	const idScalar one_m_c = 1.0 - c;
-
-	const idScalar &x = axis(0);
-	const idScalar &y = axis(1);
-	const idScalar &z = axis(2);
-
-	(*T)(0, 0) = x * x * one_m_c + c;
-	(*T)(0, 1) = x * y * one_m_c - z * s;
-	(*T)(0, 2) = x * z * one_m_c + y * s;
-
-	(*T)(1, 0) = x * y * one_m_c + z * s;
-	(*T)(1, 1) = y * y * one_m_c + c;
-	(*T)(1, 2) = y * z * one_m_c - x * s;
-
-	(*T)(2, 0) = x * z * one_m_c - y * s;
-	(*T)(2, 1) = y * z * one_m_c + x * s;
-	(*T)(2, 2) = z * z * one_m_c + c;
-}
-
-bool isPositiveDefinite(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) <= 0)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
-	{
-		return false;
-	}
-	return true;
-}
-
-bool isPositiveSemiDefinite(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) < 0)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
-	{
-		return false;
-	}
-	return true;
-}
-
-bool isPositiveSemiDefiniteFuzzy(const mat33 &m)
-{
-	// test if all upper left determinants are positive
-	if (m(0, 0) < -kIsZero)
-	{  // upper 1x1
-		return false;
-	}
-	if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero)
-	{  // upper 2x2
-		return false;
-	}
-	if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
-		 m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
-		 m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero)
-	{
-		return false;
-	}
-	return true;
-}
-
-idScalar determinant(const mat33 &m)
-{
-	return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
-		   m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
-}
-
-bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
-{
-	// TODO(Thomas) do we really want this?
-	//			  in cases where the inertia tensor about the center of mass is zero,
-	//			  the determinant of the inertia tensor about the joint axis is almost
-	//			  zero and can have a very small negative value.
-	if (!isPositiveSemiDefiniteFuzzy(I))
-	{
-		bt_id_error_message(
-			"invalid inertia matrix for body %d, not positive definite "
-			"(fixed joint)\n",
-			index);
-		bt_id_error_message(
-			"matrix is:\n"
-			"[%.20e %.20e %.20e;\n"
-			"%.20e %.20e %.20e;\n"
-			"%.20e %.20e %.20e]\n",
-			I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-			I(2, 2));
-
-		return false;
-	}
-
-	// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
-	if (!has_fixed_joint)
-	{
-		if (I(0, 0) + I(1, 1) < I(2, 2))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-		if (I(0, 0) + I(1, 1) < I(2, 2))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-		if (I(1, 1) + I(2, 2) < I(0, 0))
-		{
-			bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
-			bt_id_error_message(
-				"matrix is:\n"
-				"[%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e;\n"
-				"%.20e %.20e %.20e]\n",
-				I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
-				I(2, 2));
-			return false;
-		}
-	}
-	// check positive/zero diagonal elements
-	for (int i = 0; i < 3; i++)
-	{
-		if (I(i, i) < 0)
-		{  // accept zero
-			bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
-			return false;
-		}
-	}
-	// check symmetry
-	if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero)
-	{
-		bt_id_error_message(
-			"invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
-			"%e\n",
-			index, I(1, 0) - I(0, 1));
-		return false;
-	}
-	if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero)
-	{
-		bt_id_error_message(
-			"invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
-			"%e\n",
-			index, I(2, 0) - I(0, 2));
-		return false;
-	}
-	if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero)
-	{
-		bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
-							I(1, 2) - I(2, 1));
-		return false;
-	}
-	return true;
-}
-
-bool isValidTransformMatrix(const mat33 &m)
-{
-#define print_mat(x)                                                                                   \
-	bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
-						x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
-
-	// check for unit length column vectors
-	for (int i = 0; i < 3; i++)
-	{
-		const idScalar length_minus_1 =
-			BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
-		if (length_minus_1 > kAxisLengthEpsilon)
-		{
-			bt_id_error_message(
-				"Not a valid rotation matrix (column %d not unit length)\n"
-				"column = [%.18e %.18e %.18e]\n"
-				"length-1.0= %.18e\n",
-				i, m(0, i), m(1, i), m(2, i), length_minus_1);
-			print_mat(m);
-			return false;
-		}
-	}
-	// check for orthogonal column vectors
-	if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon)
-	{
-		bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
-		print_mat(m);
-		return false;
-	}
-	// check determinant (rotation not reflection)
-	if (determinant(m) <= 0)
-	{
-		bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
-		print_mat(m);
-		return false;
-	}
-	return true;
-}
-
-bool isUnitVector(const vec3 &vector)
-{
-	return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
-		   kIsZero;
-}
-
-vec3 rpyFromMatrix(const mat33 &rot)
-{
-	vec3 rpy;
-	rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
-	rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
-	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
-	return rpy;
-}
-}  // namespace btInverseDynamics

thirdparty/bullet/BulletInverseDynamics/IDMath.hpp

@@ -1,100 +0,0 @@
-/// @file Math utility functions used in inverse dynamics library.
-///	   Defined here as they may not be provided by the math library.
-
-#ifndef IDMATH_HPP_
-#define IDMATH_HPP_
-#include "IDConfig.hpp"
-
-namespace btInverseDynamics
-{
-/// set all elements to zero
-void setZero(vec3& v);
-/// set all elements to zero
-void setZero(vecx& v);
-/// set all elements to zero
-void setZero(mat33& m);
-/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a)
-void skew(vec3& v, mat33* result);
-/// return maximum absolute value
-idScalar maxAbs(const vecx& v);
-#ifndef ID_LINEAR_MATH_USE_EIGEN
-/// return maximum absolute value
-idScalar maxAbs(const vec3& v);
-#endif  //ID_LINEAR_MATH_USE_EIGEN
-
-#if (defined BT_ID_HAVE_MAT3X)
-idScalar maxAbsMat3x(const mat3x& m);
-void setZero(mat3x& m);
-// define math functions on mat3x here to avoid allocations in operators.
-void mul(const mat33& a, const mat3x& b, mat3x* result);
-void add(const mat3x& a, const mat3x& b, mat3x* result);
-void sub(const mat3x& a, const mat3x& b, mat3x* result);
-#endif
-
-/// get offset vector & transform matrix from DH parameters
-/// TODO: add documentation
-void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);
-
-/// Check if a 3x3 matrix is positive definite
-/// @param m a 3x3 matrix
-/// @return true if m>0, false otherwise
-bool isPositiveDefinite(const mat33& m);
-
-/// Check if a 3x3 matrix is positive semi definite
-/// @param m a 3x3 matrix
-/// @return true if m>=0, false otherwise
-bool isPositiveSemiDefinite(const mat33& m);
-/// Check if a 3x3 matrix is positive semi definite within numeric limits
-/// @param m a 3x3 matrix
-/// @return true if m>=-eps, false otherwise
-bool isPositiveSemiDefiniteFuzzy(const mat33& m);
-
-/// Determinant of 3x3 matrix
-/// NOTE: implemented here for portability, as determinant operation
-///	   will be implemented differently for various matrix/vector libraries
-/// @param m a 3x3 matrix
-/// @return det(m)
-idScalar determinant(const mat33& m);
-
-/// Test if a 3x3 matrix satisfies some properties of inertia matrices
-/// @param I a 3x3 matrix
-/// @param index body index (for error messages)
-/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted
-/// @return true if I satisfies inertia matrix properties, false otherwise.
-bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint);
-
-/// Check if a 3x3 matrix is a valid transform (rotation) matrix
-/// @param m a 3x3 matrix
-/// @return true if m is a rotation matrix, false otherwise
-bool isValidTransformMatrix(const mat33& m);
-/// Transform matrix from parent to child frame,
-/// when the child frame is rotated about @param axis by @angle
-/// (mathematically positive)
-/// @param axis the axis of rotation
-/// @param angle rotation angle
-/// @param T pointer to transform matrix
-void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T);
-
-/// Check if this is a unit vector
-/// @param vector
-/// @return true if |vector|=1 within numeric limits
-bool isUnitVector(const vec3& vector);
-
-/// @input a vector in R^3
-/// @returns corresponding spin tensor
-mat33 tildeOperator(const vec3& v);
-/// @param alpha angle in radians
-/// @returns transform matrix for ratation with @param alpha about x-axis
-mat33 transformX(const idScalar& alpha);
-/// @param beta angle in radians
-/// @returns transform matrix for ratation with @param beta about y-axis
-mat33 transformY(const idScalar& beta);
-/// @param gamma angle in radians
-/// @returns transform matrix for ratation with @param gamma about z-axis
-mat33 transformZ(const idScalar& gamma);
-///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
-/// @param rot rotation matrix
-/// @returns x-y-z Euler angles
-vec3 rpyFromMatrix(const mat33& rot);
-}  // namespace btInverseDynamics
-#endif  // IDMATH_HPP_

thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.cpp

@@ -1,548 +0,0 @@
-#include "MultiBodyTree.hpp"
-
-#include <cmath>
-#include <limits>
-#include <vector>
-
-#include "IDMath.hpp"
-#include "details/MultiBodyTreeImpl.hpp"
-#include "details/MultiBodyTreeInitCache.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::MultiBodyTree()
-	: m_is_finalized(false),
-	  m_mass_parameters_are_valid(true),
-	  m_accept_invalid_mass_parameters(false),
-	  m_impl(0x0),
-	  m_init_cache(0x0)
-{
-	m_init_cache = new InitCache();
-}
-
-MultiBodyTree::~MultiBodyTree()
-{
-	delete m_impl;
-	delete m_init_cache;
-}
-
-void MultiBodyTree::setAcceptInvalidMassParameters(bool flag)
-{
-	m_accept_invalid_mass_parameters = flag;
-}
-
-bool MultiBodyTree::getAcceptInvalidMassProperties() const
-{
-	return m_accept_invalid_mass_parameters;
-}
-
-int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const
-{
-	return m_impl->getBodyOrigin(body_index, world_origin);
-}
-
-int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const
-{
-	return m_impl->getBodyCoM(body_index, world_com);
-}
-
-int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const
-{
-	return m_impl->getBodyTransform(body_index, world_T_body);
-}
-int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const
-{
-	return m_impl->getBodyAngularVelocity(body_index, world_omega);
-}
-int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const
-{
-	return m_impl->getBodyLinearVelocity(body_index, world_velocity);
-}
-
-int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const
-{
-	return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity);
-}
-
-int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const
-{
-	return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega);
-}
-int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const
-{
-	return m_impl->getBodyLinearAcceleration(body_index, world_acceleration);
-}
-
-int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3 *r) const
-{
-	return m_impl->getParentRParentBodyRef(body_index, r);
-}
-
-int MultiBodyTree::getBodyTParentRef(const int body_index, mat33 *T) const
-{
-	return m_impl->getBodyTParentRef(body_index, T);
-}
-
-int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
-{
-	return m_impl->getBodyAxisOfMotion(body_index, axis);
-}
-
-void MultiBodyTree::printTree() { m_impl->printTree(); }
-void MultiBodyTree::printTreeData() { m_impl->printTreeData(); }
-
-int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; }
-
-int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; }
-
-int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u,
-											vecx *joint_forces)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces))
-	{
-		bt_id_error_message("error in inverse dynamics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics,
-									   const bool initialize_matrix,
-									   const bool set_lower_triangular_matrix, matxx *mass_matrix)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 ==
-		m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix,
-									set_lower_triangular_matrix, mass_matrix))
-	{
-		bt_id_error_message("error in mass matrix calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix)
-{
-	return calculateMassMatrix(q, true, true, true, mass_matrix);
-}
-
-int MultiBodyTree::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u)
-{
-	vec3 world_gravity(m_impl->m_world_gravity);
-	// temporarily set gravity to zero, to ensure we get the actual accelerations
-	setZero(m_impl->m_world_gravity);
-
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, u, dot_u,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-
-	m_impl->m_world_gravity = world_gravity;
-	return 0;
-}
-
-int MultiBodyTree::calculatePositionKinematics(const vecx &q)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, q, q,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx &q, const vecx &u)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateKinematics(q, u, u,
-										  MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in kinematics calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::calculateJacobians(const vecx &q, const vecx &u)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateJacobians(q, u,
-										 MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY))
-	{
-		bt_id_error_message("error in jacobian calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::calculateJacobians(const vecx &q)
-{
-	if (false == m_is_finalized)
-	{
-		bt_id_error_message("system has not been initialized\n");
-		return -1;
-	}
-	if (-1 == m_impl->calculateJacobians(q, q,
-										 MultiBodyTree::MultiBodyImpl::POSITION_ONLY))
-	{
-		bt_id_error_message("error in jacobian calculation\n");
-		return -1;
-	}
-	return 0;
-}
-
-int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
-{
-	return m_impl->getBodyDotJacobianTransU(body_index, world_dot_jac_trans_u);
-}
-
-int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
-{
-	return m_impl->getBodyDotJacobianRotU(body_index, world_dot_jac_rot_u);
-}
-
-int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
-{
-	return m_impl->getBodyJacobianTrans(body_index, world_jac_trans);
-}
-
-int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
-{
-	return m_impl->getBodyJacobianRot(body_index, world_jac_rot);
-}
-
-#endif
-
-int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type,
-						   const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
-						   const vec3 &body_axis_of_motion_, idScalar mass,
-						   const vec3 &body_r_body_com, const mat33 &body_I_body,
-						   const int user_int, void *user_ptr)
-{
-	if (body_index < 0)
-	{
-		bt_id_error_message("body index must be positive (got %d)\n", body_index);
-		return -1;
-	}
-	vec3 body_axis_of_motion(body_axis_of_motion_);
-	switch (joint_type)
-	{
-		case REVOLUTE:
-		case PRISMATIC:
-			// check if axis is unit vector
-			if (!isUnitVector(body_axis_of_motion))
-			{
-				bt_id_warning_message(
-					"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
-					body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
-				idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
-											 BT_ID_POW(body_axis_of_motion(1), 2) +
-											 BT_ID_POW(body_axis_of_motion(2), 2));
-				if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min()))
-				{
-					bt_id_error_message("axis of motion vector too short (%e)\n", length);
-					return -1;
-				}
-				body_axis_of_motion = (1.0 / length) * body_axis_of_motion;
-			}
-			break;
-		case FIXED:
-			break;
-		case FLOATING:
-			break;
-		case SPHERICAL:
-			break;
-		default:
-			bt_id_error_message("unknown joint type %d\n", joint_type);
-			return -1;
-	}
-
-	// sanity check for mass properties. Zero mass is OK.
-	if (mass < 0)
-	{
-		m_mass_parameters_are_valid = false;
-		bt_id_error_message("Body %d has invalid mass %e\n", body_index, mass);
-		if (!m_accept_invalid_mass_parameters)
-		{
-			return -1;
-		}
-	}
-
-	if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type))
-	{
-		m_mass_parameters_are_valid = false;
-		// error message printed in function call
-		if (!m_accept_invalid_mass_parameters)
-		{
-			return -1;
-		}
-	}
-
-	if (!isValidTransformMatrix(body_T_parent_ref))
-	{
-		return -1;
-	}
-
-	return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref,
-								 body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com,
-								 body_I_body, user_int, user_ptr);
-}
-
-int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const
-{
-	return m_impl->getParentIndex(body_index, parent_index);
-}
-
-int MultiBodyTree::getUserInt(const int body_index, int *user_int) const
-{
-	return m_impl->getUserInt(body_index, user_int);
-}
-
-int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const
-{
-	return m_impl->getUserPtr(body_index, user_ptr);
-}
-
-int MultiBodyTree::setUserInt(const int body_index, const int user_int)
-{
-	return m_impl->setUserInt(body_index, user_int);
-}
-
-int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr)
-{
-	return m_impl->setUserPtr(body_index, user_ptr);
-}
-
-int MultiBodyTree::finalize()
-{
-	const int &num_bodies = m_init_cache->numBodies();
-	const int &num_dofs = m_init_cache->numDoFs();
-
-	if (num_dofs < 0)
-	{
-		bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
-		//return -1;
-	}
-
-	// 1 allocate internal MultiBody structure
-	m_impl = new MultiBodyImpl(num_bodies, num_dofs);
-
-	// 2 build new index set assuring index(parent) < index(child)
-	if (-1 == m_init_cache->buildIndexSets())
-	{
-		return -1;
-	}
-	m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
-
-	// 3 setup internal kinematic and dynamic data
-	for (int index = 0; index < num_bodies; index++)
-	{
-		InertiaData inertia;
-		JointData joint;
-		if (-1 == m_init_cache->getInertiaData(index, &inertia))
-		{
-			return -1;
-		}
-		if (-1 == m_init_cache->getJointData(index, &joint))
-		{
-			return -1;
-		}
-
-		RigidBody &rigid_body = m_impl->m_body_list[index];
-
-		rigid_body.m_mass = inertia.m_mass;
-		rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
-		rigid_body.m_body_I_body = inertia.m_body_I_body;
-		rigid_body.m_joint_type = joint.m_type;
-		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
-		rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref;
-		rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
-		rigid_body.m_joint_type = joint.m_type;
-
-		int user_int;
-		if (-1 == m_init_cache->getUserInt(index, &user_int))
-		{
-			return -1;
-		}
-		if (-1 == m_impl->setUserInt(index, user_int))
-		{
-			return -1;
-		}
-
-		void *user_ptr;
-		if (-1 == m_init_cache->getUserPtr(index, &user_ptr))
-		{
-			return -1;
-		}
-		if (-1 == m_impl->setUserPtr(index, user_ptr))
-		{
-			return -1;
-		}
-
-		// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
-		// matrices.
-		switch (rigid_body.m_joint_type)
-		{
-			case REVOLUTE:
-				rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
-				rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
-				rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2);
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case PRISMATIC:
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0);
-				rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1);
-				rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
-				break;
-			case FIXED:
-				// NOTE/TODO: dimension really should be zero ..
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case SPHERICAL:
-				// NOTE/TODO: this is not really correct.
-				// the Jacobians should be 3x3 matrices here !
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			case FLOATING:
-				// NOTE/TODO: this is not really correct.
-				// the Jacobians should be 3x3 matrices here !
-				rigid_body.m_Jac_JR(0) = 0.0;
-				rigid_body.m_Jac_JR(1) = 0.0;
-				rigid_body.m_Jac_JR(2) = 0.0;
-				rigid_body.m_Jac_JT(0) = 0.0;
-				rigid_body.m_Jac_JT(1) = 0.0;
-				rigid_body.m_Jac_JT(2) = 0.0;
-				break;
-			default:
-				bt_id_error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
-				return -1;
-		}
-	}
-
-	// 4 assign degree of freedom indices & build per-joint-type index arrays
-	if (-1 == m_impl->generateIndexSets())
-	{
-		bt_id_error_message("generating index sets\n");
-		return -1;
-	}
-
-	// 5 do some pre-computations ..
-	m_impl->calculateStaticData();
-
-	// 6. make sure all user forces are set to zero, as this might not happen
-	//	in the vector ctors.
-	m_impl->clearAllUserForcesAndMoments();
-
-	m_is_finalized = true;
-	return 0;
-}
-
-int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity)
-{
-	return m_impl->setGravityInWorldFrame(gravity);
-}
-
-int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const
-{
-	return m_impl->getJointType(body_index, joint_type);
-}
-
-int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const
-{
-	return m_impl->getJointTypeStr(body_index, joint_type);
-}
-
-int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const
-{
-	return m_impl->getDoFOffset(body_index, q_offset);
-}
-
-int MultiBodyTree::setBodyMass(const int body_index, idScalar mass)
-{
-	return m_impl->setBodyMass(body_index, mass);
-}
-
-int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3 &first_mass_moment)
-{
-	return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment);
-}
-
-int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33 &second_mass_moment)
-{
-	return m_impl->setBodySecondMassMoment(body_index, second_mass_moment);
-}
-
-int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const
-{
-	return m_impl->getBodyMass(body_index, mass);
-}
-
-int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const
-{
-	return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment);
-}
-
-int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const
-{
-	return m_impl->getBodySecondMassMoment(body_index, second_mass_moment);
-}
-
-void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); }
-
-int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force)
-{
-	return m_impl->addUserForce(body_index, body_force);
-}
-
-int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment)
-{
-	return m_impl->addUserMoment(body_index, body_moment);
-}
-
-}  // namespace btInverseDynamics

thirdparty/bullet/BulletInverseDynamics/MultiBodyTree.hpp

@@ -1,367 +0,0 @@
-#ifndef MULTIBODYTREE_HPP_
-#define MULTIBODYTREE_HPP_
-
-#include "IDConfig.hpp"
-#include "IDMath.hpp"
-
-namespace btInverseDynamics
-{
-/// Enumeration of supported joint types
-enum JointType
-{
-	/// no degree of freedom, moves with parent
-	FIXED = 0,
-	/// one rotational degree of freedom relative to parent
-	REVOLUTE,
-	/// one translational degree of freedom relative to parent
-	PRISMATIC,
-	/// six degrees of freedom relative to parent
-	FLOATING,
-	/// three degrees of freedom, relative to parent
-	SPHERICAL
-};
-
-/// Interface class for calculating inverse dynamics for tree structured
-/// multibody systems
-///
-/// Note on degrees of freedom
-/// The q vector contains the generalized coordinate set defining the tree's configuration.
-/// Every joint adds elements that define the corresponding link's frame pose relative to
-/// its parent. For the joint types that is:
-///	- FIXED:	 none
-///	- REVOLUTE:  angle of rotation [rad]
-///	- PRISMATIC: displacement [m]
-///	- FLOATING:  Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
-///				 (in that order)
-/// - SPHERICAL: Euler x-y-z angles [rad]
-/// The u vector contains the generalized speeds, which are
-///	- FIXED:	 none
-///	- REVOLUTE:  time derivative of angle of rotation [rad/s]
-///	- PRISMATIC: time derivative of displacement [m/s]
-///	- FLOATING:  angular velocity [rad/s] (*not* time derivative of rpy angles)
-///				 and time derivative of displacement in parent frame [m/s]
-//	- SPHERICAL:  angular velocity [rad/s]
-///
-/// The q and u vectors are obtained by stacking contributions of all bodies in one
-/// vector in the order of body indices.
-///
-/// Note on generalized forces: analogous to u, i.e.,
-///	 - FIXED:	 none
-///	 - REVOLUTE:  moment [Nm], about joint axis
-///	 - PRISMATIC: force  [N], along joint axis
-///	 - FLOATING:  moment vector [Nm] and force vector [N], both in body-fixed frame
-///				  (in that order)
-///  - SPHERICAL: moment vector [Nm] 
-/// TODO - force element interface (friction, springs, dampers, etc)
-///	  - gears and motor inertia
-class MultiBodyTree
-{
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	/// The contructor.
-	/// Initialization & allocation is via addBody and buildSystem calls.
-	MultiBodyTree();
-	/// the destructor. This also deallocates all memory
-	~MultiBodyTree();
-
-	/// Add body to the system. this allocates memory and not real-time safe.
-	/// This only adds the data to an initial cache. After all bodies have been
-	/// added,
-	/// the system is setup using the buildSystem call
-	/// @param body_index index of the body to be added. Must >=0, <number of bodies,
-	///		and index of parent must be < index of body
-	/// @param parent_index index of the parent body
-	///		The root of the tree has index 0 and its parent (the world frame)
-	///		is assigned index -1
-	///		the rotation and translation relative to the parent are taken as
-	///		pose of the root body relative to the world frame. Other parameters
-	///		are ignored
-	/// @param JointType type of joint connecting the body to the parent
-	/// @param mass the mass of the body
-	/// @param body_r_body_com the center of mass of the body relative to and
-	/// described in
-	///		the body fixed frame, which is located in the joint axis connecting
-	/// the body to its parent
-	/// @param body_I_body the moment of inertia of the body w.r.t the body-fixed
-	/// frame
-	///		(ie, the reference point is the origin of the body-fixed frame and
-	/// the matrix is written
-	///		 w.r.t. those unit vectors)
-	/// @param parent_r_parent_body_ref position of joint relative to the parent
-	/// body's reference frame
-	///		for q=0, written in the parent bodies reference frame
-	/// @param body_axis_of_motion translation/rotation axis in body-fixed frame.
-	///		Ignored for joints that are not revolute or prismatic.
-	///		must be a unit vector.
-	/// @param body_T_parent_ref transform matrix from parent to body reference
-	/// frame for q=0.
-	///		This is the matrix transforming a vector represented in the
-	/// parent's reference frame into one represented
-	///		in this body's reference frame.
-	///		ie, if parent_vec is a vector in R^3 whose components are w.r.t to
-	/// the parent's reference frame,
-	///		then the same vector written w.r.t. this body's frame (for q=0) is
-	/// given by
-	///		body_vec = parent_R_body_ref * parent_vec
-	/// @param user_ptr pointer to user data
-	/// @param user_int pointer to user integer
-	/// @return 0 on success, -1 on error
-	int addBody(int body_index, int parent_index, JointType joint_type,
-				const vec3& parent_r_parent_body_ref, const mat33& body_T_parent_ref,
-				const vec3& body_axis_of_motion, idScalar mass, const vec3& body_r_body_com,
-				const mat33& body_I_body, const int user_int, void* user_ptr);
-	/// set policy for invalid mass properties
-	/// @param flag if true, invalid mass properties are accepted,
-	///		the default is false
-	void setAcceptInvalidMassParameters(bool flag);
-	/// @return the mass properties policy flag
-	bool getAcceptInvalidMassProperties() const;
-	/// build internal data structures
-	/// call this after all bodies have been added via addBody
-	/// @return 0 on success, -1 on error
-	int finalize();
-	/// pretty print ascii description of tree to stdout
-	void printTree();
-	/// print tree data to stdout
-	void printTreeData();
-	/// Calculate joint forces for given generalized state & derivatives.
-	/// This also updates kinematic terms computed in calculateKinematics.
-	/// If gravity is not set to zero, acceleration terms will contain
-	/// gravitational acceleration.
-	/// @param q generalized coordinates
-	/// @param u generalized velocities. In the general case, u=T(q)*dot(q) and dim(q)>=dim(u)
-	/// @param dot_u time derivative of u
-	/// @param joint_forces this is where the resulting joint forces will be
-	///		stored. dim(joint_forces) = dim(u)
-	/// @return 0 on success, -1 on error
-	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
-								 vecx* joint_forces);
-	/// Calculate joint space mass matrix
-	/// @param q generalized coordinates
-	/// @param initialize_matrix if true, initialize mass matrix with zero.
-	///		If mass_matrix is initialized to zero externally and only used
-	///		for mass matrix computations for the same system, it is safe to
-	///		set this to false.
-	/// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix
-	///		is also populated, otherwise not.
-	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
-	/// @return -1 on error, 0 on success
-	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
-							const bool initialize_matrix, const bool set_lower_triangular_matrix,
-							matxx* mass_matrix);
-
-	/// Calculate joint space mass matrix.
-	/// This version will update kinematics, initialize all mass_matrix elements to zero and
-	/// populate all mass matrix entries.
-	/// @param q generalized coordinates
-	/// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q))
-	/// @return -1 on error, 0 on success
-	int calculateMassMatrix(const vecx& q, matxx* mass_matrix);
-
-	/// Calculates kinematics also calculated in calculateInverseDynamics,
-	/// but not dynamics.
-	/// This function ensures that correct accelerations are computed that do not
-	/// contain gravitational acceleration terms.
-	/// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations)
-	int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u);
-	/// Calculate position kinematics
-	int calculatePositionKinematics(const vecx& q);
-	/// Calculate position and velocity kinematics
-	int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u);
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components
-	/// d(Jacobian)/dt*u
-	/// This function assumes that calculateInverseDynamics was called, or calculateKinematics,
-	/// or calculatePositionAndVelocityKinematics
-	int calculateJacobians(const vecx& q, const vecx& u);
-	/// Calculate Jacobians (dvel/du)
-	/// This function assumes that calculateInverseDynamics was called, or
-	/// one of the calculateKineamtics functions
-	int calculateJacobians(const vecx& q);
-#endif  // BT_ID_HAVE_MAT3X
-
-	/// set gravitational acceleration
-	/// the default is [0;0;-9.8] in the world frame
-	/// @param gravity the gravitational acceleration in world frame
-	/// @return 0 on success, -1 on error
-	int setGravityInWorldFrame(const vec3& gravity);
-	/// returns number of bodies in tree
-	int numBodies() const;
-	/// returns number of mechanical degrees of freedom (dimension of q-vector)
-	int numDoFs() const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyOrigin(const int body_index, vec3* world_origin) const;
-	/// get center of mass of a body, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_com pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyCoM(const int body_index, vec3* world_com) const;
-	/// get transform from of a body-fixed frame to the world frame
-	/// @param body_index index for frame/body
-	/// @param world_T_body pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyTransform(const int body_index, mat33* world_T_body) const;
-	/// get absolute angular velocity for a body, represented in the world frame
-	/// @param body_index index for frame/body
-	/// @param world_omega pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
-	/// get linear velocity of a body, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_velocity pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
-	/// get linear velocity of a body's CoM, represented in world frame
-	/// (not required for inverse dynamics, provided for convenience)
-	/// @param body_index index for frame/body
-	/// @param world_vel_com pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
-	/// get origin of a body-fixed frame, represented in world frame
-	/// NOTE: this will include the gravitational acceleration, so the actual acceleration is
-	/// obtainened by setting gravitational acceleration to zero, or subtracting it.
-	/// @param body_index index for frame/body
-	/// @param world_origin pointer for return data
-	/// @return 0 on success, -1 on error
-	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	// get translational jacobian, in world frame (dworld_velocity/du)
-	int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
-	// get rotational jacobian, in world frame (dworld_omega/du)
-	int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
-	// get product of translational jacobian derivative * generatlized velocities
-	int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
-	// get product of rotational jacobian derivative * generatlized velocities
-	int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
-#endif  // BT_ID_HAVE_MAT3X
-
-	/// returns the (internal) index of body
-	/// @param body_index is the index of a body
-	/// @param parent_index pointer to where parent index will be stored
-	/// @return 0 on success, -1 on error
-	int getParentIndex(const int body_index, int* parent_index) const;
-	/// get joint type
-	/// @param body_index index of the body
-	/// @param joint_type the corresponding joint type
-	/// @return 0 on success, -1 on failure
-	int getJointType(const int body_index, JointType* joint_type) const;
-	/// get joint type as string
-	/// @param body_index index of the body
-	/// @param joint_type string naming the corresponding joint type
-	/// @return 0 on success, -1 on failure
-	int getJointTypeStr(const int body_index, const char** joint_type) const;
-	/// get offset translation to parent body (see addBody)
-	/// @param body_index index of the body
-	/// @param r the offset translation (see above)
-	/// @return 0 on success, -1 on failure
-	int getParentRParentBodyRef(const int body_index, vec3* r) const;
-	/// get offset rotation to parent body (see addBody)
-	/// @param body_index index of the body
-	/// @param T the transform (see above)
-	/// @return 0 on success, -1 on failure
-	int getBodyTParentRef(const int body_index, mat33* T) const;
-	/// get axis of motion (see addBody)
-	/// @param body_index index of the body
-	/// @param axis the axis (see above)
-	/// @return 0 on success, -1 on failure
-	int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
-	/// get offset for degrees of freedom of this body into the q-vector
-	/// @param body_index index of the body
-	/// @param q_offset offset the q vector
-	/// @return -1 on error, 0 on success
-	int getDoFOffset(const int body_index, int* q_offset) const;
-	/// get user integer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_int   the user integer
-	/// @return 0 on success, -1 on error
-	int getUserInt(const int body_index, int* user_int) const;
-	/// get user pointer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_ptr   the user pointer
-	/// @return 0 on success, -1 on error
-	int getUserPtr(const int body_index, void** user_ptr) const;
-	/// set user integer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_int   the user integer
-	/// @return 0 on success, -1 on error
-	int setUserInt(const int body_index, const int user_int);
-	/// set user pointer. not used by the library.
-	/// @param body_index index of the body
-	/// @param user_ptr   the user pointer
-	/// @return 0 on success, -1 on error
-	int setUserPtr(const int body_index, void* const user_ptr);
-	/// set mass for a body
-	/// @param body_index index of the body
-	/// @param mass the mass to set
-	/// @return 0 on success, -1 on failure
-	int setBodyMass(const int body_index, const idScalar mass);
-	/// set first moment of mass for a body
-	/// (mass * center of mass, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param first_mass_moment the vector to set
-	/// @return 0 on success, -1 on failure
-	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
-	/// set second moment of mass for a body
-	/// (moment of inertia, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param second_mass_moment the inertia matrix
-	/// @return 0 on success, -1 on failure
-	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
-	/// get mass for a body
-	/// @param body_index index of the body
-	/// @param mass the mass
-	/// @return 0 on success, -1 on failure
-	int getBodyMass(const int body_index, idScalar* mass) const;
-	/// get first moment of mass for a body
-	/// (mass * center of mass, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param first_moment the vector
-	/// @return 0 on success, -1 on failure
-	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
-	/// get second moment of mass for a body
-	/// (moment of inertia, in body fixed frame, relative to joint)
-	/// @param body_index index of the body
-	/// @param second_mass_moment the inertia matrix
-	/// @return 0 on success, -1 on failure
-	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
-	/// set all user forces and moments to zero
-	void clearAllUserForcesAndMoments();
-	/// Add an external force to a body, acting at the origin of the body-fixed frame.
-	/// Calls to addUserForce are cumulative. Set the user force and moment to zero
-	/// via clearAllUserForcesAndMoments()
-	/// @param body_force the force represented in the body-fixed frame of reference
-	/// @return 0 on success, -1 on error
-	int addUserForce(const int body_index, const vec3& body_force);
-	/// Add an external moment to a body.
-	/// Calls to addUserMoment are cumulative. Set the user force and moment to zero
-	/// via clearAllUserForcesAndMoments()
-	/// @param body_moment the moment represented in the body-fixed frame of reference
-	/// @return 0 on success, -1 on error
-	int addUserMoment(const int body_index, const vec3& body_moment);
-
-private:
-	// flag indicating if system has been initialized
-	bool m_is_finalized;
-	// flag indicating if mass properties are physically valid
-	bool m_mass_parameters_are_valid;
-	// flag defining if unphysical mass parameters are accepted
-	bool m_accept_invalid_mass_parameters;
-	// This struct implements the inverse dynamics calculations
-	class MultiBodyImpl;
-	MultiBodyImpl* m_impl;
-	// cache data structure for initialization
-	class InitCache;
-	InitCache* m_init_cache;
-};
-}  // namespace btInverseDynamics
-#endif  // MULTIBODYTREE_HPP_

thirdparty/bullet/BulletInverseDynamics/details/IDEigenInterface.hpp

@@ -1,39 +0,0 @@
-#ifndef INVDYNEIGENINTERFACE_HPP_
-#define INVDYNEIGENINTERFACE_HPP_
-#include "../IDConfig.hpp"
-namespace btInverseDynamics
-{
-#define BT_ID_HAVE_MAT3X
-
-#ifdef BT_USE_DOUBLE_PRECISION
-typedef Eigen::Matrix<double, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
-typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
-#else
-typedef Eigen::Matrix<float, Eigen::Dynamic, 1, Eigen::DontAlign> vecx;
-typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> vec3;
-typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> mat33;
-typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> matxx;
-typedef Eigen::Matrix<float, 3, Eigen::Dynamic, Eigen::DontAlign> mat3x;
-#endif
-
-inline void resize(mat3x &m, Eigen::Index size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx *m)
-{
-	(*m)(row, col) = val;
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x *m)
-{
-	(*m)(row, col) = val;
-}
-
-}  // namespace btInverseDynamics
-#endif  // INVDYNEIGENINTERFACE_HPP_

thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp

@@ -1,202 +0,0 @@
-#ifndef IDLINEARMATHINTERFACE_HPP_
-#define IDLINEARMATHINTERFACE_HPP_
-
-#include <cstdlib>
-
-#include "../IDConfig.hpp"
-
-#include "../../LinearMath/btMatrix3x3.h"
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btMatrixX.h"
-#define BT_ID_HAVE_MAT3X
-
-namespace btInverseDynamics
-{
-class vec3;
-class vecx;
-class mat33;
-typedef btMatrixX<idScalar> matxx;
-
-class vec3 : public btVector3
-{
-public:
-	vec3() : btVector3() {}
-	vec3(const btVector3& btv) { *this = btv; }
-	idScalar& operator()(int i) { return (*this)[i]; }
-	const idScalar& operator()(int i) const { return (*this)[i]; }
-	int size() const { return 3; }
-	const vec3& operator=(const btVector3& rhs)
-	{
-		*static_cast<btVector3*>(this) = rhs;
-		return *this;
-	}
-};
-
-class mat33 : public btMatrix3x3
-{
-public:
-	mat33() : btMatrix3x3() {}
-	mat33(const btMatrix3x3& btm) { *this = btm; }
-	idScalar& operator()(int i, int j) { return (*this)[i][j]; }
-	const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
-	const mat33& operator=(const btMatrix3x3& rhs)
-	{
-		*static_cast<btMatrix3x3*>(this) = rhs;
-		return *this;
-	}
-	friend mat33 operator*(const idScalar& s, const mat33& a);
-	friend mat33 operator/(const mat33& a, const idScalar& s);
-};
-
-inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); }
-
-inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
-
-class vecx : public btVectorX<idScalar>
-{
-public:
-	vecx(int size) : btVectorX<idScalar>(size) {}
-	const vecx& operator=(const btVectorX<idScalar>& rhs)
-	{
-		*static_cast<btVectorX<idScalar>*>(this) = rhs;
-		return *this;
-	}
-
-	idScalar& operator()(int i) { return (*this)[i]; }
-	const idScalar& operator()(int i) const { return (*this)[i]; }
-
-	friend vecx operator*(const vecx& a, const idScalar& s);
-	friend vecx operator*(const idScalar& s, const vecx& a);
-
-	friend vecx operator+(const vecx& a, const vecx& b);
-	friend vecx operator-(const vecx& a, const vecx& b);
-	friend vecx operator/(const vecx& a, const idScalar& s);
-};
-
-inline vecx operator*(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result(i) = a(i) * s;
-	}
-	return result;
-}
-inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
-inline vecx operator+(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result(i) = a(i) + b(i);
-	}
-
-	return result;
-}
-
-inline vecx operator-(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result(i) = a(i) - b(i);
-	}
-	return result;
-}
-inline vecx operator/(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result(i) = a(i) / s;
-	}
-
-	return result;
-}
-
-// use btMatrixX to implement 3xX matrix
-class mat3x : public matxx
-{
-public:
-	mat3x() {}
-	mat3x(const mat3x& rhs)
-	{
-		matxx::resize(rhs.rows(), rhs.cols());
-		*this = rhs;
-	}
-	mat3x(int rows, int cols) : matxx(3, cols)
-	{
-	}
-	void operator=(const mat3x& rhs)
-	{
-		if (m_cols != rhs.m_cols)
-		{
-			bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
-			abort();
-		}
-		for (int i = 0; i < rows(); i++)
-		{
-			for (int k = 0; k < cols(); k++)
-			{
-				setElem(i, k, rhs(i, k));
-			}
-		}
-	}
-	void setZero()
-	{
-		matxx::setZero();
-	}
-};
-
-inline vec3 operator*(const mat3x& a, const vecx& b)
-{
-	vec3 result;
-	if (a.cols() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
-		abort();
-	}
-	result(0) = 0.0;
-	result(1) = 0.0;
-	result(2) = 0.0;
-	for (int i = 0; i < b.size(); i++)
-	{
-		for (int k = 0; k < 3; k++)
-		{
-			result(k) += a(k, i) * b(i);
-		}
-	}
-	return result;
-}
-
-inline void resize(mat3x& m, idArrayIdx size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
-{
-	m->setElem(row, col, val);
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
-{
-	m->setElem(row, col, val);
-}
-
-}  // namespace btInverseDynamics
-
-#endif  // IDLINEARMATHINTERFACE_HPP_

thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp

@@ -1,489 +0,0 @@
-/// @file Built-In Matrix-Vector functions
-#ifndef IDMATVEC_HPP_
-#define IDMATVEC_HPP_
-
-#include <cstdlib>
-
-#include "../IDConfig.hpp"
-#define BT_ID_HAVE_MAT3X
-
-namespace btInverseDynamics
-{
-class vec3;
-class vecx;
-class mat33;
-class matxx;
-class mat3x;
-
-/// This is a very basic implementation to enable stand-alone use of the library.
-/// The implementation is not really optimized and misses many features that you would
-/// want from a "fully featured" linear math library.
-class vec3
-{
-public:
-	idScalar& operator()(int i) { return m_data[i]; }
-	const idScalar& operator()(int i) const { return m_data[i]; }
-	const int size() const { return 3; }
-	const vec3& operator=(const vec3& rhs);
-	const vec3& operator+=(const vec3& b);
-	const vec3& operator-=(const vec3& b);
-	vec3 cross(const vec3& b) const;
-	idScalar dot(const vec3& b) const;
-
-	friend vec3 operator*(const mat33& a, const vec3& b);
-	friend vec3 operator*(const vec3& a, const idScalar& s);
-	friend vec3 operator*(const idScalar& s, const vec3& a);
-
-	friend vec3 operator+(const vec3& a, const vec3& b);
-	friend vec3 operator-(const vec3& a, const vec3& b);
-	friend vec3 operator/(const vec3& a, const idScalar& s);
-
-private:
-	idScalar m_data[3];
-};
-
-class mat33
-{
-public:
-	idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
-	const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
-	const mat33& operator=(const mat33& rhs);
-	mat33 transpose() const;
-	const mat33& operator+=(const mat33& b);
-	const mat33& operator-=(const mat33& b);
-
-	friend mat33 operator*(const mat33& a, const mat33& b);
-	friend vec3 operator*(const mat33& a, const vec3& b);
-	friend mat33 operator*(const mat33& a, const idScalar& s);
-	friend mat33 operator*(const idScalar& s, const mat33& a);
-	friend mat33 operator+(const mat33& a, const mat33& b);
-	friend mat33 operator-(const mat33& a, const mat33& b);
-	friend mat33 operator/(const mat33& a, const idScalar& s);
-
-private:
-	// layout is [0,1,2;3,4,5;6,7,8]
-	idScalar m_data[9];
-};
-
-class vecx
-{
-public:
-	vecx(int size) : m_size(size)
-	{
-		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
-	}
-	~vecx() { idFree(m_data); }
-	const vecx& operator=(const vecx& rhs);
-	idScalar& operator()(int i) { return m_data[i]; }
-	const idScalar& operator()(int i) const { return m_data[i]; }
-	const int& size() const { return m_size; }
-
-	friend vecx operator*(const vecx& a, const idScalar& s);
-	friend vecx operator*(const idScalar& s, const vecx& a);
-
-	friend vecx operator+(const vecx& a, const vecx& b);
-	friend vecx operator-(const vecx& a, const vecx& b);
-	friend vecx operator/(const vecx& a, const idScalar& s);
-
-private:
-	int m_size;
-	idScalar* m_data;
-};
-
-class matxx
-{
-public:
-	matxx()
-	{
-		m_data = 0x0;
-		m_cols = 0;
-		m_rows = 0;
-	}
-	matxx(int rows, int cols) : m_rows(rows), m_cols(cols)
-	{
-		m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
-	}
-	~matxx() { idFree(m_data); }
-	idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
-	const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
-	const int& rows() const { return m_rows; }
-	const int& cols() const { return m_cols; }
-
-private:
-	int m_rows;
-	int m_cols;
-	idScalar* m_data;
-};
-
-class mat3x
-{
-public:
-	mat3x()
-	{
-		m_data = 0x0;
-		m_cols = 0;
-	}
-	mat3x(const mat3x& rhs)
-	{
-		m_cols = rhs.m_cols;
-		allocate();
-		*this = rhs;
-	}
-	mat3x(int rows, int cols) : m_cols(cols)
-	{
-		allocate();
-	};
-	void operator=(const mat3x& rhs)
-	{
-		if (m_cols != rhs.m_cols)
-		{
-			bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
-			abort();
-		}
-		for (int i = 0; i < 3 * m_cols; i++)
-		{
-			m_data[i] = rhs.m_data[i];
-		}
-	}
-
-	~mat3x()
-	{
-		free();
-	}
-	idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
-	const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
-	int rows() const { return m_rows; }
-	const int& cols() const { return m_cols; }
-	void resize(int rows, int cols)
-	{
-		m_cols = cols;
-		free();
-		allocate();
-	}
-	void setZero()
-	{
-		memset(m_data, 0x0, sizeof(idScalar) * m_rows * m_cols);
-	}
-	// avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
-private:
-	void allocate() { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols)); }
-	void free() { idFree(m_data); }
-	enum
-	{
-		m_rows = 3
-	};
-	int m_cols;
-	idScalar* m_data;
-};
-
-inline void resize(mat3x& m, idArrayIdx size)
-{
-	m.resize(3, size);
-	m.setZero();
-}
-
-//////////////////////////////////////////////////
-// Implementations
-inline const vec3& vec3::operator=(const vec3& rhs)
-{
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
-	}
-	return *this;
-}
-
-inline vec3 vec3::cross(const vec3& b) const
-{
-	vec3 result;
-	result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
-	result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
-	result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
-
-	return result;
-}
-
-inline idScalar vec3::dot(const vec3& b) const
-{
-	return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
-}
-
-inline const mat33& mat33::operator=(const mat33& rhs)
-{
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
-	}
-	return *this;
-}
-inline mat33 mat33::transpose() const
-{
-	mat33 result;
-	result.m_data[0] = m_data[0];
-	result.m_data[1] = m_data[3];
-	result.m_data[2] = m_data[6];
-	result.m_data[3] = m_data[1];
-	result.m_data[4] = m_data[4];
-	result.m_data[5] = m_data[7];
-	result.m_data[6] = m_data[2];
-	result.m_data[7] = m_data[5];
-	result.m_data[8] = m_data[8];
-
-	return result;
-}
-
-inline mat33 operator*(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	result.m_data[0] =
-		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
-	result.m_data[1] =
-		a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7];
-	result.m_data[2] =
-		a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8];
-	result.m_data[3] =
-		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6];
-	result.m_data[4] =
-		a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7];
-	result.m_data[5] =
-		a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8];
-	result.m_data[6] =
-		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6];
-	result.m_data[7] =
-		a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7];
-	result.m_data[8] =
-		a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
-
-	return result;
-}
-
-inline const mat33& mat33::operator+=(const mat33& b)
-{
-	for (int i = 0; i < 9; i++)
-	{
-		m_data[i] += b.m_data[i];
-	}
-
-	return *this;
-}
-
-inline const mat33& mat33::operator-=(const mat33& b)
-{
-	for (int i = 0; i < 9; i++)
-	{
-		m_data[i] -= b.m_data[i];
-	}
-	return *this;
-}
-
-inline vec3 operator*(const mat33& a, const vec3& b)
-{
-	vec3 result;
-
-	result.m_data[0] =
-		a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2];
-	result.m_data[1] =
-		a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2];
-	result.m_data[2] =
-		a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
-
-	return result;
-}
-
-inline const vec3& vec3::operator+=(const vec3& b)
-{
-	for (int i = 0; i < 3; i++)
-	{
-		m_data[i] += b.m_data[i];
-	}
-	return *this;
-}
-
-inline const vec3& vec3::operator-=(const vec3& b)
-{
-	for (int i = 0; i < 3; i++)
-	{
-		m_data[i] -= b.m_data[i];
-	}
-	return *this;
-}
-
-inline mat33 operator*(const mat33& a, const idScalar& s)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-
-inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
-
-inline vec3 operator*(const vec3& a, const idScalar& s)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
-
-inline mat33 operator+(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-	return result;
-}
-inline vec3 operator+(const vec3& a, const vec3& b)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-	return result;
-}
-
-inline mat33 operator-(const mat33& a, const mat33& b)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-inline vec3 operator-(const vec3& a, const vec3& b)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-
-inline mat33 operator/(const mat33& a, const idScalar& s)
-{
-	mat33 result;
-	for (int i = 0; i < 9; i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-	return result;
-}
-
-inline vec3 operator/(const vec3& a, const idScalar& s)
-{
-	vec3 result;
-	for (int i = 0; i < 3; i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-	return result;
-}
-
-inline const vecx& vecx::operator=(const vecx& rhs)
-{
-	if (size() != rhs.size())
-	{
-		bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
-		abort();
-	}
-	if (&rhs != this)
-	{
-		memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
-	}
-	return *this;
-}
-inline vecx operator*(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] * s;
-	}
-	return result;
-}
-inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
-inline vecx operator+(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] + b.m_data[i];
-	}
-
-	return result;
-}
-inline vecx operator-(const vecx& a, const vecx& b)
-{
-	vecx result(a.size());
-	// TODO: error handling for a.size() != b.size()??
-	if (a.size() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
-		abort();
-	}
-	for (int i = 0; i < a.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] - b.m_data[i];
-	}
-	return result;
-}
-inline vecx operator/(const vecx& a, const idScalar& s)
-{
-	vecx result(a.size());
-	for (int i = 0; i < result.size(); i++)
-	{
-		result.m_data[i] = a.m_data[i] / s;
-	}
-
-	return result;
-}
-
-inline vec3 operator*(const mat3x& a, const vecx& b)
-{
-	vec3 result;
-	if (a.cols() != b.size())
-	{
-		bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
-		abort();
-	}
-	result(0) = 0.0;
-	result(1) = 0.0;
-	result(2) = 0.0;
-	for (int i = 0; i < b.size(); i++)
-	{
-		for (int k = 0; k < 3; k++)
-		{
-			result(k) += a(k, i) * b(i);
-		}
-	}
-	return result;
-}
-
-inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
-{
-	(*m)(row, col) = val;
-}
-
-inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
-{
-	(*m)(row, col) = val;
-}
-
-}  // namespace btInverseDynamics
-#endif

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -1,1286 +0,0 @@
-#include "MultiBodyTreeImpl.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
-	: m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	  ,
-	  m_m3x(3, m_num_dofs)
-#endif
-{
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	resize(m_m3x, m_num_dofs);
-#endif
-	m_body_list.resize(num_bodies_);
-	m_parent_index.resize(num_bodies_);
-	m_child_indices.resize(num_bodies_);
-	m_user_int.resize(num_bodies_);
-	m_user_ptr.resize(num_bodies_);
-
-	m_world_gravity(0) = 0.0;
-	m_world_gravity(1) = 0.0;
-	m_world_gravity(2) = -9.8;
-}
-
-const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const
-{
-	switch (type)
-	{
-		case FIXED:
-			return "fixed";
-		case REVOLUTE:
-			return "revolute";
-		case PRISMATIC:
-			return "prismatic";
-		case FLOATING:
-			return "floating";
-		case SPHERICAL:
-			return "spherical";
-	}
-	return "error: invalid";
-}
-
-inline void indent(const int &level)
-{
-	for (int j = 0; j < level; j++)
-		id_printf("  ");  // indent
-}
-
-void MultiBodyTree::MultiBodyImpl::printTree()
-{
-	id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
-	printTree(0, 0);
-}
-
-void MultiBodyTree::MultiBodyImpl::printTreeData()
-{
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		id_printf("body: %d\n", static_cast<int>(i));
-		id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
-		id_printf("q_index= %d\n", body.m_q_index);
-		id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2));
-		id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2));
-
-		id_printf("mass = %f\n", body.m_mass);
-		id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
-				  body.m_body_mass_com(2));
-		id_printf(
-			"I_o= [%f %f %f;\n"
-			"	  %f %f %f;\n"
-			"	  %f %f %f]\n",
-			body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
-			body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
-			body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
-
-		id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
-				  body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
-	}
-}
-int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
-{
-	switch (type)
-	{
-		case FIXED:
-			return 0;
-		case REVOLUTE:
-		case PRISMATIC:
-			return 1;
-		case FLOATING:
-			return 6;
-		case SPHERICAL:
-			return 3;
-	}
-	bt_id_error_message("unknown joint type %d\n", type);
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation)
-{
-	// this is adapted from URDF2Bullet.
-	// TODO: fix this and print proper graph (similar to git --log --graph)
-	int num_children = m_child_indices[index].size();
-
-	indentation += 2;
-	int count = 0;
-
-	for (int i = 0; i < num_children; i++)
-	{
-		int child_index = m_child_indices[index][i];
-		indent(indentation);
-		id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
-				  jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1,
-				  m_body_list[index].m_q_index,
-				  m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type));
-		// first grandchild
-		printTree(child_index, indentation);
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity)
-{
-	m_world_gravity = gravity;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::generateIndexSets()
-{
-	m_body_revolute_list.resize(0);
-	m_body_prismatic_list.resize(0);
-	int q_index = 0;
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		body.m_q_index = -1;
-		switch (body.m_joint_type)
-		{
-			case REVOLUTE:
-				m_body_revolute_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index++;
-				break;
-			case PRISMATIC:
-				m_body_prismatic_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index++;
-				break;
-			case FIXED:
-				// do nothing
-				break;
-			case FLOATING:
-				m_body_floating_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index += 6;
-				break;
-			case SPHERICAL:
-				m_body_spherical_list.push_back(i);
-				body.m_q_index = q_index;
-				q_index += 3;
-				break;
-			default:
-				bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
-				return -1;
-		}
-	}
-	// sanity check
-	if (q_index != m_num_dofs)
-	{
-		bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
-		return -1;
-	}
-
-	m_child_indices.resize(m_body_list.size());
-
-	for (idArrayIdx child = 1; child < m_parent_index.size(); child++)
-	{
-		const int &parent = m_parent_index[child];
-		if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1))
-		{
-			m_child_indices[parent].push_back(child);
-		}
-		else
-		{
-			if (-1 == parent)
-			{
-				// multiple bodies are directly linked to the environment, ie, not a single root
-				bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
-			}
-			else
-			{
-				// should never happen
-				bt_id_error_message(
-					"building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
-					child, parent, static_cast<int>(m_parent_index.size()));
-			}
-			return -1;
-		}
-	}
-
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::calculateStaticData()
-{
-	// relative kinematics that are not a function of q, u, dot_u
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		switch (body.m_joint_type)
-		{
-			case REVOLUTE:
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				break;
-			case PRISMATIC:
-				body.m_body_T_parent = body.m_body_T_parent_ref;
-				body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT;
-				body.m_body_ang_vel_rel(0) = 0;
-				body.m_body_ang_vel_rel(1) = 0;
-				body.m_body_ang_vel_rel(2) = 0;
-				body.m_body_ang_acc_rel(0) = 0;
-				body.m_body_ang_acc_rel(1) = 0;
-				body.m_body_ang_acc_rel(2) = 0;
-				break;
-			case FIXED:
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				body.m_body_T_parent = body.m_body_T_parent_ref;
-				body.m_body_ang_vel_rel(0) = 0;
-				body.m_body_ang_vel_rel(1) = 0;
-				body.m_body_ang_vel_rel(2) = 0;
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_body_ang_acc_rel(0) = 0;
-				body.m_body_ang_acc_rel(1) = 0;
-				body.m_body_ang_acc_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				break;
-			case FLOATING:
-				// no static data
-				break;
-			case SPHERICAL:
-				//todo: review
-				body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
-				body.m_parent_vel_rel(0) = 0;
-				body.m_parent_vel_rel(1) = 0;
-				body.m_parent_vel_rel(2) = 0;
-				body.m_parent_acc_rel(0) = 0;
-				body.m_parent_acc_rel(1) = 0;
-				body.m_parent_acc_rel(2) = 0;
-				break;
-		}
-
-			// resize & initialize jacobians to zero.
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-		body.m_body_dot_Jac_T_u(0) = 0.0;
-		body.m_body_dot_Jac_T_u(1) = 0.0;
-		body.m_body_dot_Jac_T_u(2) = 0.0;
-		body.m_body_dot_Jac_R_u(0) = 0.0;
-		body.m_body_dot_Jac_R_u(1) = 0.0;
-		body.m_body_dot_Jac_R_u(2) = 0.0;
-		resize(body.m_body_Jac_T, m_num_dofs);
-		resize(body.m_body_Jac_R, m_num_dofs);
-		body.m_body_Jac_T.setZero();
-		body.m_body_Jac_R.setZero();
-#endif  //
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
-														   const vecx &dot_u, vecx *joint_forces)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
-		joint_forces->size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
-			static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
-		return -1;
-	}
-	// 1. relative kinematics
-	if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION))
-	{
-		bt_id_error_message("error in calculateKinematics\n");
-		return -1;
-	}
-	// 2. update contributions to equations of motion for every body.
-	for (idArrayIdx i = 0; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		// 3.4 update dynamic terms (rate of change of angular & linear momentum)
-		body.m_eom_lhs_rotational =
-			body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
-			body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
-			body.m_body_moment_user;
-		body.m_eom_lhs_translational =
-			body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
-			body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
-			body.m_body_force_user;
-	}
-
-	// 3. calculate full set of forces at parent joint
-	// (not directly calculating the joint force along the free direction
-	// simplifies inclusion of fixed joints.
-	// An alternative would be to fuse bodies in a pre-processing step,
-	// but that would make changing masses online harder (eg, payload masses
-	// added with fixed  joints to a gripper)
-	// Also, this enables adding zero weight bodies as a way to calculate frame poses
-	// for force elements, etc.
-
-	for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--)
-	{
-		// sum of forces and moments acting on this body from its children
-		vec3 sum_f_children;
-		vec3 sum_m_children;
-		setZero(sum_f_children);
-		setZero(sum_m_children);
-		for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
-			 child_list_idx++)
-		{
-			const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
-			vec3 child_joint_force_in_this_frame =
-				child.m_body_T_parent.transpose() * child.m_force_at_joint;
-			sum_f_children -= child_joint_force_in_this_frame;
-			sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
-							  child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
-		}
-		RigidBody &body = m_body_list[body_idx];
-
-		body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
-		body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
-	}
-
-	// 4. Calculate Joint forces.
-	// These are the components of force_at_joint/moment_at_joint
-	// in the free directions given by Jac_JT/Jac_JR
-	// 4.1 revolute joints
-	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_revolute_list[i]];
-		// (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
-		(*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
-	}
-	// 4.2 for prismatic joints
-	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-		// (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
-		(*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
-	}
-	// 4.3 floating bodies (6-DoF joints)
-	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_floating_list[i]];
-		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
-		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
-		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
-
-		(*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
-		(*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
-		(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
-	}
-
-	// 4.4 spherical bodies (3-DoF joints)
-	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-	{
-		//todo: review
-		RigidBody &body = m_body_list[m_body_spherical_list[i]];
-		(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
-		(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
-		(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
-	}
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u,
-													  const KinUpdateType type)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
-			static_cast<int>(dot_u.size()));
-		return -1;
-	}
-	if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION)
-	{
-		bt_id_error_message("invalid type %d\n", type);
-		return -1;
-	}
-
-	// 1. update relative kinematics
-	// 1.1 for revolute
-	for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_revolute_list[i]];
-		mat33 T;
-		bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
-		body.m_body_T_parent = T * body.m_body_T_parent_ref;
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
-		}
-	}
-	// 1.2 for prismatic
-	for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-		body.m_parent_pos_parent_body =
-			body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_parent_vel_rel =
-				body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
-		}
-	}
-	// 1.3 fixed joints: nothing to do
-	// 1.4 6dof joints:
-	for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[m_body_floating_list[i]];
-
-		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-							   transformY(q(body.m_q_index + 1)) *
-							   transformX(q(body.m_q_index));
-		body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
-		body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
-		body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
-		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
-			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
-			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
-
-			body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
-			body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
-			body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
-
-			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
-			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
-			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
-
-			body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
-			body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
-			body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
-
-			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
-		}
-	}
-	
-	for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-	{
-		//todo: review
-		RigidBody &body = m_body_list[m_body_spherical_list[i]];
-
-		mat33 T;
-
-		T = transformX(q(body.m_q_index)) *
-				transformY(q(body.m_q_index + 1)) *
-				transformZ(q(body.m_q_index + 2));
-		body.m_body_T_parent = T * body.m_body_T_parent_ref;
-			
-		body.m_parent_pos_parent_body(0)=0;
-		body.m_parent_pos_parent_body(1)=0;
-		body.m_parent_pos_parent_body(2)=0;
-		
-		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
-			body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
-			body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
-			body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
-			body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
-			body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
-			body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
-		}
-	}
-
-	// 2. absolute kinematic quantities (vector valued)
-	// NOTE: this should be optimized by specializing for different body types
-	// (e.g., relative rotation is always zero for prismatic joints, etc.)
-
-	// calculations for root body
-	{
-		RigidBody &body = m_body_list[0];
-		// 3.1 update absolute positions and orientations:
-		// will be required if we add force elements (eg springs between bodies,
-		// or contacts)
-		// not required right now, added here for debugging purposes
-		body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		body.m_body_T_world = body.m_body_T_parent;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			// 3.2 update absolute velocities
-			body.m_body_ang_vel = body.m_body_ang_vel_rel;
-			body.m_body_vel = body.m_parent_vel_rel;
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			// 3.3 update absolute accelerations
-			// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
-			body.m_body_ang_acc = body.m_body_ang_acc_rel;
-			body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
-			// add gravitational acceleration to root body
-			// this is an efficient way to add gravitational terms,
-			// but it does mean that the kinematics are no longer
-			// correct at the acceleration level
-			// NOTE: To get correct acceleration kinematics, just set world_gravity to zero
-			body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
-		}
-	}
-
-	for (idArrayIdx i = 1; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		RigidBody &parent = m_body_list[m_parent_index[i]];
-		// 2.1 update absolute positions and orientations:
-		// will be required if we add force elements (eg springs between bodies,
-		// or contacts)  not required right now added here for debugging purposes
-		body.m_body_pos =
-			body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
-		body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
-
-		if (type >= POSITION_VELOCITY)
-		{
-			// 2.2 update absolute velocities
-			body.m_body_ang_vel =
-				body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
-
-			body.m_body_vel =
-				body.m_body_T_parent *
-				(parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
-				 body.m_parent_vel_rel);
-		}
-		if (type >= POSITION_VELOCITY_ACCELERATION)
-		{
-			// 2.3 update absolute accelerations
-			// NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
-			body.m_body_ang_acc =
-				body.m_body_T_parent * parent.m_body_ang_acc -
-				body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
-				body.m_body_ang_acc_rel;
-			body.m_body_acc =
-				body.m_body_T_parent *
-				(parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
-				 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
-				 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
-		}
-	}
-
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-
-void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
-{
-	const int &idx = body.m_q_index;
-	switch (body.m_joint_type)
-	{
-		case FIXED:
-			break;
-		case REVOLUTE:
-			setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
-			setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
-			setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
-			break;
-		case PRISMATIC:
-			setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2),
-						 &body.m_body_Jac_T);
-			break;
-		case FLOATING:
-			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
-			// body_Jac_T = body_T_parent.transpose();
-			setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T);
-			setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T);
-			setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T);
-
-			setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T);
-			setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T);
-			setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T);
-
-			setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T);
-			setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
-			setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
-
-			break;
-		case SPHERICAL:
-			//todo: review
-			setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
-			setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
-			break;
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type)
-{
-	if (q.size() != m_num_dofs || u.size() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"wrong vector dimension. system has %d DOFs,\n"
-			"but dim(q)= %d, dim(u)= %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
-		return -1;
-	}
-	if (type != POSITION_ONLY && type != POSITION_VELOCITY)
-	{
-		bt_id_error_message("invalid type %d\n", type);
-		return -1;
-	}
-
-	addRelativeJacobianComponent(m_body_list[0]);
-	for (idArrayIdx i = 1; i < m_body_list.size(); i++)
-	{
-		RigidBody &body = m_body_list[i];
-		RigidBody &parent = m_body_list[m_parent_index[i]];
-
-		mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R);
-		body.m_body_Jac_T = parent.m_body_Jac_T;
-		mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x);
-		sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T);
-
-		addRelativeJacobianComponent(body);
-		mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T);
-
-		if (type >= POSITION_VELOCITY)
-		{
-			body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
-									  body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
-			body.m_body_dot_Jac_T_u = body.m_body_T_parent *
-									  (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
-									   parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
-									   2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
-		}
-	}
-	return 0;
-}
-#endif
-
-static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
-{
-	switch (dof)
-	{
-		// rotational part
-		case 0:
-			Jac_JR(0) = 1;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 1:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 1;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 2:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 1;
-			setZero(Jac_JT);
-			break;
-	}
-}
-
-static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
-{
-	switch (dof)
-	{
-		// rotational part
-		case 0:
-			Jac_JR(0) = 1;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 1:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 1;
-			Jac_JR(2) = 0;
-			setZero(Jac_JT);
-			break;
-		case 2:
-			Jac_JR(0) = 0;
-			Jac_JR(1) = 0;
-			Jac_JR(2) = 1;
-			setZero(Jac_JT);
-			break;
-		// translational part
-		case 3:
-			setZero(Jac_JR);
-			Jac_JT(0) = 1;
-			Jac_JT(1) = 0;
-			Jac_JT(2) = 0;
-			break;
-		case 4:
-			setZero(Jac_JR);
-			Jac_JT(0) = 0;
-			Jac_JT(1) = 1;
-			Jac_JT(2) = 0;
-			break;
-		case 5:
-			setZero(Jac_JR);
-			Jac_JT(0) = 0;
-			Jac_JT(1) = 0;
-			Jac_JT(2) = 1;
-			break;
-	}
-}
-
-static inline int jointNumDoFs(const JointType &type)
-{
-	switch (type)
-	{
-		case FIXED:
-			return 0;
-		case REVOLUTE:
-		case PRISMATIC:
-			return 1;
-		case FLOATING:
-			return 6;
-		case SPHERICAL:
-			return 3;
-	}
-	// this should never happen
-	bt_id_error_message("invalid joint type\n");
-	// TODO add configurable abort/crash function
-	abort();
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
-													  const bool initialize_matrix,
-													  const bool set_lower_triangular_matrix,
-													  matxx *mass_matrix)
-{
-	// This calculates the joint space mass matrix for the multibody system.
-	// The algorithm is essentially an implementation of "method 3"
-	// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
-	// (Later named "Composite Rigid Body Algorithm" by Featherstone).
-	//
-	// This implementation, however, handles branched systems and uses a formulation centered
-	// on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
-
-	if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
-		mass_matrix->cols() != m_num_dofs)
-	{
-		bt_id_error_message(
-			"Dimension error. System has %d DOFs,\n"
-			"but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
-			m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
-			static_cast<int>(mass_matrix->cols()));
-		return -1;
-	}
-
-	// TODO add optimized zeroing function?
-	if (initialize_matrix)
-	{
-		for (int i = 0; i < m_num_dofs; i++)
-		{
-			for (int j = 0; j < m_num_dofs; j++)
-			{
-				setMatxxElem(i, j, 0.0, mass_matrix);
-			}
-		}
-	}
-
-	if (update_kinematics)
-	{
-		// 1. update relative kinematics
-		// 1.1 for revolute joints
-		for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_revolute_list[i]];
-			// from reference orientation (q=0) of body-fixed frame to current orientation
-			mat33 body_T_body_ref;
-			bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
-			body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
-		}
-		// 1.2 for prismatic joints
-		for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_prismatic_list[i]];
-			// body.m_body_T_parent= fixed
-			body.m_parent_pos_parent_body =
-				body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
-		}
-		// 1.3 fixed joints: nothing to do
-		// 1.4 6dof joints:
-		for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
-		{
-			RigidBody &body = m_body_list[m_body_floating_list[i]];
-
-			body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-								   transformY(q(body.m_q_index + 1)) *
-								   transformX(q(body.m_q_index));
-			body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
-			body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
-			body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
-
-			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		}
-
-		for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
-		{
-			//todo: review
-			RigidBody &body = m_body_list[m_body_spherical_list[i]];
-
-			mat33 T;
-
-			T = transformX(q(body.m_q_index)) *
-				transformY(q(body.m_q_index + 1)) *
-				transformZ(q(body.m_q_index + 2));
-			body.m_body_T_parent = T * body.m_body_T_parent_ref;
-
-			body.m_parent_pos_parent_body(0)=0;
-			body.m_parent_pos_parent_body(1)=0;
-			body.m_parent_pos_parent_body(2)=0;
-			
-			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
-		}
-	}
-	for (int i = m_body_list.size() - 1; i >= 0; i--)
-	{
-		RigidBody &body = m_body_list[i];
-		// calculate mass, center of mass and inertia of "composite rigid body",
-		// ie, sub-tree starting at current body
-		body.m_subtree_mass = body.m_mass;
-		body.m_body_subtree_mass_com = body.m_body_mass_com;
-		body.m_body_subtree_I_body = body.m_body_I_body;
-
-		for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++)
-		{
-			RigidBody &child = m_body_list[m_child_indices[i][c]];
-			mat33 body_T_child = child.m_body_T_parent.transpose();
-
-			body.m_subtree_mass += child.m_subtree_mass;
-			body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com +
-											child.m_parent_pos_parent_body * child.m_subtree_mass;
-			body.m_body_subtree_I_body +=
-				body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
-
-			if (child.m_subtree_mass > 0)
-			{
-				// Shift the reference point for the child subtree inertia using the
-				// Huygens-Steiner ("parallel axis") theorem.
-				// (First shift from child origin to child com, then from there to this body's
-				// origin)
-				vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass;
-				mat33 tilde_r_child_com = tildeOperator(r_com);
-				mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com);
-				body.m_body_subtree_I_body +=
-					child.m_subtree_mass *
-					(tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com);
-			}
-		}
-	}
-
-	for (int i = m_body_list.size() - 1; i >= 0; i--)
-	{
-		const RigidBody &body = m_body_list[i];
-
-		// determine DoF-range for body
-		const int q_index_min = body.m_q_index;
-		const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1;
-		// loop over the DoFs used by this body
-		// local joint jacobians (ok as is for 1-DoF joints)
-		vec3 Jac_JR = body.m_Jac_JR;
-		vec3 Jac_JT = body.m_Jac_JT;
-		for (int col = q_index_max; col >= q_index_min; col--)
-		{
-			// set jacobians for 6-DoF joints
-			if (FLOATING == body.m_joint_type)
-			{
-				setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
-			}
-			if (SPHERICAL == body.m_joint_type)
-			{
-				//todo: review
-				setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
-			}
-
-			vec3 body_eom_rot =
-				body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
-			vec3 body_eom_trans =
-				body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
-			setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
-
-			// rest of the mass matrix column upwards
-			{
-				// 1. for multi-dof joints, rest of the dofs of this body
-				for (int row = col - 1; row >= q_index_min; row--)
-				{
-					if (SPHERICAL == body.m_joint_type)
-					{
-						//todo: review
-						setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-					if (FLOATING == body.m_joint_type)
-					{
-						setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-				}
-				// 2. ancestor dofs
-				int child_idx = i;
-				int parent_idx = m_parent_index[i];
-				while (parent_idx >= 0)
-				{
-					const RigidBody &child_body = m_body_list[child_idx];
-					const RigidBody &parent_body = m_body_list[parent_idx];
-
-					const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
-					body_eom_rot = parent_T_child * body_eom_rot;
-					body_eom_trans = parent_T_child * body_eom_trans;
-					body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
-
-					const int parent_body_q_index_min = parent_body.m_q_index;
-					const int parent_body_q_index_max =
-						parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
-					vec3 Jac_JR = parent_body.m_Jac_JR;
-					vec3 Jac_JT = parent_body.m_Jac_JT;
-					for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
-					{
-						if (SPHERICAL == parent_body.m_joint_type)
-						{
-							//todo: review
-							setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
-						}
-						// set jacobians for 6-DoF joints
-						if (FLOATING == parent_body.m_joint_type)
-						{
-							setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
-						}
-						const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
-						setMatxxElem(col, row, Mrc, mass_matrix);
-					}
-
-					child_idx = parent_idx;
-					parent_idx = m_parent_index[child_idx];
-				}
-			}
-		}
-	}
-
-	if (set_lower_triangular_matrix)
-	{
-		for (int col = 0; col < m_num_dofs; col++)
-		{
-			for (int row = 0; row < col; row++)
-			{
-				setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
-			}
-		}
-	}
-	return 0;
-}
-
-// utility macro
-#define CHECK_IF_BODY_INDEX_IS_VALID(index)                                                  \
-	do                                                                                       \
-	{                                                                                        \
-		if (index < 0 || index >= m_num_bodies)                                              \
-		{                                                                                    \
-			bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
-			return -1;                                                                       \
-		}                                                                                    \
-	} while (0)
-
-int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*p = m_parent_index[body_index];
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*user_int = m_user_int[body_index];
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*user_ptr = m_user_ptr[body_index];
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_user_int[body_index] = user_int;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_user_ptr[body_index] = user_ptr;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	if (body.m_mass > 0)
-	{
-		*world_com = body.m_body_T_world.transpose() *
-					 (body.m_body_pos + body.m_body_mass_com / body.m_mass);
-	}
-	else
-	{
-		*world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
-	}
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_T_body = body.m_body_T_world.transpose();
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
-														vec3 *world_velocity) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
-														   vec3 *world_velocity) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	vec3 com;
-	if (body.m_mass > 0)
-	{
-		com = body.m_body_mass_com / body.m_mass;
-	}
-	else
-	{
-		com(0) = 0;
-		com(1) = 0;
-		com(2) = 0;
-	}
-
-	*world_velocity =
-		body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com));
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
-															 vec3 *world_dot_omega) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
-															vec3 *world_acceleration) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*joint_type = m_body_list[body_index].m_joint_type;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
-												  const char **joint_type) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*r = m_body_list[body_index].m_parent_pos_parent_body_ref;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*T = m_body_list[body_index].m_body_T_parent_ref;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	if (m_body_list[body_index].m_joint_type == REVOLUTE)
-	{
-		*axis = m_body_list[body_index].m_Jac_JR;
-		return 0;
-	}
-	if (m_body_list[body_index].m_joint_type == PRISMATIC)
-	{
-		*axis = m_body_list[body_index].m_Jac_JT;
-		return 0;
-	}
-	setZero(*axis);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*q_index = m_body_list[body_index].m_q_index;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_mass = mass;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
-														 const vec3 &first_mass_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_mass_com = first_mass_moment;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
-														  const mat33 &second_mass_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_I_body = second_mass_moment;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*mass = m_body_list[body_index].m_mass;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
-														 vec3 *first_mass_moment) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*first_mass_moment = m_body_list[body_index].m_body_mass_com;
-	return 0;
-}
-int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
-														  mat33 *second_mass_moment) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	*second_mass_moment = m_body_list[body_index].m_body_I_body;
-	return 0;
-}
-
-void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments()
-{
-	for (int index = 0; index < m_num_bodies; index++)
-	{
-		RigidBody &body = m_body_list[index];
-		setZero(body.m_body_force_user);
-		setZero(body.m_body_moment_user);
-	}
-}
-
-int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_force_user += body_force;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment)
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	m_body_list[body_index].m_body_moment_user += body_moment;
-	return 0;
-}
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	*world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
-	return 0;
-}
-
-int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
-{
-	CHECK_IF_BODY_INDEX_IS_VALID(body_index);
-	const RigidBody &body = m_body_list[body_index];
-	mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot);
-	return 0;
-}
-
-#endif
-}  // namespace btInverseDynamics

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp

@@ -1,288 +0,0 @@
-// The structs and classes defined here provide a basic inverse fynamics implementation used
-// by MultiBodyTree
-// User interaction should be through MultiBodyTree
-
-#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_
-#define MULTI_BODY_REFERENCE_IMPL_HPP_
-
-#include "../IDConfig.hpp"
-#include "../MultiBodyTree.hpp"
-
-namespace btInverseDynamics
-{
-/// Structure for for rigid body mass properties, connectivity and kinematic state
-/// all vectors and matrices are in body-fixed frame, if not indicated otherwise.
-/// The body-fixed frame is located in the joint connecting the body to its parent.
-struct RigidBody
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	// 1 Inertial properties
-	/// Mass
-	idScalar m_mass;
-	/// Mass times center of gravity in body-fixed frame
-	vec3 m_body_mass_com;
-	/// Moment of inertia w.r.t. body-fixed frame
-	mat33 m_body_I_body;
-
-	// 2 dynamic properties
-	/// Left-hand side of the body equation of motion, translational part
-	vec3 m_eom_lhs_translational;
-	/// Left-hand side of the body equation of motion, rotational part
-	vec3 m_eom_lhs_rotational;
-	/// Force acting at the joint when the body is cut from its parent;
-	/// includes impressed joint force in J_JT direction,
-	/// as well as constraint force,
-	/// in body-fixed frame
-	vec3 m_force_at_joint;
-	/// Moment acting at the joint when the body is cut from its parent;
-	/// includes impressed joint moment in J_JR direction, and constraint moment
-	/// in body-fixed frame
-	vec3 m_moment_at_joint;
-	/// external (user provided) force acting at the body-fixed frame's origin, written in that
-	/// frame
-	vec3 m_body_force_user;
-	/// external (user provided) moment acting at the body-fixed frame's origin, written in that
-	/// frame
-	vec3 m_body_moment_user;
-	// 3 absolute kinematic properties
-	/// Position of body-fixed frame relative to world frame
-	/// this is currently only for debugging purposes
-	vec3 m_body_pos;
-	/// Absolute velocity of body-fixed frame
-	vec3 m_body_vel;
-	/// Absolute acceleration of body-fixed frame
-	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
-	/// acceleration!
-	vec3 m_body_acc;
-	/// Absolute angular velocity
-	vec3 m_body_ang_vel;
-	/// Absolute angular acceleration
-	/// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational
-	/// acceleration!
-	vec3 m_body_ang_acc;
-
-	// 4 relative kinematic properties.
-	// these are in the parent body frame
-	/// Transform from world to body-fixed frame;
-	/// this is currently only for debugging purposes
-	mat33 m_body_T_world;
-	/// Transform from parent to body-fixed frame
-	mat33 m_body_T_parent;
-	/// Vector from parent to child frame in parent frame
-	vec3 m_parent_pos_parent_body;
-	/// Relative angular velocity
-	vec3 m_body_ang_vel_rel;
-	/// Relative linear velocity
-	vec3 m_parent_vel_rel;
-	/// Relative angular acceleration
-	vec3 m_body_ang_acc_rel;
-	/// Relative linear acceleration
-	vec3 m_parent_acc_rel;
-
-	// 5 Data describing the joint type and geometry
-	/// Type of joint
-	JointType m_joint_type;
-	/// Position of joint frame (body-fixed frame at q=0) relative to the parent frame
-	/// Components are in body-fixed frame of the parent
-	vec3 m_parent_pos_parent_body_ref;
-	/// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame
-	mat33 m_body_T_parent_ref;
-	/// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute
-	/// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
-	/// For revolute joints this is the joint axis, for prismatic joints it is a null matrix.
-	/// (NOTE: dimensions will have to be dynamic for additional joint types!)
-	vec3 m_Jac_JR;
-	/// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute
-	/// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom.
-	/// For prismatic joints this is the joint axis, for revolute joints it is a null matrix.
-	/// (NOTE: dimensions might have to be dynamic for additional joint types!)
-	vec3 m_Jac_JT;
-	/// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT
-	vec3 m_parent_Jac_JT;
-	/// Start of index range for the position degree(s) of freedom describing this body's motion
-	/// relative to
-	/// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates.
-	int m_q_index;
-
-	// 6 Scratch data for mass matrix computation using "composite rigid body algorithm"
-	/// mass of the subtree rooted in this body
-	idScalar m_subtree_mass;
-	/// center of mass * mass for subtree rooted in this body, in body-fixed frame
-	vec3 m_body_subtree_mass_com;
-	/// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame
-	mat33 m_body_subtree_I_body;
-
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// translational jacobian in body-fixed frame d(m_body_vel)/du
-	mat3x m_body_Jac_T;
-	/// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du
-	mat3x m_body_Jac_R;
-	/// components of linear acceleration depending on u
-	/// (same as is d(m_Jac_T)/dt*u)
-	vec3 m_body_dot_Jac_T_u;
-	/// components of angular acceleration depending on u
-	/// (same as is d(m_Jac_T)/dt*u)
-	vec3 m_body_dot_Jac_R_u;
-#endif
-};
-
-/// The MBS implements a tree structured multibody system
-class MultiBodyTree::MultiBodyImpl
-{
-	friend class MultiBodyTree;
-
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	enum KinUpdateType
-	{
-		POSITION_ONLY,
-		POSITION_VELOCITY,
-		POSITION_VELOCITY_ACCELERATION
-	};
-
-	/// constructor
-	/// @param num_bodies the number of bodies in the system
-	/// @param num_dofs number of degrees of freedom in the system
-	MultiBodyImpl(int num_bodies_, int num_dofs_);
-
-	/// \copydoc MultiBodyTree::calculateInverseDynamics
-	int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u,
-								 vecx* joint_forces);
-	///\copydoc MultiBodyTree::calculateMassMatrix
-	int calculateMassMatrix(const vecx& q, const bool update_kinematics,
-							const bool initialize_matrix, const bool set_lower_triangular_matrix,
-							matxx* mass_matrix);
-	/// calculate kinematics (vector quantities)
-	/// Depending on type, update positions only, positions & velocities, or positions, velocities
-	/// and accelerations.
-	int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type);
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	/// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms.
-	int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type);
-	/// \copydoc MultiBodyTree::getBodyDotJacobianTransU
-	int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const;
-	/// \copydoc MultiBodyTree::getBodyDotJacobianRotU
-	int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const;
-	/// \copydoc MultiBodyTree::getBodyJacobianTrans
-	int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const;
-	/// \copydoc MultiBodyTree::getBodyJacobianRot
-	int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const;
-	/// Add relative Jacobian component from motion relative to parent body
-	/// @param body the body to add the Jacobian component for
-	void addRelativeJacobianComponent(RigidBody& body);
-#endif
-	/// generate additional index sets from the parent_index array
-	/// @return -1 on error, 0 on success
-	int generateIndexSets();
-	/// set gravity acceleration in world frame
-	/// @param gravity gravity vector in the world frame
-	/// @return 0 on success, -1 on error
-	int setGravityInWorldFrame(const vec3& gravity);
-	/// pretty print tree
-	void printTree();
-	/// print tree data
-	void printTreeData();
-	/// initialize fixed data
-	void calculateStaticData();
-	/// \copydoc MultiBodyTree::getBodyFrame
-	int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const;
-	/// \copydoc MultiBodyTree::getParentIndex
-	int getParentIndex(const int body_index, int* m_parent_index);
-	/// \copydoc MultiBodyTree::getJointType
-	int getJointType(const int body_index, JointType* joint_type) const;
-	/// \copydoc MultiBodyTree::getJointTypeStr
-	int getJointTypeStr(const int body_index, const char** joint_type) const;
-	/// \copydoc MultiBodyTree::getParentRParentBodyRef
-	int getParentRParentBodyRef(const int body_index, vec3* r) const;
-	/// \copydoc MultiBodyTree::getBodyTParentRef
-	int getBodyTParentRef(const int body_index, mat33* T) const;
-	/// \copydoc MultiBodyTree::getBodyAxisOfMotion
-	int getBodyAxisOfMotion(const int body_index, vec3* axis) const;
-	/// \copydoc MultiBodyTree:getDoFOffset
-	int getDoFOffset(const int body_index, int* q_index) const;
-	/// \copydoc MultiBodyTree::getBodyOrigin
-	int getBodyOrigin(const int body_index, vec3* world_origin) const;
-	/// \copydoc MultiBodyTree::getBodyCoM
-	int getBodyCoM(const int body_index, vec3* world_com) const;
-	/// \copydoc MultiBodyTree::getBodyTransform
-	int getBodyTransform(const int body_index, mat33* world_T_body) const;
-	/// \copydoc MultiBodyTree::getBodyAngularVelocity
-	int getBodyAngularVelocity(const int body_index, vec3* world_omega) const;
-	/// \copydoc MultiBodyTree::getBodyLinearVelocity
-	int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const;
-	/// \copydoc MultiBodyTree::getBodyLinearVelocityCoM
-	int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const;
-	/// \copydoc MultiBodyTree::getBodyAngularAcceleration
-	int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const;
-	/// \copydoc MultiBodyTree::getBodyLinearAcceleration
-	int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const;
-	/// \copydoc MultiBodyTree::getUserInt
-	int getUserInt(const int body_index, int* user_int) const;
-	/// \copydoc MultiBodyTree::getUserPtr
-	int getUserPtr(const int body_index, void** user_ptr) const;
-	/// \copydoc MultiBodyTree::setUserInt
-	int setUserInt(const int body_index, const int user_int);
-	/// \copydoc MultiBodyTree::setUserPtr
-	int setUserPtr(const int body_index, void* const user_ptr);
-	///\copydoc MultiBodytTree::setBodyMass
-	int setBodyMass(const int body_index, const idScalar mass);
-	///\copydoc MultiBodytTree::setBodyFirstMassMoment
-	int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment);
-	///\copydoc MultiBodytTree::setBodySecondMassMoment
-	int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment);
-	///\copydoc MultiBodytTree::getBodyMass
-	int getBodyMass(const int body_index, idScalar* mass) const;
-	///\copydoc MultiBodytTree::getBodyFirstMassMoment
-	int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const;
-	///\copydoc MultiBodytTree::getBodySecondMassMoment
-	int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const;
-	/// \copydoc MultiBodyTree::clearAllUserForcesAndMoments
-	void clearAllUserForcesAndMoments();
-	/// \copydoc MultiBodyTree::addUserForce
-	int addUserForce(const int body_index, const vec3& body_force);
-	/// \copydoc MultiBodyTree::addUserMoment
-	int addUserMoment(const int body_index, const vec3& body_moment);
-
-private:
-	// debug function. print tree structure to stdout
-	void printTree(int index, int indentation);
-	// get string representation of JointType (for debugging)
-	const char* jointTypeToString(const JointType& type) const;
-	// get number of degrees of freedom from joint type
-	int bodyNumDoFs(const JointType& type) const;
-	// number of bodies in the system
-	int m_num_bodies;
-	// number of degrees of freedom
-	int m_num_dofs;
-	// Gravitational acceleration (in world frame)
-	vec3 m_world_gravity;
-	// vector of bodies in the system
-	// body 0 is used as an environment body and is allways fixed.
-	// The bodies are ordered such that a parent body always has an index
-	// smaller than its child.
-	idArray<RigidBody>::type m_body_list;
-	// Parent_index[i] is the index for i's parent body in body_list.
-	// This fully describes the tree.
-	idArray<int>::type m_parent_index;
-	// child_indices[i] contains a vector of indices of
-	// all children of the i-th body
-	idArray<idArray<int>::type>::type m_child_indices;
-	// Indices of rotary joints
-	idArray<int>::type m_body_revolute_list;
-	// Indices of prismatic joints
-	idArray<int>::type m_body_prismatic_list;
-	// Indices of floating joints
-	idArray<int>::type m_body_floating_list;
-	// Indices of spherical joints
-	idArray<int>::type m_body_spherical_list;
-	// a user-provided integer
-	idArray<int>::type m_user_int;
-	// a user-provided pointer
-	idArray<void*>::type m_user_ptr;
-#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
-	mat3x m_m3x;
-#endif
-};
-}  // namespace btInverseDynamics
-#endif

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

@@ -1,131 +0,0 @@
-#include "MultiBodyTreeInitCache.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::InitCache::InitCache()
-{
-	m_inertias.resize(0);
-	m_joints.resize(0);
-	m_num_dofs = 0;
-	m_root_index = -1;
-}
-
-int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
-									  const JointType joint_type,
-									  const vec3& parent_r_parent_body_ref,
-									  const mat33& body_T_parent_ref,
-									  const vec3& body_axis_of_motion, const idScalar mass,
-									  const vec3& body_r_body_com, const mat33& body_I_body,
-									  const int user_int, void* user_ptr)
-{
-	switch (joint_type)
-	{
-		case REVOLUTE:
-		case PRISMATIC:
-			m_num_dofs += 1;
-			break;
-		case FIXED:
-			// does not add a degree of freedom
-			// m_num_dofs+=0;
-			break;
-		case SPHERICAL:
-			m_num_dofs += 3;
-			break;
-		case FLOATING:
-			m_num_dofs += 6;
-			break;
-		default:
-			bt_id_error_message("unknown joint type %d\n", joint_type);
-			return -1;
-	}
-
-	if (-1 == parent_index)
-	{
-		if (m_root_index >= 0)
-		{
-			bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
-								body_index, m_root_index);
-			return -1;
-		}
-		m_root_index = body_index;
-	}
-
-	JointData joint;
-	joint.m_child = body_index;
-	joint.m_parent = parent_index;
-	joint.m_type = joint_type;
-	joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
-	joint.m_child_T_parent_ref = body_T_parent_ref;
-	joint.m_child_axis_of_motion = body_axis_of_motion;
-
-	InertiaData body;
-	body.m_mass = mass;
-	body.m_body_pos_body_com = body_r_body_com;
-	body.m_body_I_body = body_I_body;
-
-	m_inertias.push_back(body);
-	m_joints.push_back(joint);
-	m_user_int.push_back(user_int);
-	m_user_ptr.push_back(user_ptr);
-	return 0;
-}
-int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const
-{
-	if (index < 0 || index > static_cast<int>(m_inertias.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-
-	*inertia = m_inertias[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const
-{
-	if (index < 0 || index > static_cast<int>(m_user_int.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*user_int = m_user_int[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
-{
-	if (index < 0 || index > static_cast<int>(m_user_ptr.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*user_ptr = m_user_ptr[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const
-{
-	if (index < 0 || index > static_cast<int>(m_joints.size()))
-	{
-		bt_id_error_message("index out of range\n");
-		return -1;
-	}
-	*joint = m_joints[index];
-	return 0;
-}
-
-int MultiBodyTree::InitCache::buildIndexSets()
-{
-	// NOTE: This function assumes that proper indices were provided
-	//	   User2InternalIndex from utils can be used to facilitate this.
-
-	m_parent_index.resize(numBodies());
-	for (idArrayIdx j = 0; j < m_joints.size(); j++)
-	{
-		const JointData& joint = m_joints[j];
-		m_parent_index[joint.m_child] = joint.m_parent;
-	}
-
-	return 0;
-}
-}  // namespace btInverseDynamics

thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp

@@ -1,113 +0,0 @@
-#ifndef MULTIBODYTREEINITCACHE_HPP_
-#define MULTIBODYTREEINITCACHE_HPP_
-
-#include "../IDConfig.hpp"
-#include "../IDMath.hpp"
-#include "../MultiBodyTree.hpp"
-
-namespace btInverseDynamics
-{
-/// Mass properties of a rigid body
-struct InertiaData
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	/// mass
-	idScalar m_mass;
-	/// vector from body-fixed frame to center of mass,
-	/// in body-fixed frame, multiplied by the mass
-	vec3 m_body_pos_body_com;
-	/// moment of inertia w.r.t. the origin of the body-fixed
-	/// frame, represented in that frame
-	mat33 m_body_I_body;
-};
-
-/// Joint properties
-struct JointData
-{
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-
-	/// type of joint
-	JointType m_type;
-	/// index of parent body
-	int m_parent;
-	/// index of child body
-	int m_child;
-	/// vector from parent's body-fixed frame to child's body-fixed
-	/// frame for q=0, written in the parent's body fixed frame
-	vec3 m_parent_pos_parent_child_ref;
-	/// Transform matrix converting vectors written in the parent's frame
-	/// into vectors written in the child's frame for q=0
-	/// ie, child_vector = child_T_parent_ref * parent_vector;
-	mat33 m_child_T_parent_ref;
-	/// Axis of motion for 1 degree-of-freedom joints,
-	/// written in the child's frame
-	/// For revolute joints, the q-value is positive for a positive
-	/// rotation about this axis.
-	/// For prismatic joints, the q-value is positive for a positive
-	/// translation is this direction.
-	vec3 m_child_axis_of_motion;
-};
-
-/// Data structure to store data passed by the user.
-/// This is used in MultiBodyTree::finalize to build internal data structures.
-class MultiBodyTree::InitCache
-{
-public:
-	ID_DECLARE_ALIGNED_ALLOCATOR();
-	/// constructor
-	InitCache();
-	///\copydoc MultiBodyTree::addBody
-	int addBody(const int body_index, const int parent_index, const JointType joint_type,
-				const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
-				const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com,
-				const mat33 &body_I_body, const int user_int, void *user_ptr);
-	/// build index arrays
-	/// @return 0 on success, -1 on failure
-	int buildIndexSets();
-	/// @return number of degrees of freedom
-	int numDoFs() const { return m_num_dofs; }
-	/// @return number of bodies
-	int numBodies() const { return m_inertias.size(); }
-	/// get inertia data for index
-	/// @param index of the body
-	/// @param inertia pointer for return data
-	/// @return 0 on success, -1 on failure
-	int getInertiaData(const int index, InertiaData *inertia) const;
-	/// get joint data for index
-	/// @param index of the body
-	/// @param joint pointer for return data
-	/// @return 0 on success, -1 on failure
-	int getJointData(const int index, JointData *joint) const;
-	/// get parent index array (paren_index[i] is the index of the parent of i)
-	/// @param parent_index pointer for return data
-	void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
-	/// get user integer
-	/// @param index body index
-	/// @param user_int user integer
-	/// @return 0 on success, -1 on failure
-	int getUserInt(const int index, int *user_int) const;
-	/// get user pointer
-	/// @param index body index
-	/// @param user_int user pointer
-	/// @return 0 on success, -1 on failure
-	int getUserPtr(const int index, void **user_ptr) const;
-
-private:
-	// vector of bodies
-	idArray<InertiaData>::type m_inertias;
-	// vector of joints
-	idArray<JointData>::type m_joints;
-	// number of mechanical degrees of freedom
-	int m_num_dofs;
-	// parent index array
-	idArray<int>::type m_parent_index;
-	// user integers
-	idArray<int>::type m_user_int;
-	// user pointers
-	idArray<void *>::type m_user_ptr;
-	// index of root body (or -1 if not set)
-	int m_root_index;
-};
-}  // namespace btInverseDynamics
-#endif  // MULTIBODYTREEINITCACHE_HPP_