* Adjustment: Update Bullet version to 3.24.

Engine/lib/bullet/Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h

@@ -0,0 +1,15 @@
+#ifndef BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
+#define BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
+#include "CoilCreator.hpp"
+#include "MultiBodyTreeCreator.hpp"
+#include "btMultiBodyFromURDF.hpp"
+#include "DillCreator.hpp"
+#include "MultiBodyTreeDebugGraph.hpp"
+#include "btMultiBodyTreeCreator.hpp"
+#include "IDRandomUtil.hpp"
+#include "SimpleTreeCreator.hpp"
+#include "invdyn_bullet_comparison.hpp"
+#include "MultiBodyNameMap.hpp"
+#include "User2InternalIndex.hpp"
+
+#endif  //BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H

Engine/lib/bullet/Extras/InverseDynamics/CMakeLists.txt

@@ -0,0 +1,50 @@
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+ADD_LIBRARY(
+BulletInverseDynamicsUtils
+CloneTreeCreator.cpp
+CoilCreator.cpp
+MultiBodyTreeCreator.cpp
+btMultiBodyTreeCreator.cpp
+DillCreator.cpp
+MultiBodyTreeDebugGraph.cpp
+invdyn_bullet_comparison.cpp
+IDRandomUtil.cpp
+RandomTreeCreator.cpp
+SimpleTreeCreator.cpp
+MultiBodyNameMap.cpp
+User2InternalIndex.cpp
+)
+
+SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (BUILD_SHARED_LIBS)
+	TARGET_LINK_LIBRARIES(BulletInverseDynamicsUtils BulletInverseDynamics BulletDynamics BulletCollision Bullet3Common LinearMath)
+ENDIF (BUILD_SHARED_LIBS)
+
+IF (INSTALL_EXTRA_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamicsUtils  DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamicsUtils
+                                        RUNTIME DESTINATION bin
+                                        LIBRARY DESTINATION lib${LIB_SUFFIX}
+                                        ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h"  PATTERN
+".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils  PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES PUBLIC_HEADER "BulletInverseDynamicsUtilsCommon.h" )
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_EXTRA_LIBS)

Engine/lib/bullet/Extras/InverseDynamics/CloneTreeCreator.cpp

@@ -0,0 +1,59 @@
+#include "CloneTreeCreator.hpp"
+
+#include <cstdio>
+
+namespace btInverseDynamics
+{
+#define CHECK_NULLPTR()                                  \
+	do                                                   \
+	{                                                    \
+		if (m_reference == 0x0)                          \
+		{                                                \
+			bt_id_error_message("m_reference == 0x0\n"); \
+			return -1;                                   \
+		}                                                \
+	} while (0)
+
+#define TRY(x)                                             \
+	do                                                     \
+	{                                                      \
+		if (x == -1)                                       \
+		{                                                  \
+			bt_id_error_message("error calling " #x "\n"); \
+			return -1;                                     \
+		}                                                  \
+	} while (0)
+CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference)
+{
+	m_reference = reference;
+}
+
+CloneTreeCreator::~CloneTreeCreator() {}
+
+int CloneTreeCreator::getNumBodies(int* num_bodies) const
+{
+	CHECK_NULLPTR();
+	*num_bodies = m_reference->numBodies();
+	return 0;
+}
+
+int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+							  vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+							  vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+							  mat33* body_I_body, int* user_int, void** user_ptr) const
+{
+	CHECK_NULLPTR();
+	TRY(m_reference->getParentIndex(body_index, parent_index));
+	TRY(m_reference->getJointType(body_index, joint_type));
+	TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
+	TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
+	TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
+	TRY(m_reference->getBodyMass(body_index, mass));
+	TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
+	TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
+	TRY(m_reference->getUserInt(body_index, user_int));
+	TRY(m_reference->getUserPtr(body_index, user_ptr));
+
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/CloneTreeCreator.hpp

@@ -0,0 +1,29 @@
+#ifndef CLONETREE_CREATOR_HPP_
+#define CLONETREE_CREATOR_HPP_
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+/// Generate an identical multibody tree from a reference system.
+class CloneTreeCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor
+	/// @param reference the MultiBodyTree to clone
+	CloneTreeCreator(const MultiBodyTree* reference);
+	~CloneTreeCreator();
+	///\copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int* num_bodies) const;
+	///\copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int* parent_index, JointType* joint_type,
+				vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+				idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+				void** user_ptr) const;
+
+private:
+	const MultiBodyTree* m_reference;
+};
+}  // namespace btInverseDynamics
+#endif  // CLONETREE_CREATOR_HPP_

Engine/lib/bullet/Extras/InverseDynamics/CoilCreator.cpp

@@ -0,0 +1,73 @@
+#include <cmath>
+
+#include "CoilCreator.hpp"
+
+namespace btInverseDynamics
+{
+CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n)
+{
+	for (int i = 0; i < m_num_bodies; i++)
+	{
+		m_parent[i] = i - 1;
+	}
+
+	// DH parameters (that's what's in the paper ...)
+	const idScalar theta_DH = 0;
+	const idScalar d_DH = 0.0;
+	const idScalar a_DH = 1.0 / m_num_bodies;
+	const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies;
+	getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref,
+					&m_body_T_parent_ref);
+	// always z-axis
+	m_body_axis_of_motion(0) = 0.0;
+	m_body_axis_of_motion(1) = 0.0;
+	m_body_axis_of_motion(2) = 1.0;
+
+	m_mass = 1.0 / m_num_bodies;
+	m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies);
+	m_body_r_body_com(1) = 0.0;
+	m_body_r_body_com(2) = 0.0;
+
+	m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies);
+	m_body_I_body(0, 1) = 0.0;
+	m_body_I_body(0, 2) = 0.0;
+	m_body_I_body(1, 0) = 0.0;
+	m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies);
+	m_body_I_body(1, 2) = 0.0;
+	m_body_I_body(2, 0) = 0.0;
+	m_body_I_body(2, 1) = 0.0;
+	m_body_I_body(2, 2) = m_body_I_body(1, 1);
+}
+
+CoilCreator::~CoilCreator() {}
+
+int CoilCreator::getNumBodies(int* num_bodies) const
+{
+	*num_bodies = m_num_bodies;
+	return 0;
+}
+
+int CoilCreator::getBody(int body_index, int* parent_index, JointType* joint_type,
+						 vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+						 vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+						 mat33* body_I_body, int* user_int, void** user_ptr) const
+{
+	if (body_index < 0 || body_index >= m_num_bodies)
+	{
+		bt_id_error_message("invalid body index %d\n", body_index);
+		return -1;
+	}
+	*parent_index = m_parent[body_index];
+	*joint_type = REVOLUTE;
+	*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
+	*body_T_parent_ref = m_body_T_parent_ref;
+	*body_axis_of_motion = m_body_axis_of_motion;
+	*mass = m_mass;
+	*body_r_body_com = m_body_r_body_com;
+	*body_I_body = m_body_I_body;
+
+	*user_int = 0;
+	*user_ptr = 0;
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/CoilCreator.hpp

@@ -0,0 +1,41 @@
+#ifndef COILCREATOR_HPP_
+#define COILCREATOR_HPP_
+
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+/// Creator class for building a "coil" system as intruduced as benchmark example in
+/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
+/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.",  The International
+/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
+///
+/// This is a serial chain, with an initial configuration resembling a coil.
+class CoilCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor.
+	/// @param n the number of bodies in the system
+	CoilCreator(int n);
+	/// dtor
+	~CoilCreator();
+	// \copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int* num_bodies) const;
+	// \copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int* parent_index, JointType* joint_type,
+				vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+				idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+				void** user_ptr) const;
+
+private:
+	int m_num_bodies;
+	std::vector<int> m_parent;
+	vec3 m_parent_r_parent_body_ref;
+	mat33 m_body_T_parent_ref;
+	vec3 m_body_axis_of_motion;
+	idScalar m_mass;
+	vec3 m_body_r_body_com;
+	mat33 m_body_I_body;
+};
+}  // namespace btInverseDynamics
+#endif

Engine/lib/bullet/Extras/InverseDynamics/DillCreator.cpp

