1 #ifndef BODY_JOINT_INFO_UTILITY_H
2 #define BODY_JOINT_INFO_UTILITY_H
3
4 #include "Bullet3Common/b3Logging.h"
5
6 namespace Bullet
7 {
8 class btMultiBodyDoubleData;
9 class btMultiBodyFloatData;
10 }; // namespace Bullet
11
strDup(const char * const str)12 inline char* strDup(const char* const str)
13 {
14 #ifdef _WIN32
15 return _strdup(str);
16 #else
17 return strdup(str);
18 #endif
19 }
20
21 template <typename T, typename U>
addJointInfoFromMultiBodyData(const T * mb,U * bodyJoints,bool verboseOutput)22 void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
23 {
24 int numDofs = 0;
25 if (mb->m_baseMass>0)
26 {
27 numDofs = 6;
28 }
29
30 if (mb->m_baseName)
31 {
32 if (verboseOutput)
33 {
34 b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
35 }
36 }
37 int qOffset = 7;
38 int uOffset = 6;
39
40 for (int link = 0; link < mb->m_numLinks; link++)
41 {
42 {
43 b3JointInfo info;
44 info.m_jointName[0] = 0;
45 info.m_linkName[0] = 0;
46 info.m_flags = 0;
47 info.m_jointIndex = link;
48 info.m_qIndex =
49 (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
50 info.m_uIndex =
51 (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
52
53 if (mb->m_links[link].m_linkName)
54 {
55 if (verboseOutput)
56 {
57 b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
58 mb->m_links[link].m_linkName);
59 }
60 strcpy(info.m_linkName, mb->m_links[link].m_linkName);
61 }
62 if (mb->m_links[link].m_jointName)
63 {
64 if (verboseOutput)
65 {
66 b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
67 mb->m_links[link].m_jointName);
68 }
69 strcpy(info.m_jointName, mb->m_links[link].m_jointName);
70 //info.m_jointName = strDup(mb->m_links[link].m_jointName);
71 }
72
73 info.m_jointType = mb->m_links[link].m_jointType;
74 info.m_jointDamping = mb->m_links[link].m_jointDamping;
75 info.m_jointFriction = mb->m_links[link].m_jointFriction;
76 info.m_jointLowerLimit = mb->m_links[link].m_jointLowerLimit;
77 info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
78 info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
79 info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
80
81 info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[0];
82 info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[1];
83 info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[2];
84 info.m_parentFrame[3] = mb->m_links[link].m_zeroRotParentToThis.m_floats[0];
85 info.m_parentFrame[4] = mb->m_links[link].m_zeroRotParentToThis.m_floats[1];
86 info.m_parentFrame[5] = mb->m_links[link].m_zeroRotParentToThis.m_floats[2];
87 info.m_parentFrame[6] = mb->m_links[link].m_zeroRotParentToThis.m_floats[3];
88
89 info.m_jointAxis[0] = 0;
90 info.m_jointAxis[1] = 0;
91 info.m_jointAxis[2] = 0;
92 info.m_parentIndex = mb->m_links[link].m_parentIndex;
93
94 if (info.m_jointType == eRevoluteType)
95 {
96 info.m_jointAxis[0] = mb->m_links[link].m_jointAxisTop[0].m_floats[0];
97 info.m_jointAxis[1] = mb->m_links[link].m_jointAxisTop[0].m_floats[1];
98 info.m_jointAxis[2] = mb->m_links[link].m_jointAxisTop[0].m_floats[2];
99 }
100 if (info.m_jointType == ePrismaticType)
101 {
102 info.m_jointAxis[0] = mb->m_links[link].m_jointAxisBottom[0].m_floats[0];
103 info.m_jointAxis[1] = mb->m_links[link].m_jointAxisBottom[0].m_floats[1];
104 info.m_jointAxis[2] = mb->m_links[link].m_jointAxisBottom[0].m_floats[2];
105 }
106
107 if ((mb->m_links[link].m_jointType == eRevoluteType) ||
108 (mb->m_links[link].m_jointType == ePrismaticType))
109 {
110 info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
111 }
112 bodyJoints->m_jointInfo.push_back(info);
113 }
114 qOffset += mb->m_links[link].m_posVarCount;
115 uOffset += mb->m_links[link].m_dofCount;
116 numDofs += mb->m_links[link].m_dofCount;
117 }
118 bodyJoints->m_numDofs = numDofs;
119 }
120
121 #endif //BODY_JOINT_INFO_UTILITY_H
122