Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix portability of BulletInverseDynamics (usual issues: std::pow, M_PI, std::vector->idArray<T>::type, snprintf, pass vec3 and mat33 by const reference, not by value)

Browse Source
This commit is contained in:
erwincoumans
2015-11-17 21:51:02 -08:00
parent aa4d119f98
commit de763a26e7
11 changed files with 49 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Extras/InverseDynamics/CoilCreator.cpp
View File

@@ -12,7 +12,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) {
const idScalar theta_DH = 0;
const idScalar d_DH = 0.0;
const idScalar a_DH = 1.0 / m_num_bodies;
const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies;
const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies;
getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref,
&m_body_T_parent_ref);
// always z-axis
@@ -29,7 +29,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) {
m_body_I_body(0, 1) = 0.0;
m_body_I_body(0, 2) = 0.0;
m_body_I_body(1, 0) = 0.0;
m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies);
m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies);
m_body_I_body(1, 2) = 0.0;
m_body_I_body(2, 0) = 0.0;
m_body_I_body(2, 1) = 0.0;

22
Extras/InverseDynamics/DillCreator.cpp
View File

@@ -4,14 +4,16 @@ namespace btInverseDynamics {
DillCreator::DillCreator(int level)
: m_level(level),
m_num_bodies(std::pow(2, level)),
m_parent(m_num_bodies),
m_parent_r_parent_body_ref(m_num_bodies),
m_body_T_parent_ref(m_num_bodies),
m_body_axis_of_motion(m_num_bodies),
m_mass(m_num_bodies),
m_body_r_body_com(m_num_bodies),
m_body_I_body(m_num_bodies) {
m_num_bodies(BT_ID_POW(2, level))
{
m_parent.resize(m_num_bodies);
m_parent_r_parent_body_ref.resize(m_num_bodies);
m_body_T_parent_ref.resize(m_num_bodies);
m_body_axis_of_motion.resize(m_num_bodies);
m_mass.resize(m_num_bodies);
m_body_r_body_com.resize(m_num_bodies);
m_body_I_body.resize(m_num_bodies);
// generate names (for debugging)
for (int i = 0; i < m_num_bodies; i++) {
m_parent[i] = i - 1;
@@ -85,7 +87,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d
/// these parameters are from the paper ...
/// TODO: add proper citation
m_parent[body] = parent;
m_mass[body] = 0.1 * std::pow(size, 3);
m_mass[body] = 0.1 * BT_ID_POW(size, 3);
m_body_r_body_com[body](0) = 0.05 * size;
m_body_r_body_com[body](1) = 0;
m_body_r_body_com[body](2) = 0;
@@ -112,7 +114,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d
d_DH = 0.0;
}
const idScalar a_DH = i * 0.1;
const idScalar alpha_DH = i * M_PI / 3.0;
const idScalar alpha_DH = i * BT_ID_PI / 3.0;
m_current_body++;
recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
}

14
Extras/InverseDynamics/DillCreator.hpp
View File

@@ -34,13 +34,13 @@ private:
const idScalar a_DH_in, const idScalar alpha_DH_in);
int m_level;
int m_num_bodies;
std::vector<int> m_parent;
std::vector<vec3> m_parent_r_parent_body_ref;
std::vector<mat33> m_body_T_parent_ref;
std::vector<vec3> m_body_axis_of_motion;
std::vector<idScalar> m_mass;
std::vector<vec3> m_body_r_body_com;
std::vector<mat33> m_body_I_body;
idArray<int>::type m_parent;
idArray<vec3>::type m_parent_r_parent_body_ref;
idArray<mat33>::type m_body_T_parent_ref;
idArray<vec3>::type m_body_axis_of_motion;
idArray<idScalar>::type m_mass;
idArray<vec3>::type m_body_r_body_com;
idArray<mat33>::type m_body_I_body;
int m_current_body;
};
}

4
Extras/InverseDynamics/IDRandomUtil.cpp
View File

@@ -39,8 +39,8 @@ mat33 randomInertiaMatrix() {
// generate random valid inertia matrix by first getting valid components
// along major axes and then rotating by random amount
vec3 principal = randomInertiaPrincipal();
mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) *
transformZ(randomFloat(-M_PI, M_PI)));
mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) *
transformZ(randomFloat(-BT_ID_PI, BT_ID_PI)));
mat33 inertia;
inertia(0, 0) = principal(0);
inertia(0, 1) = 0;

2
Extras/InverseDynamics/btMultiBodyTreeCreator.hpp
View File

@@ -42,7 +42,7 @@ private:
vec3 body_r_body_com;
mat33 body_I_body;
};
std::vector<LinkData> m_data;
idArray<LinkData>::type m_data;
bool m_initialized;
};
}