@@ -0,0 +1,136 @@
+#include "DillCreator.hpp"
+#include <cmath>
+namespace btInverseDynamics
+{
+DillCreator::DillCreator(int level)
+	: m_level(level),
+	  m_num_bodies(BT_ID_POW(2, level))
+{
+	m_parent.resize(m_num_bodies);
+	m_parent_r_parent_body_ref.resize(m_num_bodies);
+	m_body_T_parent_ref.resize(m_num_bodies);
+	m_body_axis_of_motion.resize(m_num_bodies);
+	m_mass.resize(m_num_bodies);
+	m_body_r_body_com.resize(m_num_bodies);
+	m_body_I_body.resize(m_num_bodies);
+
+	// generate names (for debugging)
+	for (int i = 0; i < m_num_bodies; i++)
+	{
+		m_parent[i] = i - 1;
+
+		// all z-axis (DH convention)
+		m_body_axis_of_motion[i](0) = 0.0;
+		m_body_axis_of_motion[i](1) = 0.0;
+		m_body_axis_of_motion[i](2) = 1.0;
+	}
+
+	// recursively build data structures
+	m_current_body = 0;
+	const int parent = -1;
+	const idScalar d_DH = 0.0;
+	const idScalar a_DH = 0.0;
+	const idScalar alpha_DH = 0.0;
+
+	if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH))
+	{
+		bt_id_error_message("recurseDill failed\n");
+		abort();
+	}
+}
+
+DillCreator::~DillCreator() {}
+
+int DillCreator::getNumBodies(int* num_bodies) const
+{
+	*num_bodies = m_num_bodies;
+	return 0;
+}
+
+int DillCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+						 vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+						 vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+						 mat33* body_I_body, int* user_int, void** user_ptr) const
+{
+	if (body_index < 0 || body_index >= m_num_bodies)
+	{
+		bt_id_error_message("invalid body index %d\n", body_index);
+		return -1;
+	}
+	*parent_index = m_parent[body_index];
+	*joint_type = REVOLUTE;
+	*parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index];
+	*body_T_parent_ref = m_body_T_parent_ref[body_index];
+	*body_axis_of_motion = m_body_axis_of_motion[body_index];
+	*mass = m_mass[body_index];
+	*body_r_body_com = m_body_r_body_com[body_index];
+	*body_I_body = m_body_I_body[body_index];
+
+	*user_int = 0;
+	*user_ptr = 0;
+	return 0;
+}
+
+int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
+							 const idScalar a_DH_in, const idScalar alpha_DH_in)
+{
+	if (level < 0)
+	{
+		bt_id_error_message("invalid level parameter (%d)\n", level);
+		return -1;
+	}
+
+	if (m_current_body >= m_num_bodies || m_current_body < 0)
+	{
+		bt_id_error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
+							m_num_bodies);
+		return -1;
+	}
+
+	idScalar size = BT_ID_MAX(level, 1);
+	const int body = m_current_body;
+	//  length = 0.1 * size;
+	//  with = 2 * 0.01 * size;
+
+	/// these parameters are from the paper ...
+	/// TODO: add proper citation
+	m_parent[body] = parent;
+	m_mass[body] = 0.1 * BT_ID_POW(size, 3);
+	m_body_r_body_com[body](0) = 0.05 * size;
+	m_body_r_body_com[body](1) = 0;
+	m_body_r_body_com[body](2) = 0;
+	// initialization
+	for (int i = 0; i < 3; i++)
+	{
+		m_parent_r_parent_body_ref[body](i) = 0;
+		for (int j = 0; j < 3; j++)
+		{
+			m_body_I_body[body](i, j) = 0.0;
+			m_body_T_parent_ref[body](i, j) = 0.0;
+		}
+	}
+        const idScalar size_5 = std::pow(size, 5);
+        m_body_I_body[body](0, 0) = size_5 / 0.2e6;
+        m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
+	m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);
+
+	getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body],
+					&m_body_T_parent_ref[body]);
+
+	// attach "level" Dill systems of levels 1...level
+	for (int i = 1; i <= level; i++)
+	{
+		idScalar d_DH = 0.01 * size;
+		if (i == level)
+		{
+			d_DH = 0.0;
+		}
+		const idScalar a_DH = i * 0.1;
+		const idScalar alpha_DH = i * BT_ID_PI / 3.0;
+		m_current_body++;
+		recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
+	}
+
+	return 0;  // ok!
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/DillCreator.hpp

@@ -0,0 +1,47 @@
+#ifndef DILLCREATOR_HPP_
+#define DILLCREATOR_HPP_
+
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+/// Creator class for building a "Dill" system as intruduced as benchmark example in
+/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n))
+/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.",  The International
+/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628.
+///
+/// This is a self-similar branched tree, somewhat resembling a dill plant
+class DillCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor
+	/// @param levels the number of dill levels
+	DillCreator(int levels);
+	/// dtor
+	~DillCreator();
+	///\copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int* num_bodies) const;
+	///\copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int* parent_index, JointType* joint_type,
+				vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+				idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+				void** user_ptr) const;
+
+private:
+	/// recursively generate dill bodies.
+	/// TODO better documentation
+	int recurseDill(const int levels, const int parent, const idScalar d_DH_in,
+					const idScalar a_DH_in, const idScalar alpha_DH_in);
+	int m_level;
+	int m_num_bodies;
+	idArray<int>::type m_parent;
+	idArray<vec3>::type m_parent_r_parent_body_ref;
+	idArray<mat33>::type m_body_T_parent_ref;
+	idArray<vec3>::type m_body_axis_of_motion;
+	idArray<idScalar>::type m_mass;
+	idArray<vec3>::type m_body_r_body_com;
+	idArray<mat33>::type m_body_I_body;
+	int m_current_body;
+};
+}  // namespace btInverseDynamics
+#endif

Engine/lib/bullet/Extras/InverseDynamics/IDRandomUtil.cpp

