1 #ifndef USER2INTERNALINDEX_HPP 2 #define USER2INTERNALINDEX_HPP 3 #include <map> 4 #include <vector> 5 6 #include "BulletInverseDynamics/IDConfig.hpp" 7 8 namespace btInverseDynamics 9 { 10 /// Convert arbitrary indexing scheme to internal indexing 11 /// used for MultiBodyTree 12 class User2InternalIndex 13 { 14 public: 15 /// Ctor 16 User2InternalIndex(); 17 /// add body to index maps 18 /// @param body index of body to add (external) 19 /// @param parent index of parent body (external) 20 void addBody(const int body, const int parent); 21 /// build mapping from external to internal indexing 22 /// @return 0 on success, -1 on failure 23 int buildMapping(); 24 /// get internal index from external index 25 /// @param user external (user) index 26 /// @param internal pointer for storage of corresponding internal index 27 /// @return 0 on success, -1 on failure 28 int user2internal(const int user, int *internal) const; 29 /// get internal index from external index 30 /// @param user external (user) index 31 /// @param internal pointer for storage of corresponding internal index 32 /// @return 0 on success, -1 on failure 33 int internal2user(const int internal, int *user) const; 34 35 private: 36 int findRoot(int index); 37 void recurseIndexSets(const int user_body_index); 38 bool m_map_built; 39 std::map<int, int> m_user_parent_index_map; 40 std::map<int, int> m_user_to_internal; 41 std::map<int, int> m_internal_to_user; 42 std::map<int, std::vector<int> > m_user_child_indices; 43 int m_current_index; 44 }; 45 } // namespace btInverseDynamics 46 47 #endif // USER2INTERNALINDEX_HPP 48