add real-time safe Inverse Dynamics library+test+utils

an example for the example browser will follow.
thanks to Thomas Buschmann

Extras/InverseDynamics/User2InternalIndex.hpp

@@ -0,0 +1,46 @@
+#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;
+};
+}
+
+#endif  // USER2INTERNALINDEX_HPP