@@ -0,0 +1,76 @@
+#include <cmath>
+#include <cstdlib>
+#include <ctime>
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "BulletInverseDynamics/IDMath.hpp"
+#include "IDRandomUtil.hpp"
+
+namespace btInverseDynamics
+{
+// constants for random mass and inertia generation
+// these are arbitrary positive values.
+static const float mass_min = 0.001;
+static const float mass_max = 1.0;
+
+void randomInit() { srand(time(NULL)); }
+void randomInit(unsigned seed) { srand(seed); }
+
+int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; }
+
+float randomFloat(float low, float high)
+{
+	return low + static_cast<float>(rand()) / RAND_MAX * (high - low);
+}
+
+float randomMass() { return randomFloat(mass_min, mass_max); }
+
+vec3 randomInertiaPrincipal()
+{
+	vec3 inertia;
+	do
+	{
+		inertia(0) = randomFloat(mass_min, mass_max);
+		inertia(1) = randomFloat(mass_min, mass_max);
+		inertia(2) = randomFloat(mass_min, mass_max);
+	} while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) ||
+			 inertia(1) + inertia(2) < inertia(0));
+	return inertia;
+}
+
+mat33 randomInertiaMatrix()
+{
+	// generate random valid inertia matrix by first getting valid components
+	// along major axes and then rotating by random amount
+	vec3 principal = randomInertiaPrincipal();
+	mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) *
+			  transformZ(randomFloat(-BT_ID_PI, BT_ID_PI)));
+	mat33 inertia;
+	inertia(0, 0) = principal(0);
+	inertia(0, 1) = 0;
+	inertia(0, 2) = 0;
+	inertia(1, 0) = 0;
+	inertia(1, 1) = principal(1);
+	inertia(1, 2) = 0;
+	inertia(2, 0) = 0;
+	inertia(2, 1) = 0;
+	inertia(2, 2) = principal(2);
+	return rot * inertia * rot.transpose();
+}
+
+vec3 randomAxis()
+{
+	vec3 axis;
+	idScalar length;
+	do
+	{
+		axis(0) = randomFloat(-1.0, 1.0);
+		axis(1) = randomFloat(-1.0, 1.0);
+		axis(2) = randomFloat(-1.0, 1.0);
+
+		length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
+	} while (length < 0.01);
+
+	return axis / length;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/IDRandomUtil.hpp

@@ -0,0 +1,37 @@
+#ifndef ID_RANDOM_UTIL_HPP_
+#define ID_RANDOM_UTIL_HPP_
+#include "BulletInverseDynamics/IDConfig.hpp"
+namespace btInverseDynamics
+{
+/// seed random number generator using time()
+void randomInit();
+/// seed random number generator with identical value to get repeatable results
+void randomInit(unsigned seed);
+/// Generate (not quite) uniformly distributed random integers in [low, high]
+/// Note: this is a low-quality implementation using only rand(), as
+/// C++11 <random> is not supported in bullet.
+/// The results will *not* be perfectly uniform.
+/// \param low is the lower bound (inclusive)
+/// \param high is the lower bound (inclusive)
+/// \return a random number within [\param low, \param high]
+int randomInt(int low, int high);
+/// Generate a (not quite) uniformly distributed random floats in [low, high]
+/// Note: this is a low-quality implementation using only rand(), as
+/// C++11 <random> is not supported in bullet.
+/// The results will *not* be perfectly uniform.
+/// \param low is the lower bound (inclusive)
+/// \param high is the lower bound (inclusive)
+/// \return a random number within [\param low, \param high]
+float randomFloat(float low, float high);
+
+/// generate a random valid mass value
+/// \returns random mass
+float randomMass();
+/// generate a random valid vector of principal moments of inertia
+vec3 randomInertiaPrincipal();
+/// generate a random valid moment of inertia matrix
+mat33 randomInertiaMatrix();
+/// generate a random unit vector
+vec3 randomAxis();
+}  // namespace btInverseDynamics
+#endif

Engine/lib/bullet/Extras/InverseDynamics/LICENSE.txt

@@ -0,0 +1,12 @@
+Bullet Continuous Collision Detection and Physics Library
+http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyNameMap.cpp

@@ -0,0 +1,92 @@
+#include "MultiBodyNameMap.hpp"
+
+namespace btInverseDynamics
+{
+MultiBodyNameMap::MultiBodyNameMap() {}
+
+int MultiBodyNameMap::addBody(const int index, const std::string& name)
+{
+	if (m_index_to_body_name.count(index) > 0)
+	{
+		bt_id_error_message("trying to add index %d again\n", index);
+		return -1;
+	}
+	if (m_body_name_to_index.count(name) > 0)
+	{
+		bt_id_error_message("trying to add name %s again\n", name.c_str());
+		return -1;
+	}
+
+	m_index_to_body_name[index] = name;
+	m_body_name_to_index[name] = index;
+
+	return 0;
+}
+
+int MultiBodyNameMap::addJoint(const int index, const std::string& name)
+{
+	if (m_index_to_joint_name.count(index) > 0)
+	{
+		bt_id_error_message("trying to add index %d again\n", index);
+		return -1;
+	}
+	if (m_joint_name_to_index.count(name) > 0)
+	{
+		bt_id_error_message("trying to add name %s again\n", name.c_str());
+		return -1;
+	}
+
+	m_index_to_joint_name[index] = name;
+	m_joint_name_to_index[name] = index;
+
+	return 0;
+}
+
+int MultiBodyNameMap::getBodyName(const int index, std::string* name) const
+{
+	std::map<int, std::string>::const_iterator it = m_index_to_body_name.find(index);
+	if (it == m_index_to_body_name.end())
+	{
+		bt_id_error_message("index %d not known\n", index);
+		return -1;
+	}
+	*name = it->second;
+	return 0;
+}
+
+int MultiBodyNameMap::getJointName(const int index, std::string* name) const
+{
+	std::map<int, std::string>::const_iterator it = m_index_to_joint_name.find(index);
+	if (it == m_index_to_joint_name.end())
+	{
+		bt_id_error_message("index %d not known\n", index);
+		return -1;
+	}
+	*name = it->second;
+	return 0;
+}
+
+int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const
+{
+	std::map<std::string, int>::const_iterator it = m_body_name_to_index.find(name);
+	if (it == m_body_name_to_index.end())
+	{
+		bt_id_error_message("name %s not known\n", name.c_str());
+		return -1;
+	}
+	*index = it->second;
+	return 0;
+}
+
+int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const
+{
+	std::map<std::string, int>::const_iterator it = m_joint_name_to_index.find(name);
+	if (it == m_joint_name_to_index.end())
+	{
+		bt_id_error_message("name %s not known\n", name.c_str());
+		return -1;
+	}
+	*index = it->second;
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyNameMap.hpp

@@ -0,0 +1,55 @@
+#ifndef MULTIBODYNAMEMAP_HPP_
+#define MULTIBODYNAMEMAP_HPP_
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include <string>
+#include <map>
+
+namespace btInverseDynamics
+{
+/// \brief The MultiBodyNameMap class
+/// Utility class that stores a maps from body/joint indices to/from body and joint names
+class MultiBodyNameMap
+{
+public:
+	MultiBodyNameMap();
+	/// add a body to the map
+	/// @param index of the body
+	/// @param name name of the body
+	/// @return 0 on success, -1 on failure
+	int addBody(const int index, const std::string& name);
+	/// add a joint to the map
+	/// @param index of the joint
+	/// @param name name of the joint
+	/// @return 0 on success, -1 on failure
+	int addJoint(const int index, const std::string& name);
+	/// get body name from index
+	/// @param index of the body
+	/// @param body_name name of the body
+	/// @return 0 on success, -1 on failure
+	int getBodyName(const int index, std::string* name) const;
+	/// get joint name from index
+	/// @param index of the joint
+	/// @param joint_name name of the joint
+	/// @return 0 on success, -1 on failure
+	int getJointName(const int index, std::string* name) const;
+	/// get body index from name
+	/// @param index of the body
+	/// @param name name of the body
+	/// @return 0 on success, -1 on failure
+	int getBodyIndex(const std::string& name, int* index) const;
+	/// get joint index from name
+	/// @param index of the joint
+	/// @param name name of the joint
+	/// @return 0 on success, -1 on failure
+	int getJointIndex(const std::string& name, int* index) const;
+
+private:
+	std::map<int, std::string> m_index_to_joint_name;
+	std::map<int, std::string> m_index_to_body_name;
+
+	std::map<std::string, int> m_joint_name_to_index;
+	std::map<std::string, int> m_body_name_to_index;
+};
+}  // namespace btInverseDynamics
+#endif  // MULTIBODYNAMEMAP_HPP_

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyTreeCreator.cpp

@@ -0,0 +1,71 @@
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator)
+{
+	int num_bodies;
+	int parent_index;
+	JointType joint_type;
+	vec3 body_r_parent_body_ref;
+	mat33 body_R_parent_ref;
+	vec3 body_axis_of_motion;
+	idScalar mass;
+	vec3 body_r_body_com;
+	mat33 body_I_body;
+	int user_int;
+	void* user_ptr;
+
+	MultiBodyTree* tree = new MultiBodyTree();
+	if (0x0 == tree)
+	{
+		bt_id_error_message("cannot allocate tree\n");
+		return 0x0;
+	}
+
+	// TODO: move to some policy argument
+	tree->setAcceptInvalidMassParameters(false);
+
+	// get number of bodies in the system
+	if (-1 == creator.getNumBodies(&num_bodies))
+	{
+		bt_id_error_message("getting body indices\n");
+		delete tree;
+		return 0x0;
+	}
+
+	// get data for all bodies
+	for (int index = 0; index < num_bodies; index++)
+	{
+		// get body parameters from user callbacks
+		if (-1 ==
+			creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
+							&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
+							&body_I_body, &user_int, &user_ptr))
+		{
+			bt_id_error_message("getting data for body %d\n", index);
+			delete tree;
+			return 0x0;
+		}
+		// add body to system
+		if (-1 ==
+			tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
+						  body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
+						  body_I_body, user_int, user_ptr))
+		{
+			bt_id_error_message("adding body %d\n", index);
+			delete tree;
+			return 0x0;
+		}
+	}
+	// finalize initialization
+	if (-1 == tree->finalize())
+	{
+		bt_id_error_message("building system\n");
+		delete tree;
+		return 0x0;
+	}
+
+	return tree;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyTreeCreator.hpp

@@ -0,0 +1,47 @@
+#ifndef MULTI_BODY_TREE_CREATOR_HPP_
+#define MULTI_BODY_TREE_CREATOR_HPP_
+
+#include <string>
+#include <vector>
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "BulletInverseDynamics/IDMath.hpp"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+#include "MultiBodyNameMap.hpp"
+
+namespace btInverseDynamics
+{
+/// Interface class for initializing a MultiBodyTree instance.
+/// Data to be provided is modeled on the URDF specification.
+/// The user can derive from this class in order to programmatically
+/// initialize a system.
+class MultiBodyTreeCreator
+{
+public:
+	/// the dtor
+	virtual ~MultiBodyTreeCreator() {}
+	/// Get the number of bodies in the system
+	/// @param num_bodies write number of bodies here
+	/// @return 0 on success, -1 on error
+	virtual int getNumBodies(int* num_bodies) const = 0;
+	/// Interface for accessing link mass properties.
+	/// For detailed description of data, @sa MultiBodyTree::addBody
+	/// \copydoc MultiBodyTree::addBody
+	virtual int getBody(const int body_index, int* parent_index, JointType* joint_type,
+						vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+						vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+						mat33* body_I_body, int* user_int, void** user_ptr) const = 0;
+	/// @return a pointer to a name mapping utility class, or 0x0 if not available
+	virtual const MultiBodyNameMap* getNameMap() const { return 0x0; }
+};
+
+/// Create a multibody object.
+/// @param creator an object implementing the MultiBodyTreeCreator interface
+///        that returns data defining the system
+/// @return A pointer to an allocated multibodytree instance, or
+///         0x0 if an error occured.
+MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator);
+}  // namespace btInverseDynamics
+
+// does urdf have gravity direction ??
+
+#endif  // MULTI_BODY_TREE_CREATOR_HPP_

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp

