Vendor thirdparty Bullet source for upcoming physics server backend

1 files changed, 109 insertions, 0 deletions

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