1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
|
#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;
};
} // namespace btInverseDynamics
#endif // USER2INTERNALINDEX_HPP
|