@@ -0,0 +1,77 @@
+#include "MultiBodyTreeDebugGraph.hpp"
+
+#include <cstdio>
+
+namespace btInverseDynamics
+{
+int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
+						 const char* filename)
+{
+	if (0x0 == tree)
+	{
+		bt_id_error_message("tree pointer is null\n");
+		return -1;
+	}
+	if (0x0 == filename)
+	{
+		bt_id_error_message("filename is null\n");
+		return -1;
+	}
+
+	FILE* fp = fopen(filename, "w");
+	if (NULL == fp)
+	{
+		bt_id_error_message("cannot open file %s for writing\n", filename);
+		return -1;
+	}
+	fprintf(fp,
+			"// to generate postscript file, run dot -Tps %s -o %s.ps\n"
+			"// details see graphviz documentation at http://graphviz.org\n"
+			"digraph tree {\n",
+			filename, filename);
+
+	for (int body = 0; body < tree->numBodies(); body++)
+	{
+		std::string name;
+		if (0x0 != map)
+		{
+			if (-1 == map->getBodyName(body, &name))
+			{
+				bt_id_error_message("can't get name of body %d\n", body);
+				return -1;
+			}
+			fprintf(fp, "              %d [label=\"%d/%s\"];\n", body, body, name.c_str());
+		}
+	}
+	for (int body = 0; body < tree->numBodies(); body++)
+	{
+		int parent;
+		const char* joint_type;
+		int qi;
+		if (-1 == tree->getParentIndex(body, &parent))
+		{
+			bt_id_error_message("indexing error\n");
+			return -1;
+		}
+		if (-1 == tree->getJointTypeStr(body, &joint_type))
+		{
+			bt_id_error_message("indexing error\n");
+			return -1;
+		}
+		if (-1 == tree->getDoFOffset(body, &qi))
+		{
+			bt_id_error_message("indexing error\n");
+			return -1;
+		}
+		if (-1 != parent)
+		{
+			fprintf(fp, "              %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
+					joint_type, qi);
+		}
+	}
+
+	fprintf(fp, "}\n");
+	fclose(fp);
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp

@@ -0,0 +1,18 @@
+#ifndef MULTIBODYTREEDEBUGGRAPH_HPP_
+#define MULTIBODYTREEDEBUGGRAPH_HPP_
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+#include "MultiBodyNameMap.hpp"
+
+namespace btInverseDynamics
+{
+/// generate a dot-file of the multibody tree for generating a graph using graphviz' dot tool
+/// @param tree the multibody tree
+/// @param map to add names of links (if 0x0, no names will be added)
+/// @param filename name for the output file
+/// @return 0 on success, -1 on error
+int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
+						 const char* filename);
+}  // namespace btInverseDynamics
+
+#endif  // MULTIBODYTREEDEBUGGRAPH_HPP

Engine/lib/bullet/Extras/InverseDynamics/RandomTreeCreator.cpp

@@ -0,0 +1,91 @@
+#include "RandomTreeCreator.hpp"
+
+#include <cstdio>
+
+#include "IDRandomUtil.hpp"
+
+namespace btInverseDynamics
+{
+RandomTreeCreator::RandomTreeCreator(const int max_bodies, bool random_seed)
+{
+	// seed generator
+	if (random_seed)
+	{
+		randomInit();  // seeds with time()
+	}
+	else
+	{
+		randomInit(1);  // seeds with 1
+	}
+	m_num_bodies = randomInt(1, max_bodies);
+}
+
+RandomTreeCreator::~RandomTreeCreator() {}
+
+int RandomTreeCreator::getNumBodies(int* num_bodies) const
+{
+	*num_bodies = m_num_bodies;
+	return 0;
+}
+
+int RandomTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+							   vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+							   vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+							   mat33* body_I_body, int* user_int, void** user_ptr) const
+{
+	if (0 == body_index)
+	{  //root body
+		*parent_index = -1;
+	}
+	else
+	{
+		*parent_index = randomInt(0, body_index - 1);
+	}
+
+	switch (randomInt(0, 3))
+	{
+		case 0:
+			*joint_type = FIXED;
+			break;
+		case 1:
+			*joint_type = REVOLUTE;
+			break;
+		case 2:
+			*joint_type = PRISMATIC;
+			break;
+		case 3:
+			*joint_type = FLOATING;
+			break;
+		default:
+			bt_id_error_message("randomInt() result out of range\n");
+			return -1;
+	}
+
+	(*parent_r_parent_body_ref)(0) = randomFloat(-1.0, 1.0);
+	(*parent_r_parent_body_ref)(1) = randomFloat(-1.0, 1.0);
+	(*parent_r_parent_body_ref)(2) = randomFloat(-1.0, 1.0);
+
+	bodyTParentFromAxisAngle(randomAxis(), randomFloat(-BT_ID_PI, BT_ID_PI), body_T_parent_ref);
+
+	*body_axis_of_motion = randomAxis();
+	*mass = randomMass();
+	(*body_r_body_com)(0) = randomFloat(-1.0, 1.0);
+	(*body_r_body_com)(1) = randomFloat(-1.0, 1.0);
+	(*body_r_body_com)(2) = randomFloat(-1.0, 1.0);
+	const double a = randomFloat(-BT_ID_PI, BT_ID_PI);
+	const double b = randomFloat(-BT_ID_PI, BT_ID_PI);
+	const double c = randomFloat(-BT_ID_PI, BT_ID_PI);
+	vec3 ii = randomInertiaPrincipal();
+	mat33 ii_diag;
+	setZero(ii_diag);
+	ii_diag(0, 0) = ii(0);
+	ii_diag(1, 1) = ii(1);
+	ii_diag(2, 2) = ii(2);
+	*body_I_body = transformX(a) * transformY(b) * transformZ(c) * ii_diag *
+				   transformZ(-c) * transformY(-b) * transformX(-a);
+	*user_int = 0;
+	*user_ptr = 0;
+
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/RandomTreeCreator.hpp

@@ -0,0 +1,33 @@
+#ifndef RANDOMTREE_CREATOR_HPP_
+#define RANDOMTREE_CREATOR_HPP_
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+/// Generate a random MultiBodyTree with fixed or floating base and fixed, prismatic or revolute
+/// joints
+/// Uses a pseudo random number generator seeded from a random device.
+class RandomTreeCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor
+	/// @param max_bodies maximum number of bodies
+	/// @param gravity gravitational acceleration
+	/// @param use_seed if true, seed random number generator
+	RandomTreeCreator(const int max_bodies, bool use_seed = false);
+	~RandomTreeCreator();
+	///\copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int* num_bodies) const;
+	///\copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int* parent_index, JointType* joint_type,
+				vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+				idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+				void** user_ptr) const;
+
+private:
+	int m_num_bodies;
+};
+}  // namespace btInverseDynamics
+#endif  // RANDOMTREE_CREATOR_HPP_

Engine/lib/bullet/Extras/InverseDynamics/SimpleTreeCreator.cpp

