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