1 files changed, 45 insertions, 27 deletions

diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
index 47b4ab3890..a718db051e 100644
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
@@ -1,12 +1,13 @@
 #include "MultiBodyTreeInitCache.hpp"
 
-namespace btInverseDynamics {
-
-MultiBodyTree::InitCache::InitCache() {
+namespace btInverseDynamics
+{
+MultiBodyTree::InitCache::InitCache()
+{
 	m_inertias.resize(0);
 	m_joints.resize(0);
 	m_num_dofs = 0;
-	m_root_index=-1;
+	m_root_index = -1;
 }
 
 int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
@@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 									  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) {
+									  const int user_int, void* user_ptr)
+{
+	switch (joint_type)
+	{
 		case REVOLUTE:
 		case PRISMATIC:
 			m_num_dofs += 1;
@@ -25,21 +28,26 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			// 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:
-			error_message("unknown joint type %d\n", joint_type);
+			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
 	}
 
-	if(-1 == parent_index) {
-		if(m_root_index>=0) {
-			error_message("trying to add body %d as root, but already added %d as root body\n",
-						  body_index, m_root_index);
+	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;
+		m_root_index = body_index;
 	}
 
 	JointData joint;
@@ -61,9 +69,11 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 	m_user_ptr.push_back(user_ptr);
 	return 0;
 }
-int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const {
-	if (index < 0 || index > static_cast<int>(m_inertias.size())) {
-		error_message("index out of range\n");
+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;
 	}
 
@@ -71,43 +81,51 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert
 	return 0;
 }
 
-int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const {
-	if (index < 0 || index > static_cast<int>(m_user_int.size())) {
-		error_message("index out of range\n");
+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())) {
-		error_message("index out of range\n");
+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())) {
-		error_message("index out of range\n");
+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() {
+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++) {
+	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