@@ -0,0 +1,77 @@
+#include "SimpleTreeCreator.hpp"
+
+#include <cstdio>
+
+namespace btInverseDynamics
+{
+/// minimal "tree" (chain)
+SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim)
+{
+	m_mass = 1.0;
+	m_body_T_parent_ref(0, 0) = 1;
+	m_body_T_parent_ref(0, 1) = 0;
+	m_body_T_parent_ref(0, 2) = 0;
+	m_body_T_parent_ref(1, 0) = 0;
+	m_body_T_parent_ref(1, 1) = 1;
+	m_body_T_parent_ref(1, 2) = 0;
+	m_body_T_parent_ref(2, 0) = 0;
+	m_body_T_parent_ref(2, 1) = 0;
+	m_body_T_parent_ref(2, 2) = 1;
+
+	m_parent_r_parent_body_ref(0) = 1.0;
+	m_parent_r_parent_body_ref(1) = 0.0;
+	m_parent_r_parent_body_ref(2) = 0.0;
+
+	m_body_r_body_com(0) = 0.5;
+	m_body_r_body_com(1) = 0.0;
+	m_body_r_body_com(2) = 0.0;
+
+	m_body_I_body(0, 0) = 1;
+	m_body_I_body(0, 1) = 0;
+	m_body_I_body(0, 2) = 0;
+	m_body_I_body(1, 0) = 0;
+	m_body_I_body(1, 1) = 1;
+	m_body_I_body(1, 2) = 0;
+	m_body_I_body(2, 0) = 0;
+	m_body_I_body(2, 1) = 0;
+	m_body_I_body(2, 2) = 1;
+
+	m_axis(0) = 0;
+	m_axis(1) = 0;
+	m_axis(2) = 1;
+}
+int SimpleTreeCreator::getNumBodies(int* num_bodies) const
+{
+	*num_bodies = m_num_bodies;
+	return 0;
+}
+
+int SimpleTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
+							   vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
+							   vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
+							   mat33* body_I_body, int* user_int, void** user_ptr) const
+{
+	*parent_index = body_index - 1;
+	if (body_index % 2)
+	{
+		*joint_type = PRISMATIC;
+	}
+	else
+	{
+		*joint_type = REVOLUTE;
+	}
+	*parent_r_parent_body_ref = m_parent_r_parent_body_ref;
+	if (0 == body_index)
+	{
+		(*parent_r_parent_body_ref)(2) = 1.0;
+	}
+	*body_T_parent_ref = m_body_T_parent_ref;
+	*body_axis_of_motion = m_axis;
+	*mass = m_mass;
+	*body_r_body_com = m_body_r_body_com;
+	*body_I_body = m_body_I_body;
+	*user_int = 0;
+	*user_ptr = 0;
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/SimpleTreeCreator.hpp

@@ -0,0 +1,35 @@
+#ifndef SIMPLETREECREATOR_HPP_
+#define SIMPLETREECREATOR_HPP_
+
+#include "MultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+/// minimal "tree" (chain)
+class SimpleTreeCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor
+	/// @param dim number of bodies
+	SimpleTreeCreator(int dim);
+	// dtor
+	~SimpleTreeCreator() {}
+	///\copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int* num_bodies) const;
+	///\copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int* parent_index, JointType* joint_type,
+				vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion,
+				idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int,
+				void** user_ptr) const;
+
+private:
+	int m_num_bodies;
+	idScalar m_mass;
+	mat33 m_body_T_parent_ref;
+	vec3 m_parent_r_parent_body_ref;
+	vec3 m_body_r_body_com;
+	mat33 m_body_I_body;
+	vec3 m_axis;
+};
+}  // namespace btInverseDynamics
+#endif  // SIMPLETREECREATOR_HPP_

Engine/lib/bullet/Extras/InverseDynamics/User2InternalIndex.cpp

@@ -0,0 +1,121 @@
+#include "User2InternalIndex.hpp"
+
+namespace btInverseDynamics
+{
+User2InternalIndex::User2InternalIndex() : m_map_built(false) {}
+
+void User2InternalIndex::addBody(const int body, const int parent)
+{
+	m_user_parent_index_map[body] = parent;
+}
+
+int User2InternalIndex::findRoot(int index)
+{
+	if (0 == m_user_parent_index_map.count(index))
+	{
+		return index;
+	}
+	return findRoot(m_user_parent_index_map[index]);
+}
+
+// modelled after URDF2Bullet.cpp:void ComputeParentIndices(const
+// URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex,
+// int urdfParentIndex)
+void User2InternalIndex::recurseIndexSets(const int user_body_index)
+{
+	m_user_to_internal[user_body_index] = m_current_index;
+	m_current_index++;
+	for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++)
+	{
+		recurseIndexSets(m_user_child_indices[user_body_index][i]);
+	}
+}
+
+int User2InternalIndex::buildMapping()
+{
+	// find root index
+	int user_root_index = -1;
+	for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
+		 it != m_user_parent_index_map.end(); it++)
+	{
+		int current_root_index = findRoot(it->second);
+		if (it == m_user_parent_index_map.begin())
+		{
+			user_root_index = current_root_index;
+		}
+		else
+		{
+			if (user_root_index != current_root_index)
+			{
+				bt_id_error_message("multiple roots (at least) %d and %d\n", user_root_index,
+									current_root_index);
+				return -1;
+			}
+		}
+	}
+
+	// build child index map
+	for (std::map<int, int>::iterator it = m_user_parent_index_map.begin();
+		 it != m_user_parent_index_map.end(); it++)
+	{
+		m_user_child_indices[it->second].push_back(it->first);
+	}
+
+	m_current_index = -1;
+	// build internal index set
+	m_user_to_internal[user_root_index] = -1;  // add map for root link
+	recurseIndexSets(user_root_index);
+
+	// reverse mapping
+	for (std::map<int, int>::iterator it = m_user_to_internal.begin();
+		 it != m_user_to_internal.end(); it++)
+	{
+		m_internal_to_user[it->second] = it->first;
+	}
+
+	m_map_built = true;
+	return 0;
+}
+
+int User2InternalIndex::user2internal(const int user, int *internal) const
+{
+	if (!m_map_built)
+	{
+		return -1;
+	}
+
+	std::map<int, int>::const_iterator it;
+	it = m_user_to_internal.find(user);
+	if (it != m_user_to_internal.end())
+	{
+		*internal = it->second;
+		return 0;
+	}
+	else
+	{
+		bt_id_error_message("no user index %d\n", user);
+		return -1;
+	}
+}
+
+int User2InternalIndex::internal2user(const int internal, int *user) const
+{
+	if (!m_map_built)
+	{
+		return -1;
+	}
+
+	std::map<int, int>::const_iterator it;
+	it = m_internal_to_user.find(internal);
+	if (it != m_internal_to_user.end())
+	{
+		*user = it->second;
+		return 0;
+	}
+	else
+	{
+		bt_id_error_message("no internal index %d\n", internal);
+		return -1;
+	}
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/User2InternalIndex.hpp

@@ -0,0 +1,47 @@
+#ifndef USER2INTERNALINDEX_HPP
+#define USER2INTERNALINDEX_HPP
+#include <map>
+#include <vector>
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+
+namespace btInverseDynamics
+{
+/// Convert arbitrary indexing scheme to internal indexing
+/// used for MultiBodyTree
+class User2InternalIndex
+{
+public:
+	/// Ctor
+	User2InternalIndex();
+	/// add body to index maps
+	/// @param body index of body to add (external)
+	/// @param parent index of parent body (external)
+	void addBody(const int body, const int parent);
+	/// build mapping from external to internal indexing
+	/// @return 0 on success, -1 on failure
+	int buildMapping();
+	/// get internal index from external index
+	/// @param user external (user) index
+	/// @param internal pointer for storage of corresponding internal index
+	/// @return 0 on success, -1 on failure
+	int user2internal(const int user, int *internal) const;
+	/// get internal index from external index
+	/// @param user external (user) index
+	/// @param internal pointer for storage of corresponding internal index
+	/// @return 0 on success, -1 on failure
+	int internal2user(const int internal, int *user) const;
+
+private:
+	int findRoot(int index);
+	void recurseIndexSets(const int user_body_index);
+	bool m_map_built;
+	std::map<int, int> m_user_parent_index_map;
+	std::map<int, int> m_user_to_internal;
+	std::map<int, int> m_internal_to_user;
+	std::map<int, std::vector<int> > m_user_child_indices;
+	int m_current_index;
+};
+}  // namespace btInverseDynamics
+
+#endif  // USER2INTERNALINDEX_HPP

Engine/lib/bullet/Extras/InverseDynamics/btMultiBodyFromURDF.hpp

@@ -0,0 +1,101 @@
+#ifndef BTMULTIBODYFROMURDF_HPP
+#define BTMULTIBODYFROMURDF_HPP
+
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../../examples/Utils/b3BulletDefaultFileIO.h"
+/// Create a btMultiBody model from URDF.
+/// This is adapted from Bullet URDF loader example
+class MyBtMultiBodyFromURDF
+{
+public:
+	/// ctor
+	/// @param gravity gravitational acceleration (in world frame)
+	/// @param base_fixed if true, the root body is treated as fixed,
+	///        if false, it is treated as floating
+	MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed)
+		: m_gravity(gravity), m_base_fixed(base_fixed)
+	{
+		m_broadphase = 0x0;
+		m_dispatcher = 0x0;
+		m_solver = 0x0;
+		m_collisionConfiguration = 0x0;
+		m_dynamicsWorld = 0x0;
+		m_multibody = 0x0;
+                m_flag = 0x0;
+	}
+	/// dtor
+	~MyBtMultiBodyFromURDF()
+	{
+		delete m_dynamicsWorld;
+		delete m_solver;
+		delete m_broadphase;
+		delete m_dispatcher;
+		delete m_collisionConfiguration;
+		delete m_multibody;
+	}
+	/// @param name path to urdf file
+	void setFileName(const std::string name) { m_filename = name; }
+        void setFlag(int flag) { m_flag = flag; }
+	/// load urdf file and build btMultiBody model
+	void init()
+	{
+		this->createEmptyDynamicsWorld();
+		m_dynamicsWorld->setGravity(m_gravity);
+		b3BulletDefaultFileIO fileIO;
+		BulletURDFImporter urdf_importer(&m_nogfx, 0, &fileIO, 1, 0);
+		URDFImporterInterface &u2b(urdf_importer);
+		bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed);
+
+		if (loadOk)
+		{
+			btTransform identityTrans;
+			identityTrans.setIdentity();
+			MyMultiBodyCreator creation(&m_nogfx);
+			const bool use_multibody = true;
+			ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody,
+							   u2b.getPathPrefix(), m_flag);
+			m_multibody = creation.getBulletMultiBody();
+			m_dynamicsWorld->stepSimulation(1. / 240., 0);
+		}
+	}
+	/// @return pointer to the btMultiBody model
+	btMultiBody *getBtMultiBody() { return m_multibody; }
+
+private:
+	// internal utility function
+	void createEmptyDynamicsWorld()
+	{
+		m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+		/// use the default collision dispatcher. For parallel processing you can use a diffent
+		/// dispatcher (see Extras/BulletMultiThreaded)
+		m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+		m_broadphase = new btDbvtBroadphase();
+		m_solver = new btMultiBodyConstraintSolver;
+		m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver,
+													   m_collisionConfiguration);
+		m_dynamicsWorld->setGravity(m_gravity);
+	}
+
+	btBroadphaseInterface *m_broadphase;
+	btCollisionDispatcher *m_dispatcher;
+	btMultiBodyConstraintSolver *m_solver;
+	btDefaultCollisionConfiguration *m_collisionConfiguration;
+	btMultiBodyDynamicsWorld *m_dynamicsWorld;
+	std::string m_filename;
+	DummyGUIHelper m_nogfx;
+	btMultiBody *m_multibody;
+	const btVector3 m_gravity;
+	const bool m_base_fixed;
+        int m_flag;
+};
+#endif  // BTMULTIBODYFROMURDF_HPP

