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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
|
#ifndef BODY_JOINT_INFO_UTILITY_H
#define BODY_JOINT_INFO_UTILITY_H
#include "Bullet3Common/b3Logging.h"
namespace Bullet
{
class btMultiBodyDoubleData;
class btMultiBodyFloatData;
};
inline char* strDup(const char* const str)
{
#ifdef _WIN32
return _strdup(str);
#else
return strdup(str);
#endif
}
template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
{
if (mb->m_baseName)
{
if (verboseOutput)
{
b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
}
}
int qOffset = 7;
int uOffset = 6;
for (int link = 0; link < mb->m_numLinks; link++)
{
{
b3JointInfo info;
info.m_flags = 0;
info.m_jointIndex = link;
info.m_qIndex =
(0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
info.m_uIndex =
(0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
if (mb->m_links[link].m_linkName) {
if (verboseOutput) {
b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
mb->m_links[link].m_linkName);
}
info.m_linkName = strDup(mb->m_links[link].m_linkName);
}
if (mb->m_links[link].m_jointName) {
if (verboseOutput) {
b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
mb->m_links[link].m_jointName);
}
info.m_jointName = strDup(mb->m_links[link].m_jointName);
}
info.m_jointType = mb->m_links[link].m_jointType;
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
bodyJoints->m_jointInfo.push_back(info);
}
qOffset += mb->m_links[link].m_posVarCount;
uOffset += mb->m_links[link].m_dofCount;
}
}
#endif //BODY_JOINT_INFO_UTILITY_H
|
