vec3 randomAxis() {
vec3 axis;
idScalar length;
do {
axis(0) = randomFloat(-1.0, 1.0);
axis(1) = randomFloat(-1.0, 1.0);
axis(2) = randomFloat(-1.0, 1.0);
length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
} while (length < 0.01);
return axis / length;
}
DillCreator::DillCreator(int level)
: m_level(level),
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;
// all z-axis (DH convention)
m_body_axis_of_motion[i](0) = 0.0;
m_body_axis_of_motion[i](1) = 0.0;
m_body_axis_of_motion[i](2) = 1.0;
}
// recursively build data structures
m_current_body = 0;
const int parent = -1;
const idScalar d_DH = 0.0;
const idScalar a_DH = 0.0;
const idScalar alpha_DH = 0.0;
if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) {
error_message("recurseDill failed\n");
abort();
}
}
int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in,
const idScalar a_DH_in, const idScalar alpha_DH_in) {
if (level < 0) {
error_message("invalid level parameter (%d)\n", level);
return -1;
}
if (m_current_body >= m_num_bodies || m_current_body < 0) {
error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body,
m_num_bodies);
return -1;
}
idScalar size = BT_ID_MAX(level, 1);
const int body = m_current_body;
// length = 0.1 * size;
// with = 2 * 0.01 * size;
/// these parameters are from the paper ...
/// TODO: add proper citation
m_parent[body] = parent;
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;
// initialization
for (int i = 0; i < 3; i++) {
m_parent_r_parent_body_ref[body](i) = 0;
for (int j = 0; j < 3; j++) {
m_body_I_body[body](i, j) = 0.0;
m_body_T_parent_ref[body](i, j) = 0.0;
}
}
const idScalar size_5 = pow(size, 5);
m_body_I_body[body](0, 0) = size_5 / 0.2e6;
m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6;
m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1);
getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body],
&m_body_T_parent_ref[body]);
// attach "level" Dill systems of levels 1...level
for (int i = 1; i <= level; i++) {
idScalar d_DH = 0.01 * size;
if (i == level) {
d_DH = 0.0;
}
const idScalar a_DH = i * 0.1;
const idScalar alpha_DH = i * BT_ID_PI / 3.0;
m_current_body++;
recurseDill(i - 1, body, d_DH, a_DH, alpha_DH);
}
return 0; // ok!
}