Engine/lib/bullet/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp

@@ -0,0 +1,306 @@
+#include "btMultiBodyTreeCreator.hpp"
+
+namespace btInverseDynamics
+{
+btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {}
+
+int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose)
+{
+	if (0x0 == btmb)
+	{
+		bt_id_error_message("cannot create MultiBodyTree from null pointer\n");
+		return -1;
+	}
+
+	// in case this is a second call, discard old data
+	m_data.clear();
+	m_initialized = false;
+
+	// btMultiBody treats base link separately
+	m_data.resize(1 + btmb->getNumLinks());
+
+	// add base link data
+	{
+		LinkData &link = m_data[0];
+
+		link.parent_index = -1;
+		if (btmb->hasFixedBase())
+		{
+			link.joint_type = FIXED;
+		}
+		else
+		{
+			link.joint_type = FLOATING;
+		}
+		btTransform transform = (btmb->getBaseWorldTransform());
+		//compute inverse dynamics in body-fixed frame
+		transform.setIdentity();
+
+		link.parent_r_parent_body_ref(0) = transform.getOrigin()[0];
+		link.parent_r_parent_body_ref(1) = transform.getOrigin()[1];
+		link.parent_r_parent_body_ref(2) = transform.getOrigin()[2];
+
+		link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0];
+		link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1];
+		link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2];
+
+		link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0];
+		link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1];
+		link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2];
+
+		link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0];
+		link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1];
+		link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2];
+
+		// random unit vector. value not used for fixed or floating joints.
+		link.body_axis_of_motion(0) = 0;
+		link.body_axis_of_motion(1) = 0;
+		link.body_axis_of_motion(2) = 1;
+
+		link.mass = btmb->getBaseMass();
+		// link frame in the center of mass
+		link.body_r_body_com(0) = 0;
+		link.body_r_body_com(1) = 0;
+		link.body_r_body_com(2) = 0;
+		// BulletDynamics uses body-fixed frame in the cog, aligned with principal axes
+		link.body_I_body(0, 0) = btmb->getBaseInertia()[0];
+		link.body_I_body(0, 1) = 0.0;
+		link.body_I_body(0, 2) = 0.0;
+		link.body_I_body(1, 0) = 0.0;
+		link.body_I_body(1, 1) = btmb->getBaseInertia()[1];
+		link.body_I_body(1, 2) = 0.0;
+		link.body_I_body(2, 0) = 0.0;
+		link.body_I_body(2, 1) = 0.0;
+		link.body_I_body(2, 2) = btmb->getBaseInertia()[2];
+		// shift reference point to link origin (in joint axis)
+		mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
+		link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
+		if (verbose)
+		{
+			id_printf(
+				"base: mass= %f, bt_inertia= [%f %f %f]\n"
+				"Io= [%f %f %f;\n"
+				"    %f %f %f;\n"
+				"    %f %f %f]\n",
+				link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1],
+				btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
+				link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
+				link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
+				link.body_I_body(2, 2));
+		}
+	}
+
+	for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++)
+	{
+		if (verbose)
+		{
+			id_printf("bt->id: converting link %d\n", bt_index);
+		}
+		const btMultibodyLink &bt_link = btmb->getLink(bt_index);
+		LinkData &link = m_data[bt_index + 1];
+
+		link.parent_index = bt_link.m_parent + 1;
+
+		link.mass = bt_link.m_mass;
+		if (verbose)
+		{
+			id_printf("mass= %f\n", link.mass);
+		}
+		// from this body's pivot to this body's com in this body's frame
+		link.body_r_body_com[0] = bt_link.m_dVector[0];
+		link.body_r_body_com[1] = bt_link.m_dVector[1];
+		link.body_r_body_com[2] = bt_link.m_dVector[2];
+		if (verbose)
+		{
+			id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1],
+					  link.body_r_body_com[2]);
+		}
+		// BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes
+		link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0];
+		link.body_I_body(0, 1) = 0.0;
+		link.body_I_body(0, 2) = 0.0;
+		link.body_I_body(1, 0) = 0.0;
+		link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1];
+		link.body_I_body(1, 2) = 0.0;
+		link.body_I_body(2, 0) = 0.0;
+		link.body_I_body(2, 1) = 0.0;
+		link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2];
+		// shift reference point to link origin (in joint axis)
+		mat33 tilde_r_com = tildeOperator(link.body_r_body_com);
+		link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com;
+
+		if (verbose)
+		{
+			id_printf(
+				"link %d: mass= %f, bt_inertia= [%f %f %f]\n"
+				"Io= [%f %f %f;\n"
+				"    %f %f %f;\n"
+				"    %f %f %f]\n",
+				bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1],
+				bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1),
+				link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1),
+				link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1),
+				link.body_I_body(2, 2));
+		}
+		// transform for vectors written in parent frame to this link's body-fixed frame
+		btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis();
+		link.body_T_parent_ref(0, 0) = basis[0][0];
+		link.body_T_parent_ref(0, 1) = basis[0][1];
+		link.body_T_parent_ref(0, 2) = basis[0][2];
+		link.body_T_parent_ref(1, 0) = basis[1][0];
+		link.body_T_parent_ref(1, 1) = basis[1][1];
+		link.body_T_parent_ref(1, 2) = basis[1][2];
+		link.body_T_parent_ref(2, 0) = basis[2][0];
+		link.body_T_parent_ref(2, 1) = basis[2][1];
+		link.body_T_parent_ref(2, 2) = basis[2][2];
+		if (verbose)
+		{
+			id_printf(
+				"body_T_parent_ref= %f %f %f\n"
+				"                   %f %f %f\n"
+				"                   %f %f %f\n",
+				basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2],
+				basis[2][0], basis[2][1], basis[2][2]);
+		}
+		switch (bt_link.m_jointType)
+		{
+			case btMultibodyLink::eRevolute:
+				link.joint_type = REVOLUTE;
+				if (verbose)
+				{
+					id_printf("type= revolute\n");
+				}
+				link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0];
+				link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1];
+				link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2];
+
+				// for revolute joints, m_eVector = parentComToThisPivotOffset
+				//                      m_dVector = thisPivotToThisComOffset
+				// from parent com to pivot, in parent frame
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				break;
+			case btMultibodyLink::ePrismatic:
+				link.joint_type = PRISMATIC;
+				if (verbose)
+				{
+					id_printf("type= prismatic\n");
+				}
+				link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0];
+				link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1];
+				link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2];
+
+				// for prismatic joints, eVector
+				//                                according to documentation :
+				//                                parentComToThisComOffset
+				//                                but seems to be: from parent's com to parent's
+				//                                pivot ??
+				//                       m_dVector = thisPivotToThisComOffset
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				break;
+			case btMultibodyLink::eSpherical:
+				link.joint_type = SPHERICAL;
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				// random unit vector
+				link.body_axis_of_motion(0) = 0;
+				link.body_axis_of_motion(1) = 1;
+				link.body_axis_of_motion(2) = 0;
+				break;
+			case btMultibodyLink::ePlanar:
+				bt_id_error_message("planar joints not implemented\n");
+				return -1;
+			case btMultibodyLink::eFixed:
+				link.joint_type = FIXED;
+				// random unit vector
+				link.body_axis_of_motion(0) = 0;
+				link.body_axis_of_motion(1) = 0;
+				link.body_axis_of_motion(2) = 1;
+
+				// for fixed joints, m_dVector = thisPivotToThisComOffset;
+				//                   m_eVector = parentComToThisPivotOffset;
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				break;
+			default:
+				bt_id_error_message("unknown btMultiBody::eFeatherstoneJointType %d\n",
+									bt_link.m_jointType);
+				return -1;
+		}
+		if (link.parent_index > 0)
+		{  // parent body isn't the root
+			const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1);
+			// from parent pivot to parent com, in parent frame
+			link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0];
+			link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1];
+			link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2];
+		}
+		else
+		{
+			// parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies,
+			// whose link frame is in the CoM (ie, no notion of a pivot point)
+		}
+
+		if (verbose)
+		{
+			id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0],
+					  link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]);
+		}
+	}
+
+	m_initialized = true;
+
+	return 0;
+}
+
+int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const
+{
+	if (false == m_initialized)
+	{
+		bt_id_error_message("btMultiBody not converted yet\n");
+		return -1;
+	}
+
+	*num_bodies = static_cast<int>(m_data.size());
+	return 0;
+}
+
+int btMultiBodyTreeCreator::getBody(const int body_index, int *parent_index, JointType *joint_type,
+									vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
+									vec3 *body_axis_of_motion, idScalar *mass,
+									vec3 *body_r_body_com, mat33 *body_I_body, int *user_int,
+									void **user_ptr) const
+{
+	if (false == m_initialized)
+	{
+		bt_id_error_message("MultiBodyTree not created yet\n");
+		return -1;
+	}
+
+	if (body_index < 0 || body_index >= static_cast<int>(m_data.size()))
+	{
+		bt_id_error_message("index out of range (got %d but only %zu bodies)\n", body_index,
+							m_data.size());
+		return -1;
+	}
+
+	*parent_index = m_data[body_index].parent_index;
+	*joint_type = m_data[body_index].joint_type;
+	*parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref;
+	*body_T_parent_ref = m_data[body_index].body_T_parent_ref;
+	*body_axis_of_motion = m_data[body_index].body_axis_of_motion;
+	*mass = m_data[body_index].mass;
+	*body_r_body_com = m_data[body_index].body_r_body_com;
+	*body_I_body = m_data[body_index].body_I_body;
+
+	*user_int = -1;
+	*user_ptr = 0x0;
+
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp

@@ -0,0 +1,52 @@
+#ifndef BTMULTIBODYTREECREATOR_HPP_
+#define BTMULTIBODYTREECREATOR_HPP_
+
+#include <vector>
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "MultiBodyTreeCreator.hpp"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+
+namespace btInverseDynamics
+{
+/// MultiBodyTreeCreator implementation for converting
+/// a btMultiBody forward dynamics model into a MultiBodyTree inverse dynamics model
+class btMultiBodyTreeCreator : public MultiBodyTreeCreator
+{
+public:
+	/// ctor
+	btMultiBodyTreeCreator();
+	/// dtor
+	~btMultiBodyTreeCreator() {}
+	/// extract model data from a btMultiBody
+	/// @param btmb pointer to btMultiBody to convert
+	/// @param verbose if true, some information is printed
+	/// @return -1 on error, 0 on success
+	int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false);
+	/// \copydoc MultiBodyTreeCreator::getNumBodies
+	int getNumBodies(int *num_bodies) const;
+	///\copydoc MultiBodyTreeCreator::getBody
+	int getBody(const int body_index, int *parent_index, JointType *joint_type,
+				vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref,
+				vec3 *body_axis_of_motion, idScalar *mass, vec3 *body_r_body_com,
+				mat33 *body_I_body, int *user_int, void **user_ptr) const;
+
+private:
+	// internal struct holding data extracted from btMultiBody
+	struct LinkData
+	{
+		int parent_index;
+		JointType joint_type;
+		vec3 parent_r_parent_body_ref;
+		mat33 body_T_parent_ref;
+		vec3 body_axis_of_motion;
+		idScalar mass;
+		vec3 body_r_body_com;
+		mat33 body_I_body;
+	};
+	idArray<LinkData>::type m_data;
+	bool m_initialized;
+};
+}  // namespace btInverseDynamics
+
+#endif  // BTMULTIBODYTREECREATOR_HPP_

