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! }