Engine/lib/bullet/Extras/InverseDynamics/invdyn_bullet_comparison.cpp

@@ -0,0 +1,361 @@
+#include "invdyn_bullet_comparison.hpp"
+#include <cmath>
+#include "BulletInverseDynamics/IDConfig.hpp"
+#include "BulletInverseDynamics/MultiBodyTree.hpp"
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+
+namespace btInverseDynamics
+{
+int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
+									 btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
+									 double *acc_error)
+{
+// call function and return -1 if it does, printing an bt_id_error_message
+#define RETURN_ON_FAILURE(x)                         \
+	do                                               \
+	{                                                \
+		if (-1 == x)                                 \
+		{                                            \
+			bt_id_error_message("calling " #x "\n"); \
+			return -1;                               \
+		}                                            \
+	} while (0)
+
+	if (verbose)
+	{
+		printf("\n ===================================== \n");
+	}
+	vecx joint_forces(q.size());
+
+	// set positions and velocities for btMultiBody
+	// base link
+	mat33 world_T_base;
+	vec3 world_pos_base;
+	btTransform base_transform;
+	vec3 base_velocity;
+	vec3 base_angular_velocity;
+
+	RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity));
+	RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base));
+	RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base));
+	RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity));
+	RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity));
+
+	base_transform.setBasis(world_T_base);
+	base_transform.setOrigin(world_pos_base);
+	btmb->setBaseWorldTransform(base_transform);
+	btmb->setBaseOmega(base_angular_velocity);
+	btmb->setBaseVel(base_velocity);
+	btmb->setLinearDamping(0);
+	btmb->setAngularDamping(0);
+
+	// remaining links
+	int q_index;
+	if (btmb->hasFixedBase())
+	{
+		q_index = 0;
+	}
+	else
+	{
+		q_index = 6;
+	}
+	if (verbose)
+	{
+		printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs());
+	}
+	for (int l = 0; l < btmb->getNumLinks(); l++)
+	{
+		const btMultibodyLink &link = btmb->getLink(l);
+		if (verbose)
+		{
+			printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount,
+				   link.m_dofCount);
+		}
+		if (link.m_posVarCount == 1)
+		{
+			btmb->setJointPosMultiDof(l, &q(q_index));
+			btmb->setJointVelMultiDof(l, &u(q_index));
+			if (verbose)
+			{
+				printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index));
+			}
+			q_index++;
+		}
+	}
+	// sanity check
+	if (q_index != q.size())
+	{
+		bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
+		return -1;
+	}
+
+	// run inverse dynamics to determine joint_forces for given q, u, dot_u
+	if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces))
+	{
+		bt_id_error_message("calculating inverse dynamics\n");
+		return -1;
+	}
+
+	// set up bullet forward dynamics model
+	btScalar dt = 0;
+	btAlignedObjectArray<btScalar> scratch_r;
+	btAlignedObjectArray<btVector3> scratch_v;
+	btAlignedObjectArray<btMatrix3x3> scratch_m;
+	// this triggers switch between using either appliedConstraintForce or appliedForce
+	bool isConstraintPass = false;
+	// apply gravity forces for btMultiBody model. Must be done manually.
+	btmb->addBaseForce(btmb->getBaseMass() * gravity);
+
+	for (int link = 0; link < btmb->getNumLinks(); link++)
+	{
+		btmb->addLinkForce(link, gravity * btmb->getLinkMass(link));
+		if (verbose)
+		{
+			printf("link %d, applying gravity %f %f %f\n", link,
+				   gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link),
+				   gravity[2] * btmb->getLinkMass(link));
+		}
+	}
+
+	// apply generalized forces
+	if (btmb->hasFixedBase())
+	{
+		q_index = 0;
+	}
+	else
+	{
+		vec3 base_force;
+		base_force(0) = joint_forces(3);
+		base_force(1) = joint_forces(4);
+		base_force(2) = joint_forces(5);
+
+		vec3 base_moment;
+		base_moment(0) = joint_forces(0);
+		base_moment(1) = joint_forces(1);
+		base_moment(2) = joint_forces(2);
+
+		btmb->addBaseForce(world_T_base * base_force);
+		btmb->addBaseTorque(world_T_base * base_moment);
+		if (verbose)
+		{
+			printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4),
+				   joint_forces(5));
+			printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1),
+				   joint_forces(2));
+		}
+		q_index = 6;
+	}
+
+	for (int l = 0; l < btmb->getNumLinks(); l++)
+	{
+		const btMultibodyLink &link = btmb->getLink(l);
+		if (link.m_posVarCount == 1)
+		{
+			if (verbose)
+			{
+				printf("id:joint_force[%d]= %f, applied to link %d\n", q_index,
+					   joint_forces(q_index), l);
+			}
+			btmb->addJointTorque(l, joint_forces(q_index));
+			q_index++;
+		}
+	}
+
+	// sanity check
+	if (q_index != q.size())
+	{
+		bt_id_error_message("error in number of dofs for btMultibody and MultiBodyTree\n");
+		return -1;
+	}
+
+	// run forward kinematics & forward dynamics
+	btAlignedObjectArray<btQuaternion> world_to_local;
+	btAlignedObjectArray<btVector3> local_origin;
+	btmb->forwardKinematics(world_to_local, local_origin);
+	btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+
+	// read generalized accelerations back from btMultiBody
+	// the mapping from scratch variables to accelerations is taken from the implementation
+	// of stepVelocitiesMultiDof
+	btScalar *base_accel = &scratch_r[btmb->getNumDofs()];
+	btScalar *joint_accel = base_accel + 6;
+	*acc_error = 0;
+	int dot_u_offset = 0;
+	if (btmb->hasFixedBase())
+	{
+		dot_u_offset = 0;
+	}
+	else
+	{
+		dot_u_offset = 6;
+	}
+
+	if (true == btmb->hasFixedBase())
+	{
+		for (int i = 0; i < btmb->getNumDofs(); i++)
+		{
+			if (verbose)
+			{
+				printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
+					   dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
+			}
+			*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + dot_u_offset), 2);
+		}
+	}
+	else
+	{
+		vec3 base_dot_omega;
+		vec3 world_dot_omega;
+		world_dot_omega(0) = base_accel[0];
+		world_dot_omega(1) = base_accel[1];
+		world_dot_omega(2) = base_accel[2];
+		base_dot_omega = world_T_base.transpose() * world_dot_omega;
+
+		// com happens to coincide with link origin here. If that changes, we need to calculate
+		// ddot_com
+		vec3 base_ddot_com;
+		vec3 world_ddot_com;
+		world_ddot_com(0) = base_accel[3];
+		world_ddot_com(1) = base_accel[4];
+		world_ddot_com(2) = base_accel[5];
+		base_ddot_com = world_T_base.transpose() * world_ddot_com;
+
+		for (int i = 0; i < 3; i++)
+		{
+			if (verbose)
+			{
+				printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
+					   i, dot_u[i], base_dot_omega(i) - dot_u[i]);
+			}
+			*acc_error += BT_ID_POW(base_dot_omega(i) - dot_u(i), 2);
+		}
+		for (int i = 0; i < 3; i++)
+		{
+			if (verbose)
+			{
+				printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
+					   i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
+			}
+			*acc_error += BT_ID_POW(base_ddot_com(i) - dot_u(i + 3), 2);
+		}
+
+		for (int i = 0; i < btmb->getNumDofs(); i++)
+		{
+			if (verbose)
+			{
+				printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
+					   dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
+			}
+			*acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + 6), 2);
+		}
+	}
+	*acc_error = std::sqrt(*acc_error);
+	if (verbose)
+	{
+		printf("======dynamics-err: %e\n", *acc_error);
+	}
+	*pos_error = 0.0;
+
+	{
+		mat33 world_T_body;
+		if (-1 == id_tree->getBodyTransform(0, &world_T_body))
+		{
+			bt_id_error_message("getting transform for body %d\n", 0);
+			return -1;
+		}
+		vec3 world_com;
+		if (-1 == id_tree->getBodyCoM(0, &world_com))
+		{
+			bt_id_error_message("getting com for body %d\n", 0);
+			return -1;
+		}
+		if (verbose)
+		{
+			printf("id:com:       %f %f %f\n", world_com(0), world_com(1), world_com(2));
+
+			printf(
+				"id:transform: %f %f %f\n"
+				"              %f %f %f\n"
+				"              %f %f %f\n",
+				world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0),
+				world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1),
+				world_T_body(2, 2));
+		}
+	}
+
+	for (int l = 0; l < btmb->getNumLinks(); l++)
+	{
+		const btMultibodyLink &bt_link = btmb->getLink(l);
+
+		vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin();
+		mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis();
+		if (verbose)
+		{
+			printf("------------- link %d\n", l + 1);
+			printf("bt:com:       %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2));
+			printf(
+				"bt:transform: %f %f %f\n"
+				"              %f %f %f\n"
+				"              %f %f %f\n",
+				bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1),
+				bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2));
+		}
+		mat33 id_world_T_body;
+		vec3 id_world_com;
+
+		if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body))
+		{
+			bt_id_error_message("getting transform for body %d\n", l);
+			return -1;
+		}
+		if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com))
+		{
+			bt_id_error_message("getting com for body %d\n", l);
+			return -1;
+		}
+		if (verbose)
+		{
+			printf("id:com:       %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2));
+
+			printf(
+				"id:transform: %f %f %f\n"
+				"              %f %f %f\n"
+				"              %f %f %f\n",
+				id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2),
+				id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2),
+				id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2));
+		}
+		vec3 diff_com = bt_origin - id_world_com;
+		mat33 diff_basis = bt_basis - id_world_T_body;
+		if (verbose)
+		{
+			printf("diff-com:    %e %e %e\n", diff_com(0), diff_com(1), diff_com(2));
+
+			printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0),
+				   diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1),
+				   diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
+		}
+		double total_pos_err =
+			BT_ID_SQRT(BT_ID_POW(diff_com(0), 2) + BT_ID_POW(diff_com(1), 2) +
+					   BT_ID_POW(diff_com(2), 2) + BT_ID_POW(diff_basis(0, 0), 2) +
+					   BT_ID_POW(diff_basis(0, 1), 2) + BT_ID_POW(diff_basis(0, 2), 2) +
+					   BT_ID_POW(diff_basis(1, 0), 2) + BT_ID_POW(diff_basis(1, 1), 2) +
+					   BT_ID_POW(diff_basis(1, 2), 2) + BT_ID_POW(diff_basis(2, 0), 2) +
+					   BT_ID_POW(diff_basis(2, 1), 2) + BT_ID_POW(diff_basis(2, 2), 2));
+		if (verbose)
+		{
+			printf("======kin-pos-err: %e\n", total_pos_err);
+		}
+		if (total_pos_err > *pos_error)
+		{
+			*pos_error = total_pos_err;
+		}
+	}
+
+	return 0;
+}
+}  // namespace btInverseDynamics

Engine/lib/bullet/Extras/InverseDynamics/invdyn_bullet_comparison.hpp

@@ -0,0 +1,36 @@
+#ifndef INVDYN_BULLET_COMPARISON_HPP
+#define INVDYN_BULLET_COMPARISON_HPP
+
+#include "BulletInverseDynamics/IDConfig.hpp"
+
+class btMultiBody;
+class btVector3;
+
+namespace btInverseDynamics
+{
+class MultiBodyTree;
+
+/// this function compares the forward dynamics computations implemented in btMultiBody to
+/// the inverse dynamics implementation in MultiBodyTree. This is done in three steps
+/// 1. run inverse dynamics for (q, u, dot_u) to obtain joint forces f
+/// 2. run forward dynamics (btMultiBody) for (q,u,f) to obtain dot_u_bullet
+/// 3. compare dot_u with dot_u_bullet for cross check of forward and inverse dynamics computations
+/// @param btmb the bullet forward dynamics model
+/// @param id_tree the inverse dynamics model
+/// @param q vector of generalized coordinates (matches id_tree)
+/// @param u vector of generalized speeds (matches id_tree)
+/// @param gravity gravitational acceleration in world frame
+/// @param dot_u vector of generalized accelerations (matches id_tree)
+/// @param gravity gravitational acceleration in world frame
+/// @param base_fixed set base joint to fixed or
+/// @param pos_error is set to the maximum of the euclidean norm of position+rotation errors of all
+///        center of gravity positions and link frames
+/// @param acc_error is set to the square root of the sum of squared differences of generalized
+/// accelerations
+///        computed in step 3 relative to dot_u
+/// @return -1 on error, 0 on success
+int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose,
+									 btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error,
+									 double *acc_error);
+}  // namespace btInverseDynamics
+#endif  // INVDYN_BULLET_COMPARISON_HPP

Engine/lib/bullet/Extras/InverseDynamics/premake4.lua

@@ -0,0 +1,15 @@
+	project "BulletInverseDynamicsUtils"
+
+	kind "StaticLib"
+
+	includedirs {
+		"../../src"
+	}
+
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+	files {
+		"*.cpp",
+		"*.hpp"